From 09b0de21f3bbd56768aa5a5b0f43930ff5f995ba Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 1 Jan 2014 13:17:24 +0100 Subject: [PATCH 001/381] Deleted backside config to cut down build times --- makefiles/config_px4fmu-v1_backside.mk | 161 ------------------------- 1 file changed, 161 deletions(-) delete mode 100644 makefiles/config_px4fmu-v1_backside.mk diff --git a/makefiles/config_px4fmu-v1_backside.mk b/makefiles/config_px4fmu-v1_backside.mk deleted file mode 100644 index 5ecf93a3a1..0000000000 --- a/makefiles/config_px4fmu-v1_backside.mk +++ /dev/null @@ -1,161 +0,0 @@ -# -# Makefile for the px4fmu_default configuration -# - -# -# Use the configuration's ROMFS. -# -ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common -ROMFS_OPTIONAL_FILES = $(PX4_BASE)/Images/px4io-v1_default.bin - -# -# Board support modules -# -MODULES += drivers/device -MODULES += drivers/stm32 -MODULES += drivers/stm32/adc -MODULES += drivers/stm32/tone_alarm -MODULES += drivers/led -MODULES += drivers/px4io -MODULES += drivers/px4fmu -MODULES += drivers/boards/px4fmu-v1 -MODULES += drivers/ardrone_interface -MODULES += drivers/l3gd20 -MODULES += drivers/bma180 -MODULES += drivers/mpu6000 -MODULES += drivers/hmc5883 -MODULES += drivers/ms5611 -MODULES += drivers/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 -MODULES += drivers/roboclaw -MODULES += drivers/airspeed -MODULES += drivers/ets_airspeed -MODULES += drivers/meas_airspeed -MODULES += modules/sensors - -# -# System commands -# -MODULES += systemcmds/eeprom -MODULES += systemcmds/ramtron -MODULES += systemcmds/bl_update -MODULES += systemcmds/boardinfo -MODULES += systemcmds/i2c -MODULES += systemcmds/mixer -MODULES += systemcmds/param -MODULES += systemcmds/perf -MODULES += systemcmds/preflight_check -MODULES += systemcmds/pwm -MODULES += systemcmds/esc_calib -MODULES += systemcmds/reboot -MODULES += systemcmds/top -MODULES += systemcmds/tests -MODULES += systemcmds/config -MODULES += systemcmds/nshterm - -# -# General system control -# -MODULES += modules/commander -MODULES += modules/navigator -MODULES += modules/mavlink -MODULES += modules/mavlink_onboard -MODULES += modules/gpio_led - -# -# Estimation modules (EKF/ SO3 / other filters) -# -#MODULES += modules/attitude_estimator_ekf -MODULES += modules/att_pos_estimator_ekf -#MODULES += modules/position_estimator_inav -MODULES += examples/flow_position_estimator -MODULES += modules/attitude_estimator_so3 - -# -# Vehicle Control -# -#MODULES += modules/segway # XXX Needs GCC 4.7 fix -#MODULES += modules/fw_pos_control_l1 -#MODULES += modules/fw_att_control -#MODULES += modules/multirotor_att_control -#MODULES += modules/multirotor_pos_control -#MODULES += examples/flow_position_control -#MODULES += examples/flow_speed_control -MODULES += modules/fixedwing_backside - -# -# Logging -# -MODULES += modules/sdlog2 - -# -# Unit tests -# -#MODULES += modules/unit_test -#MODULES += modules/commander/commander_tests - -# -# Library modules -# -MODULES += modules/systemlib -MODULES += modules/systemlib/mixer -MODULES += modules/controllib -MODULES += modules/uORB - -# -# Libraries -# -LIBRARIES += lib/mathlib/CMSIS -MODULES += lib/mathlib -MODULES += lib/mathlib/math/filter -MODULES += lib/ecl -MODULES += lib/external_lgpl -MODULES += lib/geo -MODULES += lib/conversion - -# -# Demo apps -# -#MODULES += examples/math_demo -# Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/hello_sky -#MODULES += examples/px4_simple_app - -# Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/daemon -#MODULES += examples/px4_daemon_app - -# Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/debug_values -#MODULES += examples/px4_mavlink_debug - -# Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control -#MODULES += examples/fixedwing_control - -# Hardware test -#MODULES += examples/hwtest - -# -# Transitional support - add commands from the NuttX export archive. -# -# In general, these should move to modules over time. -# -# Each entry here is ... but we use a helper macro -# to make the table a bit more readable. -# -define _B - $(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4) -endef - -# command priority stack entrypoint -BUILTIN_COMMANDS := \ - $(call _B, sercon, , 2048, sercon_main ) \ - $(call _B, serdis, , 2048, serdis_main ) \ - $(call _B, sysinfo, , 2048, sysinfo_main ) From 26d26a9b379c03214fed4b47fb8a4f6896177fb0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 1 Jan 2014 13:17:47 +0100 Subject: [PATCH 002/381] Removed fp suffix warning --- makefiles/toolchain_gnu-arm-eabi.mk | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index 9fd2dd516d..bb729e1032 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -138,8 +138,7 @@ ARCHCWARNINGS = $(ARCHWARNINGS) \ -Wold-style-declaration \ -Wmissing-parameter-type \ -Wmissing-prototypes \ - -Wnested-externs \ - -Wunsuffixed-float-constants + -Wnested-externs # C++-specific warnings # From ffe5c88d98b18390e3d44c8dd0751b6d062631be Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 1 Jan 2014 13:18:21 +0100 Subject: [PATCH 003/381] Added new estimator framework, actual estimation code not yet present --- makefiles/config_px4fmu-v1_default.mk | 2 +- makefiles/config_px4fmu-v2_default.mk | 2 +- .../fw_att_pos_estimator_main.cpp | 456 ++++++++++++++++++ .../fw_att_pos_estimator_params.c | 52 ++ src/modules/fw_att_pos_estimator/module.mk | 41 ++ 5 files changed, 551 insertions(+), 2 deletions(-) create mode 100644 src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp create mode 100644 src/modules/fw_att_pos_estimator/fw_att_pos_estimator_params.c create mode 100644 src/modules/fw_att_pos_estimator/module.mk diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 25c22f1fdf..5798e9c230 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -73,7 +73,7 @@ MODULES += modules/gpio_led # MODULES += modules/attitude_estimator_ekf MODULES += modules/attitude_estimator_so3 -MODULES += modules/att_pos_estimator_ekf +MODULES += modules/fw_att_pos_estimator MODULES += modules/position_estimator_inav MODULES += examples/flow_position_estimator diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 68673c0552..7614387a5f 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -73,7 +73,7 @@ MODULES += modules/mavlink_onboard # MODULES += modules/attitude_estimator_ekf MODULES += modules/attitude_estimator_so3 -MODULES += modules/att_pos_estimator_ekf +MODULES += modules/fw_att_pos_estimator MODULES += modules/position_estimator_inav MODULES += examples/flow_position_estimator diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp new file mode 100644 index 0000000000..459c5c2f7d --- /dev/null +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -0,0 +1,456 @@ +/**************************************************************************** + * + * 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 fw_att_pos_estimator_main.cpp + * Implementation of the attitude and position estimator. + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * estimator app start / stop handling function + * + * @ingroup apps + */ +extern "C" __EXPORT int fw_att_pos_estimator_main(int argc, char *argv[]); + +class FixedwingEstimator +{ +public: + /** + * Constructor + */ + FixedwingEstimator(); + + /** + * Destructor, also kills the sensors task. + */ + ~FixedwingEstimator(); + + /** + * Start the sensors task. + * + * @return OK on success. + */ + int start(); + +private: + + bool _task_should_exit; /**< if true, sensor task should exit */ + int _estimator_task; /**< task handle for sensor task */ + + int _global_pos_sub; + int _gyro_sub; /**< gyro sensor subscription */ + int _accel_sub; /**< accel sensor subscription */ + int _mag_sub; /**< mag sensor subscription */ + int _attitude_sub; /**< raw rc channels data subscription */ + int _airspeed_sub; /**< airspeed subscription */ + int _vstatus_sub; /**< vehicle status subscription */ + int _params_sub; /**< notification of parameter updates */ + int _manual_control_sub; /**< notification of manual control updates */ + int _mission_sub; + + orb_advert_t _att_pub; /**< position setpoint */ + + struct vehicle_attitude_s _att; /**< vehicle attitude */ + struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ + struct manual_control_setpoint_s _manual; /**< r/c channel data */ + struct gyro_report _gyro; + struct accel_report _accel; + struct mag_report _mag; + struct airspeed_s _airspeed; /**< airspeed */ + struct vehicle_status_s _vstatus; /**< vehicle status */ + struct vehicle_global_position_s _global_pos; /**< global vehicle position */ + + perf_counter_t _loop_perf; /**< loop performance counter */ + + unsigned _mission_items_maxcount; /**< maximum number of mission items supported */ + struct mission_item_s * _mission_items; /**< storage for mission items */ + bool _mission_valid; /**< flag if mission is valid */ + + /** manual control states */ + float _seatbelt_hold_heading; /**< heading the system should hold in seatbelt mode */ + float _loiter_hold_lat; + float _loiter_hold_lon; + float _loiter_hold_alt; + bool _loiter_hold; + + struct { + float throttle_cruise; + } _parameters; /**< local copies of interesting parameters */ + + struct { + param_t throttle_cruise; + + } _parameter_handles; /**< handles for interesting parameters */ + + + /** + * Update our local parameter cache. + */ + int parameters_update(); + + /** + * Update control outputs + * + */ + void control_update(); + + /** + * Check for changes in vehicle status. + */ + void vehicle_status_poll(); + + /** + * Shim for calling task_main from task_create. + */ + static void task_main_trampoline(int argc, char *argv[]); + + /** + * Main sensor collection task. + */ + void task_main() __attribute__((noreturn)); +}; + +namespace estimator +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +FixedwingEstimator *g_estimator; +} + +FixedwingEstimator::FixedwingEstimator() : + + _task_should_exit(false), + _estimator_task(-1), + +/* subscriptions */ + _global_pos_sub(-1), + _gyro_sub(-1), + _accel_sub(-1), + _mag_sub(-1), + _airspeed_sub(-1), + _vstatus_sub(-1), + _params_sub(-1), + _manual_control_sub(-1), + +/* publications */ + _att_pub(-1), + +/* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "fw_att_pos_estimator")), +/* states */ + _mission_items_maxcount(20), + _mission_valid(false), + _loiter_hold(false) +{ + + _parameter_handles.throttle_cruise = param_find("NAV_DUMMY"); + + /* fetch initial parameter values */ + parameters_update(); +} + +FixedwingEstimator::~FixedwingEstimator() +{ + if (_estimator_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(_estimator_task); + break; + } + } while (_estimator_task != -1); + } + + estimator::g_estimator = nullptr; +} + +int +FixedwingEstimator::parameters_update() +{ + + //param_get(_parameter_handles.throttle_cruise, &(_parameters.throttle_cruise)); + + return OK; +} + +void +FixedwingEstimator::vehicle_status_poll() +{ + bool vstatus_updated; + + /* Check HIL state if vehicle status has changed */ + orb_check(_vstatus_sub, &vstatus_updated); + + if (vstatus_updated) { + + orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus); + } +} + +void +FixedwingEstimator::task_main_trampoline(int argc, char *argv[]) +{ + estimator::g_estimator->task_main(); +} + +void +FixedwingEstimator::task_main() +{ + + /* inform about start */ + warnx("Initializing.."); + fflush(stdout); + + /* + * do subscriptions + */ + _gyro_sub = orb_subscribe(ORB_ID(sensor_gyro)); + _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); + _mag_sub = orb_subscribe(ORB_ID(sensor_mag)); + _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); + _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); + _params_sub = orb_subscribe(ORB_ID(parameter_update)); + _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + + /* 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(); + + /* wakeup source(s) */ + struct pollfd fds[2]; + + /* Setup of loop */ + fds[0].fd = _params_sub; + fds[0].events = POLLIN; + fds[1].fd = _gyro_sub; + fds[1].events = POLLIN; + + while (!_task_should_exit) { + + /* wait for up to 500ms for data */ + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + + /* timed out - periodic check for _task_should_exit, etc. */ + if (pret == 0) + continue; + + /* this is undesirable but not much we can do - might want to flag unhappy status */ + if (pret < 0) { + warn("poll error %d, %d", pret, errno); + continue; + } + + perf_begin(_loop_perf); + + /* 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 */ + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), _params_sub, &update); + + /* update parameters from storage */ + parameters_update(); + } + + /* only run estimator if gyro updated */ + if (fds[1].revents & POLLIN) { + + static uint64_t last_run = 0; + float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; + last_run = hrt_absolute_time(); + + /* guard against too large deltaT's */ + if (deltaT > 1.0f) + deltaT = 0.01f; + + /* load local copies */ + orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro); + + bool updated; + + orb_check(_accel_sub, &updated); + if (updated) + orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); + + orb_check(_mag_sub, &updated); + if (updated) + orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag); + + orb_check(_airspeed_sub, &updated); + if (updated) + orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); + + /* lazily publish the attitude only once available */ + if (_att_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att); + + } else { + /* advertise and publish */ + _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att); + } + + } + + perf_end(_loop_perf); + } + + warnx("exiting.\n"); + + _estimator_task = -1; + _exit(0); +} + +int +FixedwingEstimator::start() +{ + ASSERT(_estimator_task == -1); + + /* start the task */ + _estimator_task = task_spawn_cmd("fw_att_pos_estimator", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2048, + (main_t)&FixedwingEstimator::task_main_trampoline, + nullptr); + + if (_estimator_task < 0) { + warn("task start failed"); + return -errno; + } + + return OK; +} + +int fw_att_pos_estimator_main(int argc, char *argv[]) +{ + if (argc < 1) + errx(1, "usage: fw_att_pos_estimator {start|stop|status}"); + + if (!strcmp(argv[1], "start")) { + + if (estimator::g_estimator != nullptr) + errx(1, "already running"); + + estimator::g_estimator = new FixedwingEstimator; + + if (estimator::g_estimator == nullptr) + errx(1, "alloc failed"); + + if (OK != estimator::g_estimator->start()) { + delete estimator::g_estimator; + estimator::g_estimator = nullptr; + err(1, "start failed"); + } + + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + if (estimator::g_estimator == nullptr) + errx(1, "not running"); + + delete estimator::g_estimator; + estimator::g_estimator = nullptr; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (estimator::g_estimator) { + errx(0, "running"); + + } else { + errx(1, "not running"); + } + } + + warnx("unrecognized command"); + return 1; +} diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_params.c b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_params.c new file mode 100644 index 0000000000..64e593a15f --- /dev/null +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_params.c @@ -0,0 +1,52 @@ +/**************************************************************************** + * + * 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 fw_att_pos_estimator_params.c + * + * Parameters defined by the attitude and position estimator task + * + * @author Lorenz Meier + */ + +#include + +#include + +/* + * Estimator parameters, accessible via MAVLink + * + */ + +PARAM_DEFINE_FLOAT(NAV_DUMMY, 0.0f); + diff --git a/src/modules/fw_att_pos_estimator/module.mk b/src/modules/fw_att_pos_estimator/module.mk new file mode 100644 index 0000000000..62bf60b60a --- /dev/null +++ b/src/modules/fw_att_pos_estimator/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Main Attitude and Position Estimator for Fixed Wing Aircraft +# + +MODULE_COMMAND = navigator + +SRCS = fw_att_pos_estimator_main.cpp \ + fw_att_pos_estimator_params.c From 5008f79a96db66f88153c264b81db2a720b560c6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 1 Jan 2014 16:48:15 +0100 Subject: [PATCH 004/381] Interfaced Pauls estimator, compiles, untested --- .../fw_att_pos_estimator_main.cpp | 187 +++++++++++++++--- src/modules/fw_att_pos_estimator/module.mk | 5 +- 2 files changed, 158 insertions(+), 34 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 459c5c2f7d..0f9c8ca821 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -55,6 +55,7 @@ #include #include #include +#include #include #include #include @@ -71,6 +72,8 @@ #include #include +#include "../../../InertialNav/code/estimator.h" + /** * estimator app start / stop handling function * @@ -78,6 +81,14 @@ */ extern "C" __EXPORT int fw_att_pos_estimator_main(int argc, char *argv[]); +__EXPORT uint32_t millis(); + +static uint64_t last_run = 0; + +uint32_t millis() { + return last_run / 1e3; +} + class FixedwingEstimator { public: @@ -103,18 +114,19 @@ private: bool _task_should_exit; /**< if true, sensor task should exit */ int _estimator_task; /**< task handle for sensor task */ - int _global_pos_sub; int _gyro_sub; /**< gyro sensor subscription */ int _accel_sub; /**< accel sensor subscription */ int _mag_sub; /**< mag sensor subscription */ int _attitude_sub; /**< raw rc channels data subscription */ int _airspeed_sub; /**< airspeed subscription */ + int _gps_sub; /**< GPS subscription */ int _vstatus_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ int _manual_control_sub; /**< notification of manual control updates */ int _mission_sub; - orb_advert_t _att_pub; /**< position setpoint */ + orb_advert_t _att_pub; /**< vehicle attitude */ + orb_advert_t _global_pos_pub; /**< global position */ struct vehicle_attitude_s _att; /**< vehicle attitude */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ @@ -125,22 +137,19 @@ private: struct airspeed_s _airspeed; /**< airspeed */ struct vehicle_status_s _vstatus; /**< vehicle status */ struct vehicle_global_position_s _global_pos; /**< global vehicle position */ + struct vehicle_gps_position_s _gps; /**< GPS position */ perf_counter_t _loop_perf; /**< loop performance counter */ - unsigned _mission_items_maxcount; /**< maximum number of mission items supported */ - struct mission_item_s * _mission_items; /**< storage for mission items */ - bool _mission_valid; /**< flag if mission is valid */ - - /** manual control states */ - float _seatbelt_hold_heading; /**< heading the system should hold in seatbelt mode */ - float _loiter_hold_lat; - float _loiter_hold_lon; - float _loiter_hold_alt; - bool _loiter_hold; + bool _initialized; struct { - float throttle_cruise; + float throttle_cruise; + uint32_t vel_delay_ms; + uint32_t pos_delay_ms; + uint32_t height_delay_ms; + uint32_t mag_delay_ms; + uint32_t tas_delay_ms; } _parameters; /**< local copies of interesting parameters */ struct { @@ -194,24 +203,23 @@ FixedwingEstimator::FixedwingEstimator() : _estimator_task(-1), /* subscriptions */ - _global_pos_sub(-1), _gyro_sub(-1), _accel_sub(-1), _mag_sub(-1), _airspeed_sub(-1), + _gps_sub(-1), _vstatus_sub(-1), _params_sub(-1), _manual_control_sub(-1), /* publications */ _att_pub(-1), + _global_pos_pub(-1), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw_att_pos_estimator")), /* states */ - _mission_items_maxcount(20), - _mission_valid(false), - _loiter_hold(false) + _initialized(false) { _parameter_handles.throttle_cruise = param_find("NAV_DUMMY"); @@ -249,8 +257,15 @@ int FixedwingEstimator::parameters_update() { + // XXX NEED TO GET HANDLES FIRST! NEEDS PARAM INIT //param_get(_parameter_handles.throttle_cruise, &(_parameters.throttle_cruise)); + _parameters.vel_delay_ms = 230; + _parameters.pos_delay_ms = 210; + _parameters.height_delay_ms = 350; + _parameters.mag_delay_ms = 30; + _parameters.tas_delay_ms = 210; + return OK; } @@ -295,8 +310,10 @@ FixedwingEstimator::task_main() /* 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); + /* rate limit gyro updates to 50 Hz */ + + /* XXX remove this!, BUT increase the data buffer size! */ + orb_set_interval(_gyro_sub, 17); parameters_update(); @@ -342,31 +359,127 @@ FixedwingEstimator::task_main() /* only run estimator if gyro updated */ if (fds[1].revents & POLLIN) { - static uint64_t last_run = 0; - float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; - last_run = hrt_absolute_time(); + + + /* load local copies */ + orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro); + orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); + + float IMUmsec = _gyro.timestamp / 1e6; + + float deltaT = (_gyro.timestamp - last_run) / 1000000.0f; + last_run = _gyro.timestamp / 1e6 ; /* guard against too large deltaT's */ if (deltaT > 1.0f) deltaT = 0.01f; - /* load local copies */ - orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro); + dt = deltaT; - bool updated; + if (_initialized) { - orb_check(_accel_sub, &updated); - if (updated) - orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); + /* predict states and covariances */ - orb_check(_mag_sub, &updated); - if (updated) + /* run the strapdown INS every sensor update */ + UpdateStrapdownEquationsNED(); + + /* store the predictions */ + StoreStates(); + + /* evaluate if on ground */ + OnGroundCheck(); + + /* prepare the delta angles and time used by the covariance prediction */ + summedDelAng = summedDelAng + correctedDelAng; + summedDelVel = summedDelVel + correctedDelVel; + dt += dtIMU; + + /* predict the covairance if the total delta angle has exceeded the threshold + * or the time limit will be exceeded on the next measurement update + */ + if ((dt >= (covTimeStepMax - dtIMU)) || (summedDelAng.length() > covDelAngMax)) { + CovariancePrediction(); + summedDelAng = summedDelAng.zero(); + summedDelVel = summedDelVel.zero(); + dt = 0.0f; + } + } + + bool gps_updated; + orb_check(_gps_sub, &gps_updated); + if (gps_updated) { + orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps); + + if (_gps.fix_type > 2 && !_initialized) { + + /* initialize */ + InitialiseFilter(); + + } else if (_gps.fix_type > 2) { + /* fuse GPS updates */ + + /* convert GPS measurements to horizontal NE, altitude and 3D velocity NED */ + calcvelNED(velNED, gpsCourse, gpsGndSpd, gpsVelD); + calcposNED(posNED, gpsLat, gpsLon, gpsHgt, latRef, lonRef, hgtRef); + + posNE[0] = posNED[0]; + posNE[1] = posNED[1]; + hgtMea = -posNED[2]; + + /* set flags for further processing */ + fuseVelData = true; + fusePosData = true; + fuseHgtData = true; + + /* recall states after adjusting for delays */ + RecallStates(statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); + RecallStates(statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); + RecallStates(statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms)); + + /* run the actual fusion */ + FuseVelposNED(); + + } else { + + /* do not fuse anything, we got no position / vel update */ + fuseVelData = false; + fusePosData = false; + fuseHgtData = false; + } + } + + bool mag_updated; + + orb_check(_mag_sub, &mag_updated); + if (mag_updated) { orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag); - orb_check(_airspeed_sub, &updated); - if (updated) + if (_initialized) { + fuseMagData = true; + RecallStates(statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); + FuseMagnetometer(); + } + + } else { + fuseMagData = false; + } + + bool airspeed_updated; + orb_check(_airspeed_sub, &airspeed_updated); + if (airspeed_updated) { orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); + if (_initialized && _airspeed.true_airspeed_m_s > 8.0f /* XXX magic number */) { + + fuseVtasData = true; + RecallStates(statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data + FuseAirspeed(); + } + + } else { + fuseVtasData = false; + } + /* lazily publish the attitude only once available */ if (_att_pub > 0) { /* publish the attitude setpoint */ @@ -377,6 +490,16 @@ FixedwingEstimator::task_main() _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att); } + /* lazily publish the position only once available */ + if (_global_pos_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos); + + } else { + /* advertise and publish */ + _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos); + } + } perf_end(_loop_perf); diff --git a/src/modules/fw_att_pos_estimator/module.mk b/src/modules/fw_att_pos_estimator/module.mk index 62bf60b60a..c1384be7e9 100644 --- a/src/modules/fw_att_pos_estimator/module.mk +++ b/src/modules/fw_att_pos_estimator/module.mk @@ -35,7 +35,8 @@ # Main Attitude and Position Estimator for Fixed Wing Aircraft # -MODULE_COMMAND = navigator +MODULE_COMMAND = fw_att_pos_estimator SRCS = fw_att_pos_estimator_main.cpp \ - fw_att_pos_estimator_params.c + fw_att_pos_estimator_params.c \ + ../../../../InertialNav/code/estimator.cpp From 75e6cf45e9fe46d65d6fdf1e48b75b2b329f0423 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 1 Jan 2014 17:04:24 +0100 Subject: [PATCH 005/381] Use a really large amount of stack to avoid running out of it --- src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 0f9c8ca821..90d02843d1 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -520,7 +520,7 @@ FixedwingEstimator::start() _estimator_task = task_spawn_cmd("fw_att_pos_estimator", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 2048, + 12048, (main_t)&FixedwingEstimator::task_main_trampoline, nullptr); From 3d68c37d3cb6eb0d177df5e90e6365cf27a5b483 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 2 Jan 2014 16:25:53 +0100 Subject: [PATCH 006/381] Now actually loading data into the filter when running it --- .../fw_att_pos_estimator_main.cpp | 108 +++++++++++++++--- 1 file changed, 94 insertions(+), 14 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 90d02843d1..b359348ed8 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -85,7 +85,8 @@ __EXPORT uint32_t millis(); static uint64_t last_run = 0; -uint32_t millis() { +uint32_t millis() +{ return last_run / 1e3; } @@ -139,6 +140,10 @@ private: struct vehicle_global_position_s _global_pos; /**< global vehicle position */ struct vehicle_gps_position_s _gps; /**< GPS position */ + struct gyro_scale _gyro_offsets; + struct accel_scale _accel_offsets; + struct mag_scale _mag_offsets; + perf_counter_t _loop_perf; /**< loop performance counter */ bool _initialized; @@ -226,6 +231,31 @@ FixedwingEstimator::FixedwingEstimator() : /* fetch initial parameter values */ parameters_update(); + + /* get offsets */ + + int fd, res; + + fd = open(GYRO_DEVICE_PATH, O_RDONLY); + + if (fd > 0) { + res = ioctl(fd, GYROIOCGSCALE, (long unsigned int)&_gyro_offsets); + close(fd); + } + + fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + + if (fd > 0) { + res = ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&_accel_offsets); + close(fd); + } + + fd = open(MAG_DEVICE_PATH, O_RDONLY); + + if (fd > 0) { + res = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&_mag_offsets); + close(fd); + } } FixedwingEstimator::~FixedwingEstimator() @@ -317,6 +347,9 @@ FixedwingEstimator::task_main() parameters_update(); + Vector3f lastAngRate; + Vector3f lastAccel; + /* wakeup source(s) */ struct pollfd fds[2]; @@ -359,13 +392,13 @@ FixedwingEstimator::task_main() /* only run estimator if gyro updated */ if (fds[1].revents & POLLIN) { - + /* load local copies */ orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro); orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); - float IMUmsec = _gyro.timestamp / 1e6; + float IMUmsec = _gyro.timestamp / 1e3; float deltaT = (_gyro.timestamp - last_run) / 1000000.0f; last_run = _gyro.timestamp / 1e6 ; @@ -378,6 +411,22 @@ FixedwingEstimator::task_main() if (_initialized) { + /* fill in last data set */ + dtIMU = dt; + + angRate.x = _gyro.x; + angRate.y = _gyro.y; + angRate.z = _gyro.z; + + accel.x = _accel.x; + accel.y = _accel.y; + accel.z = _accel.z; + + dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU; + lastAngRate = angRate; + dVelIMU = 0.5f * (accel + lastAccel) * dtIMU; + lastAccel = accel; + /* predict states and covariances */ /* run the strapdown INS every sensor update */ @@ -407,17 +456,29 @@ FixedwingEstimator::task_main() bool gps_updated; orb_check(_gps_sub, &gps_updated); + if (gps_updated) { orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps); - if (_gps.fix_type > 2 && !_initialized) { - - /* initialize */ - InitialiseFilter(); - - } else if (_gps.fix_type > 2) { + if (_gps.fix_type > 2) { /* fuse GPS updates */ + //_gps.timestamp / 1e3; + GPSstatus = _gps.fix_type; + gpsCourse = _gps.cog_rad; + gpsGndSpd = sqrtf(_gps.vel_n_m_s * _gps.vel_n_m_s + _gps.vel_e_m_s * _gps.vel_e_m_s); + gpsVelD = _gps.vel_d_m_s; + gpsLat = math::radians(_gps.lat / (double)1e7); + gpsLon = math::radians(_gps.lon / (double)1e7); + gpsHgt = _gps.alt / 1e3f; + + newDataGps = true; + + if (!_initialized) { + InitialiseFilter(); + continue; + } + /* convert GPS measurements to horizontal NE, altitude and 3D velocity NED */ calcvelNED(velNED, gpsCourse, gpsGndSpd, gpsVelD); calcposNED(posNED, gpsLat, gpsLon, gpsHgt, latRef, lonRef, hgtRef); @@ -445,16 +506,32 @@ FixedwingEstimator::task_main() fuseVelData = false; fusePosData = false; fuseHgtData = false; + + newDataGps = true; } + + } else { + newDataGps = false; } bool mag_updated; orb_check(_mag_sub, &mag_updated); + if (mag_updated) { orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag); if (_initialized) { + + magData.x = _mag.x; + magBias.x = -_mag_offsets.x_offset; + + magData.y = _mag.y; + magBias.y = -_mag_offsets.y_offset; + + magData.z = _mag.z; + magBias.z = -_mag_offsets.z_offset; + fuseMagData = true; RecallStates(statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); FuseMagnetometer(); @@ -466,11 +543,14 @@ FixedwingEstimator::task_main() bool airspeed_updated; orb_check(_airspeed_sub, &airspeed_updated); + if (airspeed_updated) { orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); if (_initialized && _airspeed.true_airspeed_m_s > 8.0f /* XXX magic number */) { + VtasMeas = _airspeed.true_airspeed_m_s; + fuseVtasData = true; RecallStates(statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data FuseAirspeed(); @@ -518,11 +598,11 @@ FixedwingEstimator::start() /* start the task */ _estimator_task = task_spawn_cmd("fw_att_pos_estimator", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 12048, - (main_t)&FixedwingEstimator::task_main_trampoline, - nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 12048, + (main_t)&FixedwingEstimator::task_main_trampoline, + nullptr); if (_estimator_task < 0) { warn("task start failed"); From f0740df53ae0f1c79f2f1298611a7a8c7e0ae8e7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 2 Jan 2014 16:59:11 +0100 Subject: [PATCH 007/381] Set new filter as default in HIL and fixed wing, fixes in estimator, WIP --- .../init.d/1000_rc_fw_easystar.hil | 2 +- .../init.d/1004_rc_fw_Rascal110.hil | 2 +- ROMFS/px4fmu_common/init.d/rc.fixedwing | 2 +- .../fw_att_pos_estimator_main.cpp | 76 +++++++++++-------- 4 files changed, 47 insertions(+), 35 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 40a13b5d17..181256dc71 100644 --- a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil +++ b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil @@ -74,7 +74,7 @@ sh /etc/init.d/rc.sensors # # Start the attitude estimator (depends on orb) # -att_pos_estimator_ekf start +fw_att_pos_estimator start # # Load mixer and start controllers (depends on px4io) 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 344d784224..858b6572ef 100644 --- a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil +++ b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil @@ -74,7 +74,7 @@ sh /etc/init.d/rc.sensors # # Start the attitude estimator (depends on orb) # -att_pos_estimator_ekf start +fw_att_pos_estimator start # # Load mixer and start controllers (depends on px4io) diff --git a/ROMFS/px4fmu_common/init.d/rc.fixedwing b/ROMFS/px4fmu_common/init.d/rc.fixedwing index f028510064..c3f24cfc4e 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fixedwing +++ b/ROMFS/px4fmu_common/init.d/rc.fixedwing @@ -21,7 +21,7 @@ gps start # # Start the attitude and position estimator # -att_pos_estimator_ekf start +fw_att_pos_estimator start # # Start attitude controller diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index b359348ed8..adf679d7e0 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -334,6 +334,7 @@ FixedwingEstimator::task_main() _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); _mag_sub = orb_subscribe(ORB_ID(sensor_mag)); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); + _gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); @@ -349,6 +350,12 @@ FixedwingEstimator::task_main() Vector3f lastAngRate; Vector3f lastAccel; + /* set initial filter state */ + fuseVelData = false; + fusePosData = false; + fuseHgtData = false; + fuseMagData = false; + fuseVtasData = false; /* wakeup source(s) */ struct pollfd fds[2]; @@ -359,6 +366,8 @@ FixedwingEstimator::task_main() fds[1].fd = _gyro_sub; fds[1].events = POLLIN; + hrt_abstime start_time = hrt_absolute_time(); + while (!_task_should_exit) { /* wait for up to 500ms for data */ @@ -376,9 +385,6 @@ FixedwingEstimator::task_main() perf_begin(_loop_perf); - /* check vehicle status for changes to publication state */ - vehicle_status_poll(); - /* only update parameters if they changed */ if (fds[0].revents & POLLIN) { /* read from param to clear updated flag */ @@ -392,7 +398,8 @@ FixedwingEstimator::task_main() /* only run estimator if gyro updated */ if (fds[1].revents & POLLIN) { - + /* check vehicle status for changes to publication state */ + vehicle_status_poll(); /* load local copies */ orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro); @@ -407,12 +414,10 @@ FixedwingEstimator::task_main() if (deltaT > 1.0f) deltaT = 0.01f; - dt = deltaT; - if (_initialized) { /* fill in last data set */ - dtIMU = dt; + dtIMU = deltaT; angRate.x = _gyro.x; angRate.y = _gyro.y; @@ -456,7 +461,6 @@ FixedwingEstimator::task_main() bool gps_updated; orb_check(_gps_sub, &gps_updated); - if (gps_updated) { orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps); @@ -474,8 +478,10 @@ FixedwingEstimator::task_main() newDataGps = true; - if (!_initialized) { + if (hrt_elapsed_time(&start_time) > 500000 && !_initialized) { InitialiseFilter(); + _initialized = true; + warnx("init done."); continue; } @@ -487,7 +493,7 @@ FixedwingEstimator::task_main() posNE[1] = posNED[1]; hgtMea = -posNED[2]; - /* set flags for further processing */ + // set flags for further processing fuseVelData = true; fusePosData = true; fuseHgtData = true; @@ -507,7 +513,7 @@ FixedwingEstimator::task_main() fusePosData = false; fuseHgtData = false; - newDataGps = true; + newDataGps = false; } } else { @@ -517,8 +523,7 @@ FixedwingEstimator::task_main() bool mag_updated; orb_check(_mag_sub, &mag_updated); - - if (mag_updated) { + if (mag_updated && _initialized) { orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag); if (_initialized) { @@ -543,41 +548,48 @@ FixedwingEstimator::task_main() bool airspeed_updated; orb_check(_airspeed_sub, &airspeed_updated); - - if (airspeed_updated) { + if (airspeed_updated && _initialized) { orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); - if (_initialized && _airspeed.true_airspeed_m_s > 8.0f /* XXX magic number */) { + if (_airspeed.true_airspeed_m_s > 8.0f /* XXX magic number */) { VtasMeas = _airspeed.true_airspeed_m_s; fuseVtasData = true; RecallStates(statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data FuseAirspeed(); + } else { + fuseVtasData = false; } } else { fuseVtasData = false; } - /* lazily publish the attitude only once available */ - if (_att_pub > 0) { - /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att); + if (_initialized) { - } else { - /* advertise and publish */ - _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att); - } + math::EulerAngles euler(eulerEst[0], eulerEst[1], eulerEst[2]); + //warnx("est: %8.4f, %8.4f, %8.4f", eulerEst[0], eulerEst[1], eulerEst[2]); - /* lazily publish the position only once available */ - if (_global_pos_pub > 0) { - /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos); + // /* lazily publish the attitude only once available */ + // if (_att_pub > 0) { + // /* publish the attitude setpoint */ + // orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att); - } else { - /* advertise and publish */ - _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos); + // } else { + // /* advertise and publish */ + // _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att); + // } + + // /* lazily publish the position only once available */ + // if (_global_pos_pub > 0) { + // /* publish the attitude setpoint */ + // orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos); + + // } else { + // /* advertise and publish */ + // _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos); + // } } } @@ -599,7 +611,7 @@ FixedwingEstimator::start() /* start the task */ _estimator_task = task_spawn_cmd("fw_att_pos_estimator", SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, + SCHED_PRIORITY_MAX - 40, 12048, (main_t)&FixedwingEstimator::task_main_trampoline, nullptr); From 17ac30f1b195b5095100abb5d595c781060f218d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 2 Jan 2014 17:08:24 +0100 Subject: [PATCH 008/381] More fixes, but things are pretty NaN still --- .../fw_att_pos_estimator_main.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index adf679d7e0..b238422b63 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -407,8 +407,8 @@ FixedwingEstimator::task_main() float IMUmsec = _gyro.timestamp / 1e3; - float deltaT = (_gyro.timestamp - last_run) / 1000000.0f; - last_run = _gyro.timestamp / 1e6 ; + float deltaT = (_gyro.timestamp - last_run) / 1e6f; + last_run = _gyro.timestamp; /* guard against too large deltaT's */ if (deltaT > 1.0f) @@ -659,7 +659,12 @@ int fw_att_pos_estimator_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (estimator::g_estimator) { - errx(0, "running"); + warnx("running"); + warnx("attitude: %8.4f, %8.4f, %8.4f", eulerEst[0], eulerEst[1], eulerEst[2]); + warnx("states [1-3]: %8.4f, %8.4f, %8.4f", states[0], states[1], states[2]); + warnx("states [4-6]: %8.4f, %8.4f, %8.4f", states[3], states[4], states[5]); + warnx("states [7-9]: %8.4f, %8.4f, %8.4f", states[6], states[7], states[8]); + exit(0); } else { errx(1, "not running"); From cbac002fb3997c2d3a4cf47ab0fe81c8020baaff Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 2 Jan 2014 19:18:19 +0100 Subject: [PATCH 009/381] Use the already compensated mag vector and initialize offsets to zero --- .../fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index b238422b63..49622dda38 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -528,14 +528,15 @@ FixedwingEstimator::task_main() if (_initialized) { + // XXX we compensate the offsets upfront - should be close to zero. magData.x = _mag.x; - magBias.x = -_mag_offsets.x_offset; + magBias.x = 0.0f; // _mag_offsets.x_offset magData.y = _mag.y; - magBias.y = -_mag_offsets.y_offset; + magBias.y = 0.0f; // _mag_offsets.y_offset magData.z = _mag.z; - magBias.z = -_mag_offsets.z_offset; + magBias.z = 0.0f; // _mag_offsets.y_offset fuseMagData = true; RecallStates(statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); From 2bf4e7912436e3fde31567a9419a5d861d58b6c5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 2 Jan 2014 20:08:39 +0100 Subject: [PATCH 010/381] Fix up init --- .../fw_att_pos_estimator_main.cpp | 122 ++++++++++-------- 1 file changed, 69 insertions(+), 53 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 49622dda38..1ed526e83f 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -414,24 +414,26 @@ FixedwingEstimator::task_main() if (deltaT > 1.0f) deltaT = 0.01f; + + // Always store data, independent of init status + /* fill in last data set */ + dtIMU = deltaT; + + angRate.x = _gyro.x; + angRate.y = _gyro.y; + angRate.z = _gyro.z; + + accel.x = _accel.x; + accel.y = _accel.y; + accel.z = _accel.z; + + dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU; + lastAngRate = angRate; + dVelIMU = 0.5f * (accel + lastAccel) * dtIMU; + lastAccel = accel; + if (_initialized) { - /* fill in last data set */ - dtIMU = deltaT; - - angRate.x = _gyro.x; - angRate.y = _gyro.y; - angRate.z = _gyro.z; - - accel.x = _accel.x; - accel.y = _accel.y; - accel.z = _accel.z; - - dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU; - lastAngRate = angRate; - dVelIMU = 0.5f * (accel + lastAccel) * dtIMU; - lastAccel = accel; - /* predict states and covariances */ /* run the strapdown INS every sensor update */ @@ -485,26 +487,29 @@ FixedwingEstimator::task_main() continue; } - /* convert GPS measurements to horizontal NE, altitude and 3D velocity NED */ - calcvelNED(velNED, gpsCourse, gpsGndSpd, gpsVelD); - calcposNED(posNED, gpsLat, gpsLon, gpsHgt, latRef, lonRef, hgtRef); + if (_initialized) { - posNE[0] = posNED[0]; - posNE[1] = posNED[1]; - hgtMea = -posNED[2]; + /* convert GPS measurements to horizontal NE, altitude and 3D velocity NED */ + calcvelNED(velNED, gpsCourse, gpsGndSpd, gpsVelD); + calcposNED(posNED, gpsLat, gpsLon, gpsHgt, latRef, lonRef, hgtRef); - // set flags for further processing - fuseVelData = true; - fusePosData = true; - fuseHgtData = true; + posNE[0] = posNED[0]; + posNE[1] = posNED[1]; + hgtMea = -posNED[2]; - /* recall states after adjusting for delays */ - RecallStates(statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); - RecallStates(statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); - RecallStates(statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms)); + // set flags for further processing + fuseVelData = true; + fusePosData = true; + fuseHgtData = true; - /* run the actual fusion */ - FuseVelposNED(); + /* recall states after adjusting for delays */ + RecallStates(statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); + RecallStates(statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); + RecallStates(statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms)); + + /* run the actual fusion */ + FuseVelposNED(); + } } else { @@ -521,23 +526,22 @@ FixedwingEstimator::task_main() } bool mag_updated; - orb_check(_mag_sub, &mag_updated); - if (mag_updated && _initialized) { + if (mag_updated) { orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag); + // XXX we compensate the offsets upfront - should be close to zero. + magData.x = _mag.x; + magBias.x = 0.0f; // _mag_offsets.x_offset + + magData.y = _mag.y; + magBias.y = 0.0f; // _mag_offsets.y_offset + + magData.z = _mag.z; + magBias.z = 0.0f; // _mag_offsets.y_offset + if (_initialized) { - // XXX we compensate the offsets upfront - should be close to zero. - magData.x = _mag.x; - magBias.x = 0.0f; // _mag_offsets.x_offset - - magData.y = _mag.y; - magBias.y = 0.0f; // _mag_offsets.y_offset - - magData.z = _mag.z; - magBias.z = 0.0f; // _mag_offsets.y_offset - fuseMagData = true; RecallStates(statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); FuseMagnetometer(); @@ -556,9 +560,12 @@ FixedwingEstimator::task_main() VtasMeas = _airspeed.true_airspeed_m_s; - fuseVtasData = true; - RecallStates(statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data - FuseAirspeed(); + if (_initialized) { + + fuseVtasData = true; + RecallStates(statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data + FuseAirspeed(); + } } else { fuseVtasData = false; } @@ -569,8 +576,11 @@ FixedwingEstimator::task_main() if (_initialized) { - math::EulerAngles euler(eulerEst[0], eulerEst[1], eulerEst[2]); - //warnx("est: %8.4f, %8.4f, %8.4f", eulerEst[0], eulerEst[1], eulerEst[2]); + math::Quaternion q(states[0], states[1], states[2], states[3]); + + _att.q = q; + _att.q_valid = true; + _att.R_valid = false; // /* lazily publish the attitude only once available */ // if (_att_pub > 0) { @@ -661,10 +671,16 @@ int fw_att_pos_estimator_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (estimator::g_estimator) { warnx("running"); - warnx("attitude: %8.4f, %8.4f, %8.4f", eulerEst[0], eulerEst[1], eulerEst[2]); - warnx("states [1-3]: %8.4f, %8.4f, %8.4f", states[0], states[1], states[2]); - warnx("states [4-6]: %8.4f, %8.4f, %8.4f", states[3], states[4], states[5]); - warnx("states [7-9]: %8.4f, %8.4f, %8.4f", states[6], states[7], states[8]); + + math::Quaternion q(states[0], states[1], states[2], states[3]); + math::EulerAngles euler(q); + + printf("attitude: roll: %8.4f, pitch %8.4f, yaw: %8.4f degrees", + math::degrees(euler.getPhi()), math::degrees(euler.getTheta()), math::degrees(euler.getPsi())); + + printf("states [1-3]: %8.4f, %8.4f, %8.4f\n", states[0], states[1], states[2]); + printf("states [4-6]: %8.4f, %8.4f, %8.4f\n", states[3], states[4], states[5]); + printf("states [7-9]: %8.4f, %8.4f, %8.4f\n", states[6], states[7], states[8]); exit(0); } else { From 421727274251e5fc81ca5ae503a180b7326a480d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 2 Jan 2014 20:17:53 +0100 Subject: [PATCH 011/381] Filter returns some results. Needs careful range and unit checking, but on track --- .../fw_att_pos_estimator_main.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 1ed526e83f..59fb3dc31a 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -578,9 +578,9 @@ FixedwingEstimator::task_main() math::Quaternion q(states[0], states[1], states[2], states[3]); - _att.q = q; - _att.q_valid = true; - _att.R_valid = false; + // _att.q[0] = q.r; + // _att.q_valid = true; + // _att.R_valid = false; // /* lazily publish the attitude only once available */ // if (_att_pub > 0) { @@ -623,7 +623,7 @@ FixedwingEstimator::start() _estimator_task = task_spawn_cmd("fw_att_pos_estimator", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, - 12048, + 6000, (main_t)&FixedwingEstimator::task_main_trampoline, nullptr); @@ -676,11 +676,11 @@ int fw_att_pos_estimator_main(int argc, char *argv[]) math::EulerAngles euler(q); printf("attitude: roll: %8.4f, pitch %8.4f, yaw: %8.4f degrees", - math::degrees(euler.getPhi()), math::degrees(euler.getTheta()), math::degrees(euler.getPsi())); + (double)math::degrees(euler.getPhi()), (double)math::degrees(euler.getTheta()), (double)math::degrees(euler.getPsi())); - printf("states [1-3]: %8.4f, %8.4f, %8.4f\n", states[0], states[1], states[2]); - printf("states [4-6]: %8.4f, %8.4f, %8.4f\n", states[3], states[4], states[5]); - printf("states [7-9]: %8.4f, %8.4f, %8.4f\n", states[6], states[7], states[8]); + printf("states [1-3]: %8.4f, %8.4f, %8.4f\n", (double)states[0], (double)states[1], (double)states[2]); + printf("states [4-6]: %8.4f, %8.4f, %8.4f\n", (double)states[3], (double)states[4], (double)states[5]); + printf("states [7-9]: %8.4f, %8.4f, %8.4f\n", (double)states[6], (double)states[7], (double)states[8]); exit(0); } else { From 8ad4b72fc76d702e7eac27c0aecfdb2885a8331d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 2 Jan 2014 22:17:16 +0100 Subject: [PATCH 012/381] Compile fixes, publishing estimated attitude now --- .../fw_att_pos_estimator_main.cpp | 44 ++++++++++++++----- 1 file changed, 34 insertions(+), 10 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 1ed526e83f..5de97f9b93 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -577,20 +577,44 @@ FixedwingEstimator::task_main() if (_initialized) { math::Quaternion q(states[0], states[1], states[2], states[3]); + math::Dcm R(q); + math::EulerAngles euler(R); - _att.q = q; + for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) + _att.R[i][j] = R(i, j); + + _att.timestamp = _gyro.timestamp; + _att.q[0] = states[0]; + _att.q[1] = states[1]; + _att.q[2] = states[2]; + _att.q[3] = states[3]; _att.q_valid = true; - _att.R_valid = false; + _att.R_valid = true; - // /* lazily publish the attitude only once available */ - // if (_att_pub > 0) { - // /* publish the attitude setpoint */ - // orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att); + _att.timestamp = _gyro.timestamp; + _att.roll = euler.getPhi(); + _att.pitch = euler.getTheta(); + _att.yaw = euler.getPsi(); + // XXX find the right state + _att.rollspeed = _gyro.x - states[11]; + _att.pitchspeed = _gyro.y - states[12]; + _att.yawspeed = _gyro.z - states[13]; + // gyro offsets + _att.rate_offsets[0] = states[11]; + _att.rate_offsets[1] = states[12]; + _att.rate_offsets[2] = states[13]; - // } else { - // /* advertise and publish */ - // _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att); - // } + /* lazily publish the attitude only once available */ + if (_att_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att); + + } else { + /* advertise and publish */ + _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att); + } + + _global_pos.timestamp = _gyro.timestamp; // /* lazily publish the position only once available */ // if (_global_pos_pub > 0) { From a5e95bde0a77cdd894b926860dba8bc98eaf4dbd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 3 Jan 2014 00:35:18 +0100 Subject: [PATCH 013/381] Added lots of instrumentation to ensure all data sources are coming in clean --- .../fw_att_pos_estimator_main.cpp | 27 ++++++++++++++++++- 1 file changed, 26 insertions(+), 1 deletion(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index f25ab5ec03..ef51c7524c 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -145,6 +145,12 @@ private: struct mag_scale _mag_offsets; perf_counter_t _loop_perf; /**< loop performance counter */ + perf_counter_t _perf_gyro; /// 2) { /* fuse GPS updates */ @@ -529,6 +552,7 @@ FixedwingEstimator::task_main() orb_check(_mag_sub, &mag_updated); if (mag_updated) { orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag); + perf_count(_perf_mag); // XXX we compensate the offsets upfront - should be close to zero. magData.x = _mag.x; @@ -555,6 +579,7 @@ FixedwingEstimator::task_main() orb_check(_airspeed_sub, &airspeed_updated); if (airspeed_updated && _initialized) { orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); + perf_count(_perf_airspeed); if (_airspeed.true_airspeed_m_s > 8.0f /* XXX magic number */) { From f9e810d915ad53962e136f9f9e5cf285765fb59f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 5 Jan 2014 01:56:04 +0100 Subject: [PATCH 014/381] Enable logging in HIL --- ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil | 2 ++ 1 file changed, 2 insertions(+) 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 181256dc71..6189d8fe3f 100644 --- a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil +++ b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil @@ -83,5 +83,7 @@ mixer load /dev/pwm_output /etc/mixers/FMU_AET.mix fw_pos_control_l1 start fw_att_control start +sdlog2 start -r 50 -a -b 16 + echo "[HIL] setup done, running" From d8f04556beca666d1f932a289f2437cbe6b53958 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 5 Jan 2014 01:56:24 +0100 Subject: [PATCH 015/381] Strip unused variable --- src/lib/geo/geo.c | 1 - 1 file changed, 1 deletion(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 43105fdba7..ce2c64a59c 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -199,7 +199,6 @@ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, doub double lat_next_rad = lat_next * M_DEG_TO_RAD; double lon_next_rad = lon_next * M_DEG_TO_RAD; - double d_lat = lat_next_rad - lat_now_rad; double d_lon = lon_next_rad - lon_now_rad; /* conscious mix of double and float trig function to maximize speed and efficiency */ From 8bd532c8556d7f99d1d66c161f182aeebd1ab325 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 5 Jan 2014 01:57:01 +0100 Subject: [PATCH 016/381] Change slightly the way we handle timestamps, apply same unknown scaling as on logfiles --- .../fw_att_pos_estimator_main.cpp | 22 ++++++++++--------- 1 file changed, 12 insertions(+), 10 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index ef51c7524c..44c25c407a 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -84,10 +84,11 @@ extern "C" __EXPORT int fw_att_pos_estimator_main(int argc, char *argv[]); __EXPORT uint32_t millis(); static uint64_t last_run = 0; +static uint32_t IMUmsec = 0; uint32_t millis() { - return last_run / 1e3; + return IMUmsec; } class FixedwingEstimator @@ -427,7 +428,7 @@ FixedwingEstimator::task_main() orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); } - float IMUmsec = _gyro.timestamp / 1e3; + IMUmsec = _gyro.timestamp / 1e3f; float deltaT = (_gyro.timestamp - last_run) / 1e6f; last_run = _gyro.timestamp; @@ -501,8 +502,6 @@ FixedwingEstimator::task_main() gpsLon = math::radians(_gps.lon / (double)1e7); gpsHgt = _gps.alt / 1e3f; - newDataGps = true; - if (hrt_elapsed_time(&start_time) > 500000 && !_initialized) { InitialiseFilter(); _initialized = true; @@ -540,12 +539,13 @@ FixedwingEstimator::task_main() fuseVelData = false; fusePosData = false; fuseHgtData = false; - - newDataGps = false; } } else { - newDataGps = false; + /* do not fuse anything, we got no position / vel update */ + fuseVelData = false; + fusePosData = false; + fuseHgtData = false; } bool mag_updated; @@ -555,13 +555,15 @@ FixedwingEstimator::task_main() perf_count(_perf_mag); // XXX we compensate the offsets upfront - should be close to zero. - magData.x = _mag.x; + // XXX the purpose of the 0.001 factor is unclear + // 0.001f + magData.x = 0.001f * _mag.x; magBias.x = 0.0f; // _mag_offsets.x_offset - magData.y = _mag.y; + magData.y = 0.001f * _mag.y; magBias.y = 0.0f; // _mag_offsets.y_offset - magData.z = _mag.z; + magData.z = 0.001f * _mag.z; magBias.z = 0.0f; // _mag_offsets.y_offset if (_initialized) { From 581d702478d8671faaf48c32dd7071e06d2b4254 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 6 Jan 2014 09:25:39 +0100 Subject: [PATCH 017/381] Added support for Octo Cox --- ROMFS/px4fmu_common/init.d/rcS | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 39c8384a76..90f2e2b178 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -223,7 +223,9 @@ then # 7000 .. 7999 Hexa + # 8000 .. 8999 Octo X # 9000 .. 9999 Octo + - # 10000 .. 19999 Wide arm / H frame + # 10000 .. 10999 Wide arm / H frame + # 11000 .. 11999 Hexa Cox + # 12000 .. 12999 Octo Cox if param compare SYS_AUTOSTART 4008 8 then @@ -287,6 +289,13 @@ then sh /etc/init.d/rc.octo set MODE custom fi + + if param compare SYS_AUTOSTART 12001 + then + set MIXER /etc/mixers/FMU_octo_cox.mix + sh /etc/init.d/rc.octo + set MODE custom + fi if param compare SYS_AUTOSTART 10015 15 then From 395033eeb0b76eacef78d489a69af8442fadf947 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Jan 2014 09:47:49 +0100 Subject: [PATCH 018/381] Switch to 21 state version, publish local position as well --- .../fw_att_pos_estimator_main.cpp | 41 ++++++++++++++++++- 1 file changed, 40 insertions(+), 1 deletion(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 44c25c407a..e6747604be 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -35,6 +35,8 @@ * @file fw_att_pos_estimator_main.cpp * Implementation of the attitude and position estimator. * + * @author Paul Riseborough + * @author Lorenz Meier */ #include @@ -55,6 +57,7 @@ #include #include #include +#include #include #include #include @@ -129,6 +132,7 @@ private: orb_advert_t _att_pub; /**< vehicle attitude */ orb_advert_t _global_pos_pub; /**< global position */ + orb_advert_t _local_pos_pub; /**< position in local frame */ struct vehicle_attitude_s _att; /**< vehicle attitude */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ @@ -139,6 +143,7 @@ private: struct airspeed_s _airspeed; /**< airspeed */ struct vehicle_status_s _vstatus; /**< vehicle status */ 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 gyro_scale _gyro_offsets; @@ -227,6 +232,7 @@ FixedwingEstimator::FixedwingEstimator() : /* publications */ _att_pub(-1), _global_pos_pub(-1), + _local_pos_pub(-1), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw_att_pos_estimator")), @@ -603,6 +609,15 @@ FixedwingEstimator::task_main() if (_initialized) { + // State vector: + // 0-3: quaternions (q0, q1, q2, q3) + // 4-6: Velocity - m/sec (North, East, Down) + // 7-9: Position - m (North, East, Down) + // 10-12: Delta Angle bias - rad (X,Y,Z) + // 13-14: Wind Vector - m/sec (North,East) + // 15-17: Earth Magnetic Field Vector - milligauss (North, East, Down) + // 18-20: Body Magnetic Field Vector - milligauss (X,Y,Z) + math::Quaternion q(states[0], states[1], states[2], states[3]); math::Dcm R(q); math::EulerAngles euler(R); @@ -641,9 +656,33 @@ FixedwingEstimator::task_main() _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att); } + _local_pos.timestamp = _gyro.timestamp; + _local_pos.x = states[7]; + _local_pos.y = states[8]; + _local_pos.z = states[9]; + + _local_pos.vx = states[4]; + _local_pos.vy = states[5]; + _local_pos.vz = states[6]; + + _local_pos.xy_valid = true; + _local_pos.z_valid = true; + _local_pos.v_xy_valid = true; + _local_pos.v_z_valid = true; + + /* lazily publish the local position only once available */ + if (_local_pos_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &_local_pos); + + } else { + /* advertise and publish */ + _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &_local_pos); + } + _global_pos.timestamp = _gyro.timestamp; - // /* lazily publish the position only once available */ + // /* lazily publish the global position only once available */ // if (_global_pos_pub > 0) { // /* publish the attitude setpoint */ // orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos); From 55eaa38c651be41eee31b86330edbf27869385f8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 9 Jan 2014 08:10:03 +0100 Subject: [PATCH 019/381] Documentation cleanup in navigator --- src/modules/navigator/navigator_main.cpp | 24 ++++++++++++++---------- 1 file changed, 14 insertions(+), 10 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index bb7520a03f..63952be6ee 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1,9 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * @author Jean Cyr - * @author Julian Oes + * 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 @@ -34,11 +31,18 @@ * ****************************************************************************/ /** - * @file navigator_main.c + * @file navigator_main.cpp * Implementation of the main navigation state machine. * * Handles missions, geo fencing and failsafe navigation behavior. * Published the mission item triplet for the position controller. + * + * @author Julian Oes + * @author Jean Cyr + * @author Anton Babushkin + * @author Thomas Gubler + * @author Lorenz Meier + * */ #include @@ -349,11 +353,11 @@ private: namespace navigator { -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; +// /* oddly, ERROR is not defined for c++ */ +// #ifdef ERROR +// # undef ERROR +// #endif +// static const int ERROR = -1; Navigator *g_navigator; } From 03c543aba6440c25c5d349791b5a6b33914cb74c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 9 Jan 2014 08:10:35 +0100 Subject: [PATCH 020/381] MAVLink multi-port WIP, not compiling yet --- src/modules/mavlink/mavlink.c | 802 +------- src/modules/mavlink/mavlink_bridge_header.h | 3 +- src/modules/mavlink/mavlink_main.cpp | 1819 +++++++++++++++++ src/modules/mavlink/mavlink_main.h | 255 +++ ...rb_listener.c => mavlink_orb_listener.cpp} | 199 +- src/modules/mavlink/mavlink_orb_listener.h | 143 ++ src/modules/mavlink/mavlink_parameters.c | 230 --- src/modules/mavlink/mavlink_parameters.h | 104 - src/modules/mavlink/mavlink_receiver.cpp | 12 +- .../{mavlink_hil.h => mavlink_receiver.h} | 64 +- src/modules/mavlink/module.mk | 12 +- src/modules/mavlink/orb_topics.h | 125 -- src/modules/mavlink/util.h | 3 +- src/modules/mavlink/waypoints.c | 730 ------- src/modules/mavlink/waypoints.h | 13 +- 15 files changed, 2416 insertions(+), 2098 deletions(-) create mode 100644 src/modules/mavlink/mavlink_main.cpp create mode 100644 src/modules/mavlink/mavlink_main.h rename src/modules/mavlink/{orb_listener.c => mavlink_orb_listener.cpp} (82%) create mode 100644 src/modules/mavlink/mavlink_orb_listener.h delete mode 100644 src/modules/mavlink/mavlink_parameters.c delete mode 100644 src/modules/mavlink/mavlink_parameters.h rename src/modules/mavlink/{mavlink_hil.h => mavlink_receiver.h} (65%) delete mode 100644 src/modules/mavlink/orb_topics.h delete mode 100644 src/modules/mavlink/waypoints.c diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 4d3c9dd2cb..aba8c98daf 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,68 +33,49 @@ /** * @file mavlink.c - * MAVLink 1.0 protocol implementation. + * Adapter functions expected by the protocol library * * @author Lorenz Meier */ #include #include -#include #include -#include #include -#include -#include #include #include "mavlink_bridge_header.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - #include -#include -#include -#include -#include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include -#include "waypoints.h" -#include "orb_topics.h" -#include "mavlink_hil.h" -#include "util.h" -#include "waypoints.h" -#include "mavlink_parameters.h" +// +// #include +// #include +// #include +// #include -#include +// #include "waypoints.h" +// #include "orb_topics.h" +// #include "mavlink_hil.h" +// #include "util.h" +// #include "waypoints.h" +// #include "mavlink_parameters.h" + +// #include /* define MAVLink specific parameters */ PARAM_DEFINE_INT32(MAV_SYS_ID, 1); PARAM_DEFINE_INT32(MAV_COMP_ID, 50); PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_FIXED_WING); -__EXPORT int mavlink_main(int argc, char *argv[]); - -static int mavlink_thread_main(int argc, char *argv[]); - -/* thread state */ -volatile bool thread_should_exit = false; -static volatile bool thread_running = false; -static int mavlink_task; - -/* pthreads */ -static pthread_t receive_thread; -static pthread_t uorb_receive_thread; - -/* terminate MAVLink on user request - disabled by default */ -static bool mavlink_link_termination_allowed = false; - mavlink_system_t mavlink_system = { 100, 50, @@ -105,353 +85,40 @@ mavlink_system_t mavlink_system = { 0 }; // System ID, 1-255, Component/Subsystem ID, 1-255 -/* XXX not widely used */ -uint8_t chan = MAVLINK_COMM_0; - -/* XXX probably should be in a header... */ -extern pthread_t receive_start(int uart); - -/* Allocate storage space for waypoints */ -static mavlink_wpm_storage wpm_s; -mavlink_wpm_storage *wpm = &wpm_s; - -bool mavlink_hil_enabled = false; - -/* protocol interface */ -static int uart; -static int baudrate; -bool gcs_link = true; - -/* interface mode */ -static enum { - MAVLINK_INTERFACE_MODE_OFFBOARD, - MAVLINK_INTERFACE_MODE_ONBOARD -} mavlink_link_mode = MAVLINK_INTERFACE_MODE_OFFBOARD; - -static struct mavlink_logbuffer lb; - -static void mavlink_update_system(void); -static int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); -static void usage(void); -int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval); - - - -int -set_hil_on_off(bool hil_enabled) -{ - int ret = OK; - - /* Enable HIL */ - if (hil_enabled && !mavlink_hil_enabled) { - - mavlink_hil_enabled = true; - - /* ramp up some HIL-related subscriptions */ - unsigned hil_rate_interval; - - if (baudrate < 19200) { - /* 10 Hz */ - hil_rate_interval = 100; - - } else if (baudrate < 38400) { - /* 10 Hz */ - hil_rate_interval = 100; - - } else if (baudrate < 115200) { - /* 20 Hz */ - hil_rate_interval = 50; - - } else { - /* 200 Hz */ - hil_rate_interval = 5; - } - - orb_set_interval(mavlink_subs.spa_sub, hil_rate_interval); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, hil_rate_interval); - } - - if (!hil_enabled && mavlink_hil_enabled) { - mavlink_hil_enabled = false; - orb_set_interval(mavlink_subs.spa_sub, 200); - - } else { - ret = ERROR; - } - - return ret; -} - -void -get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode) -{ - /* reset MAVLink mode bitfield */ - *mavlink_base_mode = 0; - *mavlink_custom_mode = 0; - - /** - * Set mode flags - **/ - - /* HIL */ - if (v_status.hil_state == HIL_STATE_ON) { - *mavlink_base_mode |= MAV_MODE_FLAG_HIL_ENABLED; - } - - /* arming state */ - if (v_status.arming_state == ARMING_STATE_ARMED - || v_status.arming_state == ARMING_STATE_ARMED_ERROR) { - *mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; - } - - /* main state */ - *mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; - union px4_custom_mode custom_mode; - custom_mode.data = 0; - if (v_status.main_state == MAIN_STATE_MANUAL) { - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (v_status.is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; - } else if (v_status.main_state == MAIN_STATE_SEATBELT) { - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT; - } else if (v_status.main_state == MAIN_STATE_EASY) { - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY; - } else if (v_status.main_state == MAIN_STATE_AUTO) { - *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - if (control_mode.nav_state == NAV_STATE_NONE) { // failsafe, shouldn't happen - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; - } else if (control_mode.nav_state == NAV_STATE_READY) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; - } else if (control_mode.nav_state == NAV_STATE_LOITER) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; - } else if (control_mode.nav_state == NAV_STATE_MISSION) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; - } else if (control_mode.nav_state == NAV_STATE_RTL) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; - } - } - *mavlink_custom_mode = custom_mode.data; - - /** - * Set mavlink state - **/ - - /* set calibration state */ - if (v_status.arming_state == ARMING_STATE_INIT - || v_status.arming_state == ARMING_STATE_IN_AIR_RESTORE - || v_status.arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review - *mavlink_state = MAV_STATE_UNINIT; - } else if (v_status.arming_state == ARMING_STATE_ARMED) { - *mavlink_state = MAV_STATE_ACTIVE; - } else if (v_status.arming_state == ARMING_STATE_ARMED_ERROR) { - *mavlink_state = MAV_STATE_CRITICAL; - } else if (v_status.arming_state == ARMING_STATE_STANDBY) { - *mavlink_state = MAV_STATE_STANDBY; - } else if (v_status.arming_state == ARMING_STATE_REBOOT) { - *mavlink_state = MAV_STATE_POWEROFF; - } else { - warnx("Unknown mavlink state"); - *mavlink_state = MAV_STATE_CRITICAL; - } -} - - -int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval) -{ - int ret = OK; - - switch (mavlink_msg_id) { - case MAVLINK_MSG_ID_SCALED_IMU: - /* sensor sub triggers scaled IMU */ - orb_set_interval(subs->sensor_sub, min_interval); - break; - - case MAVLINK_MSG_ID_HIGHRES_IMU: - /* sensor sub triggers highres IMU */ - orb_set_interval(subs->sensor_sub, min_interval); - break; - - case MAVLINK_MSG_ID_RAW_IMU: - /* sensor sub triggers RAW IMU */ - orb_set_interval(subs->sensor_sub, min_interval); - break; - - case MAVLINK_MSG_ID_ATTITUDE: - /* attitude sub triggers attitude */ - orb_set_interval(subs->att_sub, min_interval); - break; - - case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW: - /* actuator_outputs triggers this message */ - orb_set_interval(subs->act_0_sub, min_interval); - orb_set_interval(subs->act_1_sub, min_interval); - orb_set_interval(subs->act_2_sub, min_interval); - orb_set_interval(subs->act_3_sub, min_interval); - orb_set_interval(subs->actuators_sub, min_interval); - orb_set_interval(subs->actuators_effective_sub, min_interval); - orb_set_interval(subs->spa_sub, min_interval); - orb_set_interval(mavlink_subs.rates_setpoint_sub, min_interval); - break; - - case MAVLINK_MSG_ID_MANUAL_CONTROL: - /* manual_control_setpoint triggers this message */ - orb_set_interval(subs->man_control_sp_sub, min_interval); - break; - - case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT: - orb_set_interval(subs->debug_key_value, min_interval); - break; - - default: - /* not found */ - ret = ERROR; - break; - } - - return ret; -} - - -/**************************************************************************** - * MAVLink text message logger - ****************************************************************************/ - -static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); - -static const struct file_operations mavlink_fops = { - .ioctl = mavlink_dev_ioctl -}; - -static int -mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) -{ - static unsigned int total_counter = 0; - - switch (cmd) { - case (int)MAVLINK_IOC_SEND_TEXT_INFO: - case (int)MAVLINK_IOC_SEND_TEXT_CRITICAL: - case (int)MAVLINK_IOC_SEND_TEXT_EMERGENCY: { - const char *txt = (const char *)arg; - struct mavlink_logmessage msg; - strncpy(msg.text, txt, sizeof(msg.text)); - mavlink_logbuffer_write(&lb, &msg); - total_counter++; - return OK; - } - - default: - return ENOTTY; - } -} - -#define MAVLINK_OFFBOARD_CONTROL_FLAG_ARMED 0x10 - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb) -{ - /* process baud rate */ - int speed; - - switch (baud) { - case 0: speed = B0; break; - - case 50: speed = B50; break; - - case 75: speed = B75; break; - - case 110: speed = B110; break; - - case 134: speed = B134; break; - - case 150: speed = B150; break; - - case 200: speed = B200; break; - - case 300: speed = B300; break; - - case 600: speed = B600; break; - - case 1200: speed = B1200; break; - - case 1800: speed = B1800; break; - - case 2400: speed = B2400; break; - - case 4800: speed = B4800; break; - - case 9600: speed = B9600; break; - - case 19200: speed = B19200; break; - - case 38400: speed = B38400; break; - - case 57600: speed = B57600; break; - - case 115200: speed = B115200; break; - - case 230400: speed = B230400; break; - - case 460800: speed = B460800; break; - - case 921600: speed = B921600; break; - - default: - warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600\n\n", baud); - return -EINVAL; - } - - /* open uart */ - warnx("UART is %s, baudrate is %d\n", uart_name, baud); - uart = open(uart_name, O_RDWR | O_NOCTTY); - - /* Try to set baud rate */ - struct termios uart_config; - int termios_state; - *is_usb = false; - - /* Back up the original uart configuration to restore it after exit */ - if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { - warnx("ERROR get termios config %s: %d\n", uart_name, termios_state); - close(uart); - return -1; - } - - /* Fill the struct for the new configuration */ - tcgetattr(uart, &uart_config); - - /* Clear ONLCR flag (which appends a CR for every LF) */ - uart_config.c_oflag &= ~ONLCR; - - /* USB serial is indicated by /dev/ttyACM0*/ - if (strcmp(uart_name, "/dev/ttyACM0") != OK && strcmp(uart_name, "/dev/ttyACM1") != OK) { - - /* Set baud rate */ - 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); - 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); - close(uart); - return -1; - } - - return uart; -} - +/* + * Internal function to send the bytes through the right serial port + */ void mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length) { + int uart = -1; + + switch (channel) { + case MAVLINK_COMM_0: + uart = Mavlink::get_uart_fd(0); + break; + case MAVLINK_COMM_1: + uart = Mavlink::get_uart_fd(1); + break; + case MAVLINK_COMM_2: + uart = Mavlink::get_uart_fd(2); + break; + case MAVLINK_COMM_3: + uart = Mavlink::get_uart_fd(3); + break; + case MAVLINK_COMM_4: + uart = Mavlink::get_uart_fd(4); + break; + case MAVLINK_COMM_5: + uart = Mavlink::get_uart_fd(5); + break; + case MAVLINK_COMM_6: + uart = Mavlink::get_uart_fd(6); + break; + } + write(uart, ch, (size_t)(sizeof(uint8_t) * length)); + } /* @@ -471,362 +138,3 @@ extern mavlink_message_t *mavlink_get_channel_buffer(uint8_t channel) static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS]; return &m_mavlink_buffer[channel]; } - -void mavlink_update_system(void) -{ - static bool initialized = false; - static param_t param_system_id; - static param_t param_component_id; - static param_t param_system_type; - - if (!initialized) { - param_system_id = param_find("MAV_SYS_ID"); - param_component_id = param_find("MAV_COMP_ID"); - param_system_type = param_find("MAV_TYPE"); - initialized = true; - } - - /* update system and component id */ - int32_t system_id; - param_get(param_system_id, &system_id); - - if (system_id > 0 && system_id < 255) { - mavlink_system.sysid = system_id; - } - - int32_t component_id; - param_get(param_component_id, &component_id); - - if (component_id > 0 && component_id < 255) { - mavlink_system.compid = component_id; - } - - int32_t system_type; - param_get(param_system_type, &system_type); - - if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) { - mavlink_system.type = system_type; - } -} - -/** - * MAVLink Protocol main function. - */ -int mavlink_thread_main(int argc, char *argv[]) -{ - /* initialize mavlink text message buffering */ - mavlink_logbuffer_init(&lb, 10); - - int ch; - char *device_name = "/dev/ttyS1"; - baudrate = 57600; - - /* work around some stupidity in task_create's argv handling */ - argc -= 2; - argv += 2; - - while ((ch = getopt(argc, argv, "b:d:eo")) != EOF) { - switch (ch) { - case 'b': - baudrate = strtoul(optarg, NULL, 10); - - if (baudrate < 9600 || baudrate > 921600) - errx(1, "invalid baud rate '%s'", optarg); - - break; - - case 'd': - device_name = optarg; - break; - - case 'e': - mavlink_link_termination_allowed = true; - break; - - case 'o': - mavlink_link_mode = MAVLINK_INTERFACE_MODE_ONBOARD; - break; - - default: - usage(); - break; - } - } - - struct termios uart_config_original; - - bool usb_uart; - - /* print welcome text */ - warnx("MAVLink v1.0 serial interface starting..."); - - /* inform about mode */ - warnx((mavlink_link_mode == MAVLINK_INTERFACE_MODE_ONBOARD) ? "ONBOARD MODE" : "DOWNLINK MODE"); - - /* Flush stdout in case MAVLink is about to take it over */ - fflush(stdout); - - /* default values for arguments */ - uart = mavlink_open_uart(baudrate, device_name, &uart_config_original, &usb_uart); - - if (uart < 0) - err(1, "could not open %s", device_name); - - /* create the device node that's used for sending text log messages, etc. */ - register_driver(MAVLINK_LOG_DEVICE, &mavlink_fops, 0666, NULL); - - /* Initialize system properties */ - mavlink_update_system(); - - /* start the MAVLink receiver */ - receive_thread = receive_start(uart); - - /* start the ORB receiver */ - uorb_receive_thread = uorb_receive_start(); - - /* initialize waypoint manager */ - mavlink_wpm_init(wpm); - - /* all subscriptions are now active, set up initial guess about rate limits */ - if (baudrate >= 230400) { - /* 200 Hz / 5 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 20); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 20); - /* 50 Hz / 20 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 30); - /* 20 Hz / 50 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 10); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50); - /* 10 Hz */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 100); - /* 10 Hz */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 100); - - } else if (baudrate >= 115200) { - /* 20 Hz / 50 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 50); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 50); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 50); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50); - /* 5 Hz / 200 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200); - /* 5 Hz / 200 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 200); - /* 2 Hz */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); - - } else if (baudrate >= 57600) { - /* 10 Hz / 100 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 300); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 300); - /* 10 Hz / 100 ms ATTITUDE */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 200); - /* 5 Hz / 200 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200); - /* 5 Hz / 200 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 500); - /* 2 Hz */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); - /* 2 Hz */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 500); - - } else { - /* very low baud rate, limit to 1 Hz / 1000 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 1000); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 1000); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 1000); - /* 1 Hz / 1000 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 1000); - /* 0.5 Hz */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 2000); - /* 0.1 Hz */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 10000); - } - - int mission_result_sub = orb_subscribe(ORB_ID(mission_result)); - struct mission_result_s mission_result; - memset(&mission_result, 0, sizeof(mission_result)); - - thread_running = true; - - /* arm counter to go off immediately */ - unsigned lowspeed_counter = 10; - - while (!thread_should_exit) { - - /* 1 Hz */ - if (lowspeed_counter == 10) { - mavlink_update_system(); - - /* translate the current system state to mavlink state and mode */ - uint8_t mavlink_state = 0; - uint8_t mavlink_base_mode = 0; - uint32_t mavlink_custom_mode = 0; - get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); - - /* send heartbeat */ - mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_base_mode, mavlink_custom_mode, mavlink_state); - - /* switch HIL mode if required */ - if (v_status.hil_state == HIL_STATE_ON) - set_hil_on_off(true); - else if (v_status.hil_state == HIL_STATE_OFF) - set_hil_on_off(false); - - /* send status (values already copied in the section above) */ - mavlink_msg_sys_status_send(chan, - v_status.onboard_control_sensors_present, - v_status.onboard_control_sensors_enabled, - v_status.onboard_control_sensors_health, - v_status.load * 1000.0f, - v_status.battery_voltage * 1000.0f, - v_status.battery_current * 1000.0f, - v_status.battery_remaining, - v_status.drop_rate_comm, - v_status.errors_comm, - v_status.errors_count1, - v_status.errors_count2, - v_status.errors_count3, - v_status.errors_count4); - lowspeed_counter = 0; - } - - lowspeed_counter++; - - bool updated; - orb_check(mission_result_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result); - - if (mission_result.mission_reached) { - mavlink_wpm_send_waypoint_reached((uint16_t)mission_result.mission_index); - } - } - - mavlink_waypoint_eventloop(hrt_absolute_time()); - - /* sleep quarter the time */ - usleep(25000); - - /* check if waypoint has been reached against the last positions */ - mavlink_waypoint_eventloop(hrt_absolute_time()); - - /* sleep quarter the time */ - usleep(25000); - - /* send parameters at 20 Hz (if queued for sending) */ - mavlink_pm_queued_send(); - mavlink_waypoint_eventloop(hrt_absolute_time()); - - /* sleep quarter the time */ - usleep(25000); - - mavlink_waypoint_eventloop(hrt_absolute_time()); - - if (baudrate > 57600) { - mavlink_pm_queued_send(); - } - - /* sleep 10 ms */ - usleep(10000); - - /* send one string at 10 Hz */ - if (!mavlink_logbuffer_is_empty(&lb)) { - struct mavlink_logmessage msg; - int lb_ret = mavlink_logbuffer_read(&lb, &msg); - - if (lb_ret == OK) { - mavlink_missionlib_send_gcs_string(msg.text); - } - } - - /* sleep 15 ms */ - usleep(15000); - } - - /* wait for threads to complete */ - pthread_join(receive_thread, NULL); - pthread_join(uorb_receive_thread, NULL); - - /* Reset the UART flags to original state */ - tcsetattr(uart, TCSANOW, &uart_config_original); - - /* destroy log buffer */ - //mavlink_logbuffer_destroy(&lb); - - thread_running = false; - - return 0; -} - -static void -usage() -{ - fprintf(stderr, "usage: mavlink start [-d ] [-b ]\n" - " mavlink stop\n" - " mavlink status\n"); - exit(1); -} - -int mavlink_main(int argc, char *argv[]) -{ - - if (argc < 2) { - warnx("missing command"); - usage(); - } - - if (!strcmp(argv[1], "start")) { - - /* this is not an error */ - if (thread_running) - errx(0, "mavlink already running"); - - thread_should_exit = false; - mavlink_task = task_spawn_cmd("mavlink", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 2048, - mavlink_thread_main, - (const char **)argv); - - while (!thread_running) { - usleep(200); - } - - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - - /* this is not an error */ - if (!thread_running) - errx(0, "mavlink already stopped"); - - thread_should_exit = true; - - while (thread_running) { - usleep(200000); - warnx("."); - } - - warnx("terminated."); - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - errx(0, "running"); - - } else { - errx(1, "not running"); - } - } - - warnx("unrecognized command"); - usage(); - /* not getting here */ - return 0; -} - diff --git a/src/modules/mavlink/mavlink_bridge_header.h b/src/modules/mavlink/mavlink_bridge_header.h index 149efda60d..1fae3ee9d0 100644 --- a/src/modules/mavlink/mavlink_bridge_header.h +++ b/src/modules/mavlink/mavlink_bridge_header.h @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp new file mode 100644 index 0000000000..339030a868 --- /dev/null +++ b/src/modules/mavlink/mavlink_main.cpp @@ -0,0 +1,1819 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file mavlink_main.cpp + * MAVLink 1.0 protocol implementation. + * + * @author Lorenz Meier + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +#include "mavlink_bridge_header.h" +#include "mavlink_parameters.h" +#include +#include "math.h" /* isinf / isnan checks */ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +/** + * mavlink app start / stop handling function + * + * @ingroup apps + */ +extern "C" __EXPORT int mavlink_main(int argc, char *argv[]); + + + +namespace mavlink +{ + + Mavlink *g_mavlink; +} + +Mavlink::Mavlink(const char *port, unsigned baud_rate) : + + _task_should_exit(false), + _mavlink_task(-1), + _mavlink_fd(-1), + _mavlink_incoming_fd(-1), + +/* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "mavlink")), + _mavlink_hil_enabled(false) + // _params_sub(-1) +{ + // _parameter_handles.min_altitude = param_find("NAV_MIN_ALT"); + +} + +Mavlink::~Mavlink() +{ + if (_mavlink_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(_mavlink_task); + break; + } + } while (_mavlink_task != -1); + } +} + +int Mavlink::instance_count() +{ + /* note: a local buffer count will help if this ever is called often */ + Mavlink* inst = _head; + unsigned inst_index = 0; + while (inst->_head != nullptr) { + inst = inst->_head; + inst_index++; + } + + return inst_index + 1; +} + +Mavlink* Mavlink::new_instance(const char *port, unsigned baud_rate) +{ + Mavlink* inst = new Mavlink(port, baud_rate); + Mavlink* parent = _head; + while (parent->_head != nullptr) + parent = parent->_head; + + /* now parent points to a null pointer, fill it */ + parent->_head = inst; + + return inst; +} + +Mavlink* Mavlink::get_instance(unsigned instance) +{ + Mavlink* inst = _head; + unsigned inst_index = 0; + while (inst->_head != nullptr && inst_index < instance) { + inst = inst->_head; + inst_index++; + } + + if (inst_index < instance) { + inst = nullptr; + } + + return inst; +} + +void +Mavlink::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)); + +} + +/* XXX not widely used */ +uint8_t chan = MAVLINK_COMM_0; + +/* XXX probably should be in a header... */ +extern pthread_t receive_start(int uart); + +/* Allocate storage space for waypoints */ +static mavlink_wpm_storage wpm_s; +mavlink_wpm_storage *wpm = &wpm_s; + +bool mavlink_hil_enabled = false; + +/* protocol interface */ +static int uart; +static int baudrate; +bool gcs_link = true; + +/* interface mode */ +static enum { + MAVLINK_INTERFACE_MODE_OFFBOARD, + MAVLINK_INTERFACE_MODE_ONBOARD +} mavlink_link_mode = MAVLINK_INTERFACE_MODE_OFFBOARD; + +static struct mavlink_logbuffer lb; + +static void mavlink_update_system(void); +static int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); +static void usage(void); +int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval); + +/**************************************************************************** + * MAVLink text message logger + ****************************************************************************/ + +static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); + +static const struct file_operations mavlink_fops = { + .ioctl = mavlink_dev_ioctl +}; + +static int +mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) +{ + static unsigned int total_counter = 0; + + switch (cmd) { + case (int)MAVLINK_IOC_SEND_TEXT_INFO: + case (int)MAVLINK_IOC_SEND_TEXT_CRITICAL: + case (int)MAVLINK_IOC_SEND_TEXT_EMERGENCY: { + const char *txt = (const char *)arg; + struct mavlink_logmessage msg; + strncpy(msg.text, txt, sizeof(msg.text)); + mavlink_logbuffer_write(&lb, &msg); + total_counter++; + return OK; + } + + default: + return ENOTTY; + } +} + +void mavlink_update_system(void) +{ + static bool initialized = false; + static param_t param_system_id; + static param_t param_component_id; + static param_t param_system_type; + + if (!initialized) { + param_system_id = param_find("MAV_SYS_ID"); + param_component_id = param_find("MAV_COMP_ID"); + param_system_type = param_find("MAV_TYPE"); + initialized = true; + } + + /* update system and component id */ + int32_t system_id; + param_get(param_system_id, &system_id); + + if (system_id > 0 && system_id < 255) { + mavlink_system.sysid = system_id; + } + + int32_t component_id; + param_get(param_component_id, &component_id); + + if (component_id > 0 && component_id < 255) { + mavlink_system.compid = component_id; + } + + int32_t system_type; + param_get(param_system_type, &system_type); + + if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) { + mavlink_system.type = system_type; + } +} + +int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb) +{ + /* process baud rate */ + int speed; + + switch (baud) { + case 0: speed = B0; break; + + case 50: speed = B50; break; + + case 75: speed = B75; break; + + case 110: speed = B110; break; + + case 134: speed = B134; break; + + case 150: speed = B150; break; + + case 200: speed = B200; break; + + case 300: speed = B300; break; + + case 600: speed = B600; break; + + case 1200: speed = B1200; break; + + case 1800: speed = B1800; break; + + case 2400: speed = B2400; break; + + case 4800: speed = B4800; break; + + case 9600: speed = B9600; break; + + case 19200: speed = B19200; break; + + case 38400: speed = B38400; break; + + case 57600: speed = B57600; break; + + case 115200: speed = B115200; break; + + case 230400: speed = B230400; break; + + case 460800: speed = B460800; break; + + case 921600: speed = B921600; break; + + default: + warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600\n\n", baud); + return -EINVAL; + } + + /* open uart */ + warnx("UART is %s, baudrate is %d\n", uart_name, baud); + uart = open(uart_name, O_RDWR | O_NOCTTY); + + /* Try to set baud rate */ + struct termios uart_config; + int termios_state; + *is_usb = false; + + /* Back up the original uart configuration to restore it after exit */ + if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { + warnx("ERROR get termios config %s: %d\n", uart_name, termios_state); + close(uart); + return -1; + } + + /* Fill the struct for the new configuration */ + tcgetattr(uart, &uart_config); + + /* Clear ONLCR flag (which appends a CR for every LF) */ + uart_config.c_oflag &= ~ONLCR; + + /* USB serial is indicated by /dev/ttyACM0*/ + if (strcmp(uart_name, "/dev/ttyACM0") != OK && strcmp(uart_name, "/dev/ttyACM1") != OK) { + + /* Set baud rate */ + 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); + 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); + close(uart); + return -1; + } + + return uart; +} + +int +set_hil_on_off(bool hil_enabled) +{ + int ret = OK; + + /* Enable HIL */ + if (hil_enabled && !mavlink_hil_enabled) { + + mavlink_hil_enabled = true; + + /* ramp up some HIL-related subscriptions */ + unsigned hil_rate_interval; + + if (baudrate < 19200) { + /* 10 Hz */ + hil_rate_interval = 100; + + } else if (baudrate < 38400) { + /* 10 Hz */ + hil_rate_interval = 100; + + } else if (baudrate < 115200) { + /* 20 Hz */ + hil_rate_interval = 50; + + } else { + /* 200 Hz */ + hil_rate_interval = 5; + } + + orb_set_interval(mavlink_subs.spa_sub, hil_rate_interval); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, hil_rate_interval); + } + + if (!hil_enabled && mavlink_hil_enabled) { + mavlink_hil_enabled = false; + orb_set_interval(mavlink_subs.spa_sub, 200); + + } else { + ret = ERROR; + } + + return ret; +} + +void +get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode) +{ + /* reset MAVLink mode bitfield */ + *mavlink_base_mode = 0; + *mavlink_custom_mode = 0; + + /** + * Set mode flags + **/ + + /* HIL */ + if (v_status.hil_state == HIL_STATE_ON) { + *mavlink_base_mode |= MAV_MODE_FLAG_HIL_ENABLED; + } + + /* arming state */ + if (v_status.arming_state == ARMING_STATE_ARMED + || v_status.arming_state == ARMING_STATE_ARMED_ERROR) { + *mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; + } + + /* main state */ + *mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; + union px4_custom_mode custom_mode; + custom_mode.data = 0; + if (v_status.main_state == MAIN_STATE_MANUAL) { + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (v_status.is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; + } else if (v_status.main_state == MAIN_STATE_SEATBELT) { + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT; + } else if (v_status.main_state == MAIN_STATE_EASY) { + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY; + } else if (v_status.main_state == MAIN_STATE_AUTO) { + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + if (control_mode.nav_state == NAV_STATE_NONE) { // failsafe, shouldn't happen + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; + } else if (control_mode.nav_state == NAV_STATE_READY) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; + } else if (control_mode.nav_state == NAV_STATE_LOITER) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; + } else if (control_mode.nav_state == NAV_STATE_MISSION) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; + } else if (control_mode.nav_state == NAV_STATE_RTL) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; + } + } + *mavlink_custom_mode = custom_mode.data; + + /** + * Set mavlink state + **/ + + /* set calibration state */ + if (v_status.arming_state == ARMING_STATE_INIT + || v_status.arming_state == ARMING_STATE_IN_AIR_RESTORE + || v_status.arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review + *mavlink_state = MAV_STATE_UNINIT; + } else if (v_status.arming_state == ARMING_STATE_ARMED) { + *mavlink_state = MAV_STATE_ACTIVE; + } else if (v_status.arming_state == ARMING_STATE_ARMED_ERROR) { + *mavlink_state = MAV_STATE_CRITICAL; + } else if (v_status.arming_state == ARMING_STATE_STANDBY) { + *mavlink_state = MAV_STATE_STANDBY; + } else if (v_status.arming_state == ARMING_STATE_REBOOT) { + *mavlink_state = MAV_STATE_POWEROFF; + } else { + warnx("Unknown mavlink state"); + *mavlink_state = MAV_STATE_CRITICAL; + } +} + + +int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval) +{ + int ret = OK; + + switch (mavlink_msg_id) { + case MAVLINK_MSG_ID_SCALED_IMU: + /* sensor sub triggers scaled IMU */ + orb_set_interval(subs->sensor_sub, min_interval); + break; + + case MAVLINK_MSG_ID_HIGHRES_IMU: + /* sensor sub triggers highres IMU */ + orb_set_interval(subs->sensor_sub, min_interval); + break; + + case MAVLINK_MSG_ID_RAW_IMU: + /* sensor sub triggers RAW IMU */ + orb_set_interval(subs->sensor_sub, min_interval); + break; + + case MAVLINK_MSG_ID_ATTITUDE: + /* attitude sub triggers attitude */ + orb_set_interval(subs->att_sub, min_interval); + break; + + case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW: + /* actuator_outputs triggers this message */ + orb_set_interval(subs->act_0_sub, min_interval); + orb_set_interval(subs->act_1_sub, min_interval); + orb_set_interval(subs->act_2_sub, min_interval); + orb_set_interval(subs->act_3_sub, min_interval); + orb_set_interval(subs->actuators_sub, min_interval); + orb_set_interval(subs->actuators_effective_sub, min_interval); + orb_set_interval(subs->spa_sub, min_interval); + orb_set_interval(mavlink_subs.rates_setpoint_sub, min_interval); + break; + + case MAVLINK_MSG_ID_MANUAL_CONTROL: + /* manual_control_setpoint triggers this message */ + orb_set_interval(subs->man_control_sp_sub, min_interval); + break; + + case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT: + orb_set_interval(subs->debug_key_value, min_interval); + break; + + default: + /* not found */ + ret = ERROR; + break; + } + + return ret; +} + +extern mavlink_system_t mavlink_system; + +extern int mavlink_missionlib_send_message(mavlink_message_t *msg); +extern int mavlink_missionlib_send_gcs_string(const char *string); + +/** + * If the queue index is not at 0, the queue sending + * logic will send parameters from the current index + * to len - 1, the end of the param list. + */ +static unsigned int mavlink_param_queue_index = 0; + +/** + * Callback for param interface. + */ +void mavlink_pm_callback(void *arg, param_t param); + +void mavlink_pm_callback(void *arg, param_t param) +{ + mavlink_pm_send_param(param); + usleep(*(unsigned int *)arg); +} + +void mavlink_pm_send_all_params(unsigned int delay) +{ + unsigned int dbuf = delay; + param_foreach(&mavlink_pm_callback, &dbuf, false); +} + +int mavlink_pm_queued_send() +{ + if (mavlink_param_queue_index < param_count()) { + mavlink_pm_send_param(param_for_index(mavlink_param_queue_index)); + mavlink_param_queue_index++; + return 0; + + } else { + return 1; + } +} + +void mavlink_pm_start_queued_send() +{ + mavlink_param_queue_index = 0; +} + +int mavlink_pm_send_param_for_index(uint16_t index) +{ + return mavlink_pm_send_param(param_for_index(index)); +} + +int mavlink_pm_send_param_for_name(const char *name) +{ + return mavlink_pm_send_param(param_find(name)); +} + +int mavlink_pm_send_param(param_t param) +{ + if (param == PARAM_INVALID) return 1; + + /* buffers for param transmission */ + static char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN]; + float val_buf; + static mavlink_message_t tx_msg; + + /* query parameter type */ + param_type_t type = param_type(param); + /* copy parameter name */ + strncpy((char *)name_buf, param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + + /* + * Map onboard parameter type to MAVLink type, + * endianess matches (both little endian) + */ + uint8_t mavlink_type; + + if (type == PARAM_TYPE_INT32) { + mavlink_type = MAVLINK_TYPE_INT32_T; + + } else if (type == PARAM_TYPE_FLOAT) { + mavlink_type = MAVLINK_TYPE_FLOAT; + + } else { + mavlink_type = MAVLINK_TYPE_FLOAT; + } + + /* + * get param value, since MAVLink encodes float and int params in the same + * space during transmission, copy param onto float val_buf + */ + + int ret; + + if ((ret = param_get(param, &val_buf)) != OK) { + return ret; + } + + mavlink_msg_param_value_pack_chan(mavlink_system.sysid, + mavlink_system.compid, + MAVLINK_COMM_0, + &tx_msg, + name_buf, + val_buf, + mavlink_type, + param_count(), + param_get_index(param)); + ret = mavlink_missionlib_send_message(&tx_msg); + return ret; +} + +void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg) +{ + switch (msg->msgid) { + case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { + /* Start sending parameters */ + mavlink_pm_start_queued_send(); + mavlink_missionlib_send_gcs_string("[mavlink pm] sending list"); + } break; + + case MAVLINK_MSG_ID_PARAM_SET: { + + /* Handle parameter setting */ + + if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) { + mavlink_param_set_t mavlink_param_set; + mavlink_msg_param_set_decode(msg, &mavlink_param_set); + + if (mavlink_param_set.target_system == mavlink_system.sysid && ((mavlink_param_set.target_component == mavlink_system.compid) || (mavlink_param_set.target_component == MAV_COMP_ID_ALL))) { + /* local name buffer to enforce null-terminated string */ + char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; + strncpy(name, mavlink_param_set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + /* enforce null termination */ + name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; + /* attempt to find parameter, set and send it */ + param_t param = param_find(name); + + if (param == PARAM_INVALID) { + char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; + sprintf(buf, "[mavlink pm] unknown: %s", name); + mavlink_missionlib_send_gcs_string(buf); + + } else { + /* set and send parameter */ + param_set(param, &(mavlink_param_set.param_value)); + mavlink_pm_send_param(param); + } + } + } + } break; + + case MAVLINK_MSG_ID_PARAM_REQUEST_READ: { + mavlink_param_request_read_t mavlink_param_request_read; + mavlink_msg_param_request_read_decode(msg, &mavlink_param_request_read); + + if (mavlink_param_request_read.target_system == mavlink_system.sysid && ((mavlink_param_request_read.target_component == mavlink_system.compid) || (mavlink_param_request_read.target_component == MAV_COMP_ID_ALL))) { + /* when no index is given, loop through string ids and compare them */ + if (mavlink_param_request_read.param_index == -1) { + /* local name buffer to enforce null-terminated string */ + char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; + strncpy(name, mavlink_param_request_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + /* enforce null termination */ + name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; + /* attempt to find parameter and send it */ + mavlink_pm_send_param_for_name(name); + + } else { + /* when index is >= 0, send this parameter again */ + mavlink_pm_send_param_for_index(mavlink_param_request_read.param_index); + } + } + + } break; + } +} + +void publish_mission() +{ + /* Initialize mission publication if necessary */ + if (mission_pub < 0) { + mission_pub = orb_advertise(ORB_ID(mission), &mission); + + } else { + orb_publish(ORB_ID(mission), mission_pub, &mission); + } +} + +int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item) +{ + /* only support global waypoints for now */ + switch (mavlink_mission_item->frame) { + case MAV_FRAME_GLOBAL: + mission_item->lat = (double)mavlink_mission_item->x; + mission_item->lon = (double)mavlink_mission_item->y; + mission_item->altitude = mavlink_mission_item->z; + mission_item->altitude_is_relative = false; + break; + + case MAV_FRAME_GLOBAL_RELATIVE_ALT: + mission_item->lat = (double)mavlink_mission_item->x; + mission_item->lon = (double)mavlink_mission_item->y; + mission_item->altitude = mavlink_mission_item->z; + mission_item->altitude_is_relative = true; + break; + + case MAV_FRAME_LOCAL_NED: + case MAV_FRAME_LOCAL_ENU: + return MAV_MISSION_UNSUPPORTED_FRAME; + case MAV_FRAME_MISSION: + default: + return MAV_MISSION_ERROR; + } + + switch (mavlink_mission_item->command) { + case MAV_CMD_NAV_TAKEOFF: + mission_item->pitch_min = mavlink_mission_item->param2; + break; + default: + mission_item->acceptance_radius = mavlink_mission_item->param2; + break; + } + + mission_item->yaw = _wrap_pi(mavlink_mission_item->param4*M_DEG_TO_RAD_F); + mission_item->loiter_radius = fabsf(mavlink_mission_item->param3); + mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */ + mission_item->nav_cmd = mavlink_mission_item->command; + + mission_item->time_inside = mavlink_mission_item->param1; + mission_item->autocontinue = mavlink_mission_item->autocontinue; + // mission_item->index = mavlink_mission_item->seq; + mission_item->origin = ORIGIN_MAVLINK; + + return OK; +} + +int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item) +{ + if (mission_item->altitude_is_relative) { + mavlink_mission_item->frame = MAV_FRAME_GLOBAL; + } else { + mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; + } + + switch (mission_item->nav_cmd) { + case NAV_CMD_TAKEOFF: + mavlink_mission_item->param2 = mission_item->pitch_min; + break; + default: + mavlink_mission_item->param2 = mission_item->acceptance_radius; + break; + } + + mavlink_mission_item->x = (float)mission_item->lat; + mavlink_mission_item->y = (float)mission_item->lon; + mavlink_mission_item->z = mission_item->altitude; + + mavlink_mission_item->param4 = mission_item->yaw*M_RAD_TO_DEG_F; + mavlink_mission_item->param3 = mission_item->loiter_radius*(float)mission_item->loiter_direction; + mavlink_mission_item->command = mission_item->nav_cmd; + mavlink_mission_item->param1 = mission_item->time_inside; + mavlink_mission_item->autocontinue = mission_item->autocontinue; + // mavlink_mission_item->seq = mission_item->index; + + return OK; +} + +void mavlink_wpm_init(mavlink_wpm_storage *state) +{ + state->size = 0; + state->max_size = MAVLINK_WPM_MAX_WP_COUNT; + state->current_state = MAVLINK_WPM_STATE_IDLE; + state->current_partner_sysid = 0; + state->current_partner_compid = 0; + state->timestamp_lastaction = 0; + state->timestamp_last_send_setpoint = 0; + state->timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT; + state->current_dataman_id = 0; +} + +/* + * @brief Sends an waypoint ack message + */ +void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type) +{ + mavlink_message_t msg; + mavlink_mission_ack_t wpa; + + wpa.target_system = sysid; + wpa.target_component = compid; + wpa.type = type; + + mavlink_msg_mission_ack_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpa); + mavlink_missionlib_send_message(&msg); + + if (verbose) warnx("Sent waypoint ack (%u) to ID %u", wpa.type, wpa.target_system); +} + +/* + * @brief Broadcasts the new target waypoint and directs the MAV to fly there + * + * This function broadcasts its new active waypoint sequence number and + * sends a message to the controller, advising it to fly to the coordinates + * of the waypoint with a given orientation + * + * @param seq The waypoint sequence number the MAV should fly to. + */ +void mavlink_wpm_send_waypoint_current(uint16_t seq) +{ + if (seq < wpm->size) { + mavlink_message_t msg; + mavlink_mission_current_t wpc; + + wpc.seq = seq; + + mavlink_msg_mission_current_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpc); + mavlink_missionlib_send_message(&msg); + + if (verbose) warnx("Broadcasted new current waypoint %u", wpc.seq); + + } else { + mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds"); + if (verbose) warnx("ERROR: index out of bounds"); + } +} + +void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count) +{ + mavlink_message_t msg; + mavlink_mission_count_t wpc; + + wpc.target_system = sysid; + wpc.target_component = compid; + wpc.count = mission.count; + + mavlink_msg_mission_count_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpc); + mavlink_missionlib_send_message(&msg); + + if (verbose) warnx("Sent waypoint count (%u) to ID %u", wpc.count, wpc.target_system); +} + +void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq) +{ + + struct mission_item_s mission_item; + ssize_t len = sizeof(struct mission_item_s); + + dm_item_t dm_current; + + if (wpm->current_dataman_id == 0) { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + } else { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; + } + + if (dm_read(dm_current, seq, &mission_item, len) == len) { + + /* create mission_item_s from mavlink_mission_item_t */ + mavlink_mission_item_t wp; + map_mission_item_to_mavlink_mission_item(&mission_item, &wp); + + mavlink_message_t msg; + wp.target_system = sysid; + wp.target_component = compid; + wp.seq = seq; + mavlink_msg_mission_item_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wp); + mavlink_missionlib_send_message(&msg); + + if (verbose) warnx("Sent waypoint %u to ID %u", wp.seq, wp.target_system); + } else { + mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR); + if (verbose) warnx("ERROR: could not read WP%u", seq); + } +} + +void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq) +{ + if (seq < wpm->max_size) { + mavlink_message_t msg; + mavlink_mission_request_t wpr; + wpr.target_system = sysid; + wpr.target_component = compid; + wpr.seq = seq; + mavlink_msg_mission_request_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpr); + mavlink_missionlib_send_message(&msg); + + if (verbose) warnx("Sent waypoint request %u to ID %u", wpr.seq, wpr.target_system); + + } else { + mavlink_missionlib_send_gcs_string("ERROR: Waypoint index exceeds list capacity"); + if (verbose) warnx("ERROR: Waypoint index exceeds list capacity"); + } +} + +/* + * @brief emits a message that a waypoint reached + * + * This function broadcasts a message that a waypoint is reached. + * + * @param seq The waypoint sequence number the MAV has reached. + */ +void mavlink_wpm_send_waypoint_reached(uint16_t seq) +{ + mavlink_message_t msg; + mavlink_mission_item_reached_t wp_reached; + + wp_reached.seq = seq; + + mavlink_msg_mission_item_reached_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wp_reached); + mavlink_missionlib_send_message(&msg); + + if (verbose) warnx("Sent waypoint %u reached message", wp_reached.seq); +} + + +void mavlink_waypoint_eventloop(uint64_t now) +{ + /* check for timed-out operations */ + if (now - wpm->timestamp_lastaction > wpm->timeout && wpm->current_state != MAVLINK_WPM_STATE_IDLE) { + + mavlink_missionlib_send_gcs_string("Operation timeout"); + + if (verbose) warnx("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", wpm->current_state); + + wpm->current_state = MAVLINK_WPM_STATE_IDLE; + wpm->current_partner_sysid = 0; + wpm->current_partner_compid = 0; + } +} + + +void mavlink_wpm_message_handler(const mavlink_message_t *msg) +{ + uint64_t now = hrt_absolute_time(); + + switch (msg->msgid) { + + case MAVLINK_MSG_ID_MISSION_ACK: { + mavlink_mission_ack_t wpa; + mavlink_msg_mission_ack_decode(msg, &wpa); + + if ((msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid) && (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/)) { + wpm->timestamp_lastaction = now; + + if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { + if (wpm->current_wp_id == wpm->size - 1) { + + wpm->current_state = MAVLINK_WPM_STATE_IDLE; + wpm->current_wp_id = 0; + } + } + + } else { + mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch"); + if (verbose) warnx("REJ. WP CMD: curr partner id mismatch"); + } + + break; + } + + case MAVLINK_MSG_ID_MISSION_SET_CURRENT: { + 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*/) { + wpm->timestamp_lastaction = now; + + if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) { + if (wpc.seq < wpm->size) { + + mission.current_index = wpc.seq; + publish_mission(); + + mavlink_wpm_send_waypoint_current(wpc.seq); + + } else { + mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list"); + if (verbose) warnx("IGN WP CURR CMD: Not in list"); + } + + } else { + mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy"); + if (verbose) warnx("IGN WP CURR CMD: Busy"); + + } + + } else { + mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); + if (verbose) warnx("REJ. WP CMD: target id mismatch"); + } + + break; + } + + case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: { + 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*/) { + wpm->timestamp_lastaction = now; + + if (wpm->current_state == MAVLINK_WPM_STATE_IDLE || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { + if (wpm->size > 0) { + + wpm->current_state = MAVLINK_WPM_STATE_SENDLIST; + wpm->current_wp_id = 0; + wpm->current_partner_sysid = msg->sysid; + wpm->current_partner_compid = msg->compid; + + } else { + if (verbose) warnx("No waypoints send"); + } + + wpm->current_count = wpm->size; + mavlink_wpm_send_waypoint_count(msg->sysid, msg->compid, wpm->current_count); + + } else { + mavlink_missionlib_send_gcs_string("IGN REQUEST LIST: Busy"); + if (verbose) warnx("IGN REQUEST LIST: Busy"); + } + } else { + mavlink_missionlib_send_gcs_string("REJ. REQUEST LIST: target id mismatch"); + if (verbose) warnx("REJ. REQUEST LIST: target id mismatch"); + } + + break; + } + + case MAVLINK_MSG_ID_MISSION_REQUEST: { + mavlink_mission_request_t wpr; + mavlink_msg_mission_request_decode(msg, &wpr); + + if (msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid && wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) { + wpm->timestamp_lastaction = now; + + if (wpr.seq >= wpm->size) { + + mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list"); + if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.", wpr.seq); + break; + } + + /* + * Ensure that we are in the correct state and that the first request has id 0 + * and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint) + */ + if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { + + if (wpr.seq == 0) { + if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); + wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; + } else { + mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0"); + if (verbose) warnx("REJ. WP CMD: First id != 0"); + break; + } + + } else if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { + + if (wpr.seq == wpm->current_wp_id) { + + if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); + + } else if (wpr.seq == wpm->current_wp_id + 1) { + + if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); + + } else { + mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected"); + if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).", wpr.seq, wpm->current_wp_id, wpm->current_wp_id + 1); + break; + } + + } else { + + mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); + if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).", wpm->current_state); + break; + } + + wpm->current_wp_id = wpr.seq; + wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; + + if (wpr.seq < wpm->size) { + + mavlink_wpm_send_waypoint(wpm->current_partner_sysid, wpm->current_partner_compid,wpm->current_wp_id); + + } else { + mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR); + if (verbose) warnx("ERROR: Waypoint %u out of bounds", wpr.seq); + } + + + } else { + //we we're target but already communicating with someone else + if ((wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) && !(msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid)) { + + mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); + if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.", msg->sysid, wpm->current_partner_sysid); + + } else { + + mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); + if (verbose) warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); + } + } + break; + } + + case MAVLINK_MSG_ID_MISSION_COUNT: { + mavlink_mission_count_t wpc; + mavlink_msg_mission_count_decode(msg, &wpc); + + if (wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_wpm_comp_id*/) { + wpm->timestamp_lastaction = now; + + if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) { + + if (wpc.count > NUM_MISSIONS_SUPPORTED) { + if (verbose) warnx("Too many waypoints: %d, supported: %d", wpc.count, NUM_MISSIONS_SUPPORTED); + mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_NO_SPACE); + break; + } + + if (wpc.count == 0) { + mavlink_missionlib_send_gcs_string("COUNT 0"); + if (verbose) warnx("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE"); + break; + } + + if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid); + + wpm->current_state = MAVLINK_WPM_STATE_GETLIST; + wpm->current_wp_id = 0; + wpm->current_partner_sysid = msg->sysid; + wpm->current_partner_compid = msg->compid; + wpm->current_count = wpc.count; + + mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id); + + } else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST) { + + if (wpm->current_wp_id == 0) { + mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN"); + if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) again from %u", wpc.count, msg->sysid); + } else { + mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); + if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.", wpm->current_wp_id); + } + } else { + mavlink_missionlib_send_gcs_string("IGN MISSION_COUNT CMD: Busy"); + if (verbose) warnx("IGN MISSION_COUNT CMD: Busy"); + } + } else { + + mavlink_missionlib_send_gcs_string("REJ. WP COUNT CMD: target id mismatch"); + if (verbose) warnx("IGNORED WAYPOINT COUNT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); + } + } + break; + + case MAVLINK_MSG_ID_MISSION_ITEM: { + mavlink_mission_item_t wp; + mavlink_msg_mission_item_decode(msg, &wp); + + if (wp.target_system == mavlink_system.sysid && wp.target_component == mavlink_wpm_comp_id) { + + wpm->timestamp_lastaction = now; + + /* + * ensure that we are in the correct state and that the first waypoint has id 0 + * and the following waypoints have the correct ids + */ + + if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST) { + + if (wp.seq != 0) { + mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP not 0"); + warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.", wp.seq); + break; + } + } else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { + + if (wp.seq >= wpm->current_count) { + mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP out of bounds"); + warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was out of bounds.", wp.seq); + break; + } + + if (wp.seq != wpm->current_wp_id) { + warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was not the expected %u.", wp.seq, wpm->current_wp_id); + mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id); + break; + } + } + + wpm->current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS; + + struct mission_item_s mission_item; + + int ret = map_mavlink_mission_item_to_mission_item(&wp, &mission_item); + + if (ret != OK) { + mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, ret); + wpm->current_state = MAVLINK_WPM_STATE_IDLE; + break; + } + + ssize_t len = sizeof(struct mission_item_s); + + dm_item_t dm_next; + + if (wpm->current_dataman_id == 0) { + dm_next = DM_KEY_WAYPOINTS_OFFBOARD_1; + mission.dataman_id = 1; + } else { + dm_next = DM_KEY_WAYPOINTS_OFFBOARD_0; + mission.dataman_id = 0; + } + + if (dm_write(dm_next, wp.seq, DM_PERSIST_IN_FLIGHT_RESET, &mission_item, len) != len) { + mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR); + wpm->current_state = MAVLINK_WPM_STATE_IDLE; + break; + } + + if (wp.current) { + mission.current_index = wp.seq; + } + + wpm->current_wp_id = wp.seq + 1; + + if (wpm->current_wp_id == wpm->current_count && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { + + if (verbose) warnx("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE", wpm->current_count); + + mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED); + + mission.count = wpm->current_count; + + publish_mission(); + + wpm->current_dataman_id = mission.dataman_id; + wpm->size = wpm->current_count; + + wpm->current_state = MAVLINK_WPM_STATE_IDLE; + + } else { + mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id); + } + + } else { + mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); + if (verbose) warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); + } + + break; + } + + case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: { + 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 (wpm->current_state == MAVLINK_WPM_STATE_IDLE) { + wpm->timestamp_lastaction = now; + + wpm->size = 0; + + /* prepare mission topic */ + mission.dataman_id = -1; + mission.count = 0; + mission.current_index = -1; + publish_mission(); + + if (dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_0) == OK && dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_1) == OK) { + mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED); + } else { + mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR); + } + + + } else { + mavlink_missionlib_send_gcs_string("IGN WP CLEAR CMD: Busy"); + if (verbose) warnx("IGN WP CLEAR CMD: Busy"); + } + + + } else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm->current_state != MAVLINK_WPM_STATE_IDLE) { + + mavlink_missionlib_send_gcs_string("REJ. WP CLERR CMD: target id mismatch"); + if (verbose) warnx("IGNORED WAYPOINT CLEAR COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); + } + + break; + } + + default: { + /* other messages might should get caught by mavlink and others */ + break; + } + } +} + +void +mavlink_missionlib_send_message(mavlink_message_t *msg) +{ + uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg); + + mavlink_send_uart_bytes(chan, missionlib_msg_buf, len); +} + + + +int +mavlink_missionlib_send_gcs_string(const char *string) +{ + const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN; + mavlink_statustext_t statustext; + int i = 0; + + while (i < len - 1) { + statustext.text[i] = string[i]; + + if (string[i] == '\0') + break; + + i++; + } + + if (i > 1) { + /* Enforce null termination */ + statustext.text[i] = '\0'; + mavlink_message_t msg; + + mavlink_msg_statustext_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &statustext); + mavlink_missionlib_send_message(&msg); + return OK; + + } else { + return 1; + } +} + +void +Mavlink::task_main_trampoline(int argc, char *argv[]) +{ + mavlink::g_mavlink->task_main(); +} + +void +Mavlink::task_main() +{ + /* inform about start */ + warnx("Initializing.."); + fflush(stdout); + + /* initialize logging device */ + // YYY + + _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + + mavlink_log_info(_mavlink_fd, "[mavlink] started"); + + /* wakeup source(s) */ + struct pollfd fds[1]; + + /* Setup of loop */ + fds[0].fd = _params_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])), 100); + + /* timed out - periodic check for _task_should_exit, etc. */ + if (pret == 0) { + continue; + } + + /* this is undesirable but not much we can do - might want to flag unhappy status */ + if (pret < 0) { + warn("poll error %d, %d", pret, errno); + continue; + } + + perf_begin(_loop_perf); + + /* parameters updated */ + if (fds[0].revents & POLLIN) { + parameters_update(); + } + + + + + + /* initialize mavlink text message buffering */ + mavlink_logbuffer_init(&lb, 10); + + int ch; + char *device_name = "/dev/ttyS1"; + baudrate = 57600; + + /* work around some stupidity in task_create's argv handling */ + argc -= 2; + argv += 2; + + while ((ch = getopt(argc, argv, "b:d:eo")) != EOF) { + switch (ch) { + case 'b': + baudrate = strtoul(optarg, NULL, 10); + + if (baudrate < 9600 || baudrate > 921600) + errx(1, "invalid baud rate '%s'", optarg); + + break; + + case 'd': + device_name = optarg; + break; + + case 'e': + mavlink_link_termination_allowed = true; + break; + + case 'o': + mavlink_link_mode = MAVLINK_INTERFACE_MODE_ONBOARD; + break; + + default: + usage(); + break; + } + } + + struct termios uart_config_original; + + bool usb_uart; + + /* print welcome text */ + warnx("MAVLink v1.0 serial interface starting..."); + + /* inform about mode */ + warnx((mavlink_link_mode == MAVLINK_INTERFACE_MODE_ONBOARD) ? "ONBOARD MODE" : "DOWNLINK MODE"); + + /* Flush stdout in case MAVLink is about to take it over */ + fflush(stdout); + + /* default values for arguments */ + uart = mavlink_open_uart(baudrate, device_name, &uart_config_original, &usb_uart); + + if (uart < 0) + err(1, "could not open %s", device_name); + + /* create the device node that's used for sending text log messages, etc. */ + register_driver(MAVLINK_LOG_DEVICE, &mavlink_fops, 0666, NULL); + + /* Initialize system properties */ + mavlink_update_system(); + + /* start the MAVLink receiver */ + receive_thread = receive_start(uart); + + /* start the ORB receiver */ + uorb_receive_thread = uorb_receive_start(); + + /* initialize waypoint manager */ + mavlink_wpm_init(wpm); + + /* all subscriptions are now active, set up initial guess about rate limits */ + if (baudrate >= 230400) { + /* 200 Hz / 5 ms */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 20); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 20); + /* 50 Hz / 20 ms */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 30); + /* 20 Hz / 50 ms */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 10); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50); + /* 10 Hz */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 100); + /* 10 Hz */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 100); + + } else if (baudrate >= 115200) { + /* 20 Hz / 50 ms */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 50); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 50); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 50); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50); + /* 5 Hz / 200 ms */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200); + /* 5 Hz / 200 ms */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 200); + /* 2 Hz */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); + + } else if (baudrate >= 57600) { + /* 10 Hz / 100 ms */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 300); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 300); + /* 10 Hz / 100 ms ATTITUDE */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 200); + /* 5 Hz / 200 ms */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200); + /* 5 Hz / 200 ms */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 500); + /* 2 Hz */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); + /* 2 Hz */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 500); + + } else { + /* very low baud rate, limit to 1 Hz / 1000 ms */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 1000); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 1000); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 1000); + /* 1 Hz / 1000 ms */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 1000); + /* 0.5 Hz */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 2000); + /* 0.1 Hz */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 10000); + } + + int mission_result_sub = orb_subscribe(ORB_ID(mission_result)); + struct mission_result_s mission_result; + memset(&mission_result, 0, sizeof(mission_result)); + + thread_running = true; + + /* arm counter to go off immediately */ + unsigned lowspeed_counter = 10; + + while (!thread_should_exit) { + + /* 1 Hz */ + if (lowspeed_counter == 10) { + mavlink_update_system(); + + /* translate the current system state to mavlink state and mode */ + uint8_t mavlink_state = 0; + uint8_t mavlink_base_mode = 0; + uint32_t mavlink_custom_mode = 0; + get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); + + /* send heartbeat */ + mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_base_mode, mavlink_custom_mode, mavlink_state); + + /* switch HIL mode if required */ + if (v_status.hil_state == HIL_STATE_ON) + set_hil_on_off(true); + else if (v_status.hil_state == HIL_STATE_OFF) + set_hil_on_off(false); + + /* send status (values already copied in the section above) */ + mavlink_msg_sys_status_send(chan, + v_status.onboard_control_sensors_present, + v_status.onboard_control_sensors_enabled, + v_status.onboard_control_sensors_health, + v_status.load * 1000.0f, + v_status.battery_voltage * 1000.0f, + v_status.battery_current * 1000.0f, + v_status.battery_remaining, + v_status.drop_rate_comm, + v_status.errors_comm, + v_status.errors_count1, + v_status.errors_count2, + v_status.errors_count3, + v_status.errors_count4); + lowspeed_counter = 0; + } + + lowspeed_counter++; + + bool updated; + orb_check(mission_result_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result); + + if (mission_result.mission_reached) { + mavlink_wpm_send_waypoint_reached((uint16_t)mission_result.mission_index); + } + } + + mavlink_waypoint_eventloop(hrt_absolute_time()); + + /* sleep quarter the time */ + usleep(25000); + + /* check if waypoint has been reached against the last positions */ + mavlink_waypoint_eventloop(hrt_absolute_time()); + + /* sleep quarter the time */ + usleep(25000); + + /* send parameters at 20 Hz (if queued for sending) */ + mavlink_pm_queued_send(); + mavlink_waypoint_eventloop(hrt_absolute_time()); + + /* sleep quarter the time */ + usleep(25000); + + mavlink_waypoint_eventloop(hrt_absolute_time()); + + if (baudrate > 57600) { + mavlink_pm_queued_send(); + } + + /* sleep 10 ms */ + usleep(10000); + + /* send one string at 10 Hz */ + if (!mavlink_logbuffer_is_empty(&lb)) { + struct mavlink_logmessage msg; + int lb_ret = mavlink_logbuffer_read(&lb, &msg); + + if (lb_ret == OK) { + mavlink_missionlib_send_gcs_string(msg.text); + } + } + + /* sleep 15 ms */ + usleep(15000); + } + + /* wait for threads to complete */ + pthread_join(receive_thread, NULL); + pthread_join(uorb_receive_thread, NULL); + + /* Reset the UART flags to original state */ + tcsetattr(uart, TCSANOW, &uart_config_original); + + /* destroy log buffer */ + //mavlink_logbuffer_destroy(&lb); + + thread_running = false; + + return 0; + + + + + + + + perf_end(_loop_perf); + } + + warnx("exiting."); + + _mavlink_task = -1; + _exit(0); +} + +int +Mavlink::start() +{ + ASSERT(_mavlink_task == -1); + + /* start the task */ + char buf[32]; + sprintf(buf, "mavlink if%d", Mavlink::instance_count()); + + _mavlink_task = task_spawn_cmd(buf, + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2048, + (main_t)&Mavlink::task_main_trampoline, + nullptr); + + // while (!this->is_running()) { + // usleep(200); + // } + + if (_mavlink_task < 0) { + warn("task start failed"); + return -errno; + } + + return OK; +} + +void +Mavlink::status() +{ + warnx("Running"); +} + +static void usage() +{ + errx(1, "usage: mavlink {start|stop|status}"); +} + +int mavlink_main(int argc, char *argv[]) +{ + if (argc < 2) { + usage(); + } + + if (!strcmp(argv[1], "start")) { + + Mavlink *instance = Mavlink::new_instance("/dev/ttyS0", 57600); + + if (mavlink::g_mavlink == nullptr) + mavlink::g_mavlink = instance; + + // if (mavlink::g_mavlink != nullptr) { + // errx(1, "already running"); + // } + + // mavlink::g_mavlink = new Mavlink; + + // if (mavlink::g_mavlink == nullptr) { + // errx(1, "alloc failed"); + // } + + // if (OK != mavlink::g_mavlink->start()) { + // delete mavlink::g_mavlink; + // mavlink::g_mavlink = nullptr; + // err(1, "start failed"); + // } + + return 0; + } + + // if (mavlink::g_mavlink == nullptr) + // errx(1, "not running"); + + // if (!strcmp(argv[1], "stop")) { + // delete mavlink::g_mavlink; + // mavlink::g_mavlink = nullptr; + + // } else if (!strcmp(argv[1], "status")) { + // mavlink::g_mavlink->status(); + + // } else { + // usage(); + // } + + return 0; +} diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h new file mode 100644 index 0000000000..8d6f0bd6f7 --- /dev/null +++ b/src/modules/mavlink/mavlink_main.h @@ -0,0 +1,255 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file mavlink_main.h + * MAVLink 1.0 protocol interface definition. + * + * @author Lorenz Meier + */ + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class Mavlink +{ +public: + /** + * Constructor + */ + Mavlink(const char *port, unsigned baud_rate); + + /** + * Destructor, also kills the mavlinks task. + */ + ~Mavlink(); + + /** + * Start the mavlink task. + * + * @return OK on success. + */ + int start(); + + /** + * Display the mavlink status. + */ + void status(); + + static int instance_count(); + + static Mavlink* new_instance(const char *port, unsigned baud_rate); + + static Mavlink* get_instance(unsigned instance); + + struct mavlink_subscriptions { + int sensor_sub; + int att_sub; + int global_pos_sub; + int act_0_sub; + int act_1_sub; + int act_2_sub; + int act_3_sub; + int gps_sub; + int man_control_sp_sub; + int safety_sub; + int actuators_sub; + int armed_sub; + int actuators_effective_sub; + int local_pos_sub; + int spa_sub; + int spl_sub; + int triplet_sub; + int debug_key_value; + int input_rc_sub; + int optical_flow; + int rates_setpoint_sub; + int home_sub; + int airspeed_sub; + int navigation_capabilities_sub; + int control_mode_sub; + }; + + struct mavlink_subscriptions subs; + + /** Global position */ + struct vehicle_global_position_s global_pos; + /** Local position */ + struct vehicle_local_position_s local_pos; + /** navigation capabilities */ + struct navigation_capabilities_s nav_cap; + /** Vehicle status */ + struct vehicle_status_s v_status; + /** Vehicle control mode */ + struct vehicle_control_mode_s control_mode; + /** RC channels */ + struct rc_channels_s rc; + /** Actuator armed state */ + struct actuator_armed_s armed; + +private: + + bool _task_should_exit; /**< if true, sensor task should exit */ + int _mavlink_task; /**< task handle for sensor task */ + + int _mavlink_fd; + int _mavlink_incoming_fd; /**< file descriptor on which to receive incoming strings */ + + perf_counter_t _loop_perf; /**< loop performance counter */ + + /* states */ + bool _mavlink_hil_enabled; /**< Hardware in the loop mode */ + static Mavlink* _head; + + int _params_sub; + + orb_advert_t mission_pub = -1; +struct mission_s mission; + +uint8_t mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER; + + /** + * Update our local parameter cache. + */ + void parameters_update(); + + /** + * Enable / disable Hardware in the Loop simulation mode. + * + * @param hil_enabled The new HIL enable/disable state. + * @return OK if the HIL state changed, ERROR if the + * requested change could not be made or was + * redundant. + */ + int set_hil_on_off(bool hil_enabled); + + + /** + * Handle parameter related messages. + */ + void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg); + + /** + * Send all parameters at once. + * + * This function blocks until all parameters have been sent. + * it delays each parameter by the passed amount of microseconds. + * + * @param delay The delay in us between sending all parameters. + */ + void mavlink_pm_send_all_params(unsigned int delay); + + /** + * Send one parameter. + * + * @param param The parameter id to send. + * @return zero on success, nonzero on failure. + */ + int mavlink_pm_send_param(param_t param); + + /** + * Send one parameter identified by index. + * + * @param index The index of the parameter to send. + * @return zero on success, nonzero else. + */ + int mavlink_pm_send_param_for_index(uint16_t index); + + /** + * Send one parameter identified by name. + * + * @param name The index of the parameter to send. + * @return zero on success, nonzero else. + */ + int mavlink_pm_send_param_for_name(const char *name); + + /** + * Send a queue of parameters, one parameter per function call. + * + * @return zero on success, nonzero on failure + */ + int mavlink_pm_queued_send(void); + + /** + * Start sending the parameter queue. + * + * This function will not directly send parameters, but instead + * activate the sending of one parameter on each call of + * mavlink_pm_queued_send(). + * @see mavlink_pm_queued_send() + */ + void mavlink_pm_start_queued_send(void); + + /** + * Shim for calling task_main from task_create. + */ + void task_main_trampoline(int argc, char *argv[]); + + /** + * Main sensor collection task. + */ + void task_main() __attribute__((noreturn)); + +}; diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/mavlink_orb_listener.cpp similarity index 82% rename from src/modules/mavlink/orb_listener.c rename to src/modules/mavlink/mavlink_orb_listener.cpp index 6e177bc4d9..3edad0f16f 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/mavlink_orb_listener.cpp @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -33,7 +32,7 @@ ****************************************************************************/ /** - * @file orb_listener.c + * @file orb_listener.cpp * Monitors ORB topics and sends update messages as appropriate. * * @author Lorenz Meier @@ -58,44 +57,11 @@ #include -#include "waypoints.h" -#include "orb_topics.h" -#include "mavlink_hil.h" -#include "util.h" +#include "mavlink_orb_listener.h" +#include "mavlink_main.h" +//#include "util.h" -extern bool gcs_link; - -struct vehicle_global_position_s global_pos; -struct vehicle_local_position_s local_pos; -struct navigation_capabilities_s nav_cap; -struct vehicle_status_s v_status; -struct vehicle_control_mode_s control_mode; -struct rc_channels_s rc; -struct rc_input_values rc_raw; -struct actuator_armed_s armed; -struct actuator_controls_s actuators_0; -struct vehicle_attitude_s att; -struct airspeed_s airspeed; - -struct mavlink_subscriptions mavlink_subs; - -static int status_sub; -static int rc_sub; - -static unsigned int sensors_raw_counter; -static unsigned int attitude_counter; -static unsigned int gps_counter; - -/* - * Last sensor loop time - * some outputs are better timestamped - * with this "global" reference. - */ -static uint64_t last_sensor_timestamp; - -static hrt_abstime last_sent_vfr = 0; - -static void *uorb_receive_thread(void *arg); + void *uorb_receive_thread(void *arg); struct listener { void (*callback)(const struct listener *l); @@ -105,30 +71,28 @@ struct listener { uint16_t cm_uint16_from_m_float(float m); -static void l_sensor_combined(const struct listener *l); -static void l_vehicle_attitude(const struct listener *l); -static void l_vehicle_gps_position(const struct listener *l); -static void l_vehicle_status(const struct listener *l); -static void l_rc_channels(const struct listener *l); -static void l_input_rc(const struct listener *l); -static void l_global_position(const struct listener *l); -static void l_local_position(const struct listener *l); -static void l_global_position_setpoint(const struct listener *l); -static void l_local_position_setpoint(const struct listener *l); -static void l_attitude_setpoint(const struct listener *l); -static void l_actuator_outputs(const struct listener *l); -static void l_actuator_armed(const struct listener *l); -static void l_manual_control_setpoint(const struct listener *l); -static void l_vehicle_attitude_controls(const struct listener *l); -static void l_debug_key_value(const struct listener *l); -static void l_optical_flow(const struct listener *l); -static void l_vehicle_rates_setpoint(const struct listener *l); -static void l_home(const struct listener *l); -static void l_airspeed(const struct listener *l); -static void l_nav_cap(const struct listener *l); -static void l_control_mode(const struct listener *l); -static const struct listener listeners[] = { +uint16_t +cm_uint16_from_m_float(float m) +{ + if (m < 0.0f) { + return 0; + + } else if (m > 655.35f) { + return 65535; + } + + return (uint16_t)(m * 100.0f); +} + +MavlinkOrbListener::MavlinkOrbListener(Mavlink* parent) : + + _task_should_exit(false), + _loop_perf(perf_alloc(PC_ELAPSED, "mavlink orb")), + _mavlink(parent), + _n_listeners(0) +{ + static const struct listener listeners[] = { {l_sensor_combined, &mavlink_subs.sensor_sub, 0}, {l_vehicle_attitude, &mavlink_subs.att_sub, 0}, {l_vehicle_gps_position, &mavlink_subs.gps_sub, 0}, @@ -156,23 +120,12 @@ static const struct listener listeners[] = { {l_control_mode, &mavlink_subs.control_mode_sub, 0}, }; -static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]); + _n_listeners = sizeof(listeners) / sizeof(listeners[0]); -uint16_t -cm_uint16_from_m_float(float m) -{ - if (m < 0.0f) { - return 0; - - } else if (m > 655.35f) { - return 65535; - } - - return (uint16_t)(m * 100.0f); } void -l_sensor_combined(const struct listener *l) +MavlinkOrbListener::l_sensor_combined(const struct listener *l) { struct sensor_combined_s raw; @@ -213,7 +166,7 @@ l_sensor_combined(const struct listener *l) } if (gcs_link) - mavlink_msg_highres_imu_send(MAVLINK_COMM_0, last_sensor_timestamp, + mavlink_msg_highres_imu_send(_mavlink->get_mavlink_chan(), last_sensor_timestamp, raw.accelerometer_m_s2[0], raw.accelerometer_m_s2[1], raw.accelerometer_m_s2[2], raw.gyro_rad_s[0], raw.gyro_rad_s[1], raw.gyro_rad_s[2], @@ -227,14 +180,14 @@ l_sensor_combined(const struct listener *l) } void -l_vehicle_attitude(const struct listener *l) +MavlinkOrbListener::l_vehicle_attitude(const struct listener *l) { /* copy attitude data into local buffer */ orb_copy(ORB_ID(vehicle_attitude), mavlink_subs.att_sub, &att); if (gcs_link) { /* send sensor values */ - mavlink_msg_attitude_send(MAVLINK_COMM_0, + mavlink_msg_attitude_send(_mavlink->get_mavlink_chan(), last_sensor_timestamp / 1000, att.roll, att.pitch, @@ -250,12 +203,12 @@ l_vehicle_attitude(const struct listener *l) float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy); uint16_t heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F; float throttle = armed.armed ? actuators_0.control[3] * 100.0f : 0.0f; - mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vz); + mavlink_msg_vfr_hud_send(_mavlink->get_mavlink_chan(), airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vz); } /* send quaternion values if it exists */ if(att.q_valid) { - mavlink_msg_attitude_quaternion_send(MAVLINK_COMM_0, + mavlink_msg_attitude_quaternion_send(_mavlink->get_mavlink_chan(), last_sensor_timestamp / 1000, att.q[0], att.q[1], @@ -271,7 +224,7 @@ l_vehicle_attitude(const struct listener *l) } void -l_vehicle_gps_position(const struct listener *l) +MavlinkOrbListener::l_vehicle_gps_position(const struct listener *l) { struct vehicle_gps_position_s gps; @@ -282,7 +235,7 @@ l_vehicle_gps_position(const struct listener *l) float cog_deg = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F; /* GPS position */ - mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0, + mavlink_msg_gps_raw_int_send(_mavlink->get_mavlink_chan(), gps.timestamp_position, gps.fix_type, gps.lat, @@ -296,7 +249,7 @@ l_vehicle_gps_position(const struct listener *l) /* update SAT info every 10 seconds */ if (gps.satellite_info_available && (gps_counter % 50 == 0)) { - mavlink_msg_gps_status_send(MAVLINK_COMM_0, + mavlink_msg_gps_status_send(_mavlink->get_mavlink_chan(), gps.satellites_visible, gps.satellite_prn, gps.satellite_used, @@ -309,7 +262,7 @@ l_vehicle_gps_position(const struct listener *l) } void -l_vehicle_status(const struct listener *l) +MavlinkOrbListener::l_vehicle_status(const struct listener *l) { /* immediately communicate state changes back to user */ orb_copy(ORB_ID(vehicle_status), status_sub, &v_status); @@ -337,7 +290,7 @@ l_vehicle_status(const struct listener *l) } void -l_rc_channels(const struct listener *l) +MavlinkOrbListener::l_rc_channels(const struct listener *l) { /* copy rc channels into local buffer */ orb_copy(ORB_ID(rc_channels), rc_sub, &rc); @@ -345,7 +298,7 @@ l_rc_channels(const struct listener *l) } void -l_input_rc(const struct listener *l) +MavlinkOrbListener::l_input_rc(const struct listener *l) { /* copy rc channels into local buffer */ orb_copy(ORB_ID(input_rc), mavlink_subs.input_rc_sub, &rc_raw); @@ -373,12 +326,12 @@ l_input_rc(const struct listener *l) } void -l_global_position(const struct listener *l) +MavlinkOrbListener::l_global_position(const struct listener *l) { /* copy global position data into local buffer */ orb_copy(ORB_ID(vehicle_global_position), mavlink_subs.global_pos_sub, &global_pos); - mavlink_msg_global_position_int_send(MAVLINK_COMM_0, + mavlink_msg_global_position_int_send(_mavlink->get_mavlink_chan(), global_pos.timestamp / 1000, global_pos.lat, global_pos.lon, @@ -391,13 +344,13 @@ l_global_position(const struct listener *l) } void -l_local_position(const struct listener *l) +MavlinkOrbListener::l_local_position(const struct listener *l) { /* copy local position data into local buffer */ orb_copy(ORB_ID(vehicle_local_position), mavlink_subs.local_pos_sub, &local_pos); if (gcs_link) - mavlink_msg_local_position_ned_send(MAVLINK_COMM_0, + mavlink_msg_local_position_ned_send(_mavlink->get_mavlink_chan(), local_pos.timestamp / 1000, local_pos.x, local_pos.y, @@ -408,7 +361,7 @@ l_local_position(const struct listener *l) } void -l_global_position_setpoint(const struct listener *l) +MavlinkOrbListener::l_global_position_setpoint(const struct listener *l) { struct mission_item_triplet_s triplet; orb_copy(ORB_ID(mission_item_triplet), mavlink_subs.triplet_sub, &triplet); @@ -422,7 +375,7 @@ l_global_position_setpoint(const struct listener *l) coordinate_frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; if (gcs_link) - mavlink_msg_global_position_setpoint_int_send(MAVLINK_COMM_0, + mavlink_msg_global_position_setpoint_int_send(_mavlink->get_mavlink_chan(), coordinate_frame, (int32_t)(triplet.current.lat * 1e7d), (int32_t)(triplet.current.lon * 1e7d), @@ -431,7 +384,7 @@ l_global_position_setpoint(const struct listener *l) } void -l_local_position_setpoint(const struct listener *l) +MavlinkOrbListener::l_local_position_setpoint(const struct listener *l) { struct vehicle_local_position_setpoint_s local_sp; @@ -439,7 +392,7 @@ l_local_position_setpoint(const struct listener *l) orb_copy(ORB_ID(vehicle_local_position_setpoint), mavlink_subs.spl_sub, &local_sp); if (gcs_link) - mavlink_msg_local_position_setpoint_send(MAVLINK_COMM_0, + mavlink_msg_local_position_setpoint_send(_mavlink->get_mavlink_chan(), MAV_FRAME_LOCAL_NED, local_sp.x, local_sp.y, @@ -448,7 +401,7 @@ l_local_position_setpoint(const struct listener *l) } void -l_attitude_setpoint(const struct listener *l) +MavlinkOrbListener::l_attitude_setpoint(const struct listener *l) { struct vehicle_attitude_setpoint_s att_sp; @@ -456,7 +409,7 @@ l_attitude_setpoint(const struct listener *l) orb_copy(ORB_ID(vehicle_attitude_setpoint), mavlink_subs.spa_sub, &att_sp); if (gcs_link) - mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_0, + mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_mavlink->get_mavlink_chan(), att_sp.timestamp / 1000, att_sp.roll_body, att_sp.pitch_body, @@ -465,7 +418,7 @@ l_attitude_setpoint(const struct listener *l) } void -l_vehicle_rates_setpoint(const struct listener *l) +MavlinkOrbListener::l_vehicle_rates_setpoint(const struct listener *l) { struct vehicle_rates_setpoint_s rates_sp; @@ -473,7 +426,7 @@ l_vehicle_rates_setpoint(const struct listener *l) orb_copy(ORB_ID(vehicle_rates_setpoint), mavlink_subs.rates_setpoint_sub, &rates_sp); if (gcs_link) - mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(MAVLINK_COMM_0, + mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_mavlink->get_mavlink_chan(), rates_sp.timestamp / 1000, rates_sp.roll, rates_sp.pitch, @@ -482,7 +435,7 @@ l_vehicle_rates_setpoint(const struct listener *l) } void -l_actuator_outputs(const struct listener *l) +MavlinkOrbListener::l_actuator_outputs(const struct listener *l) { struct actuator_outputs_s act_outputs; @@ -497,7 +450,7 @@ l_actuator_outputs(const struct listener *l) orb_copy(ids[l->arg], *l->subp, &act_outputs); if (gcs_link) { - mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, + mavlink_msg_servo_output_raw_send(_mavlink->get_mavlink_chan(), last_sensor_timestamp / 1000, l->arg /* port number - needs GCS support */, /* QGC has port number support already */ act_outputs.output[0], @@ -583,13 +536,13 @@ l_actuator_outputs(const struct listener *l) } void -l_actuator_armed(const struct listener *l) +MavlinkOrbListener::l_actuator_armed(const struct listener *l) { orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed); } void -l_manual_control_setpoint(const struct listener *l) +MavlinkOrbListener::l_manual_control_setpoint(const struct listener *l) { struct manual_control_setpoint_s man_control; @@ -597,7 +550,7 @@ l_manual_control_setpoint(const struct listener *l) orb_copy(ORB_ID(manual_control_setpoint), mavlink_subs.man_control_sp_sub, &man_control); if (gcs_link) - mavlink_msg_manual_control_send(MAVLINK_COMM_0, + mavlink_msg_manual_control_send(_mavlink->get_mavlink_chan(), mavlink_system.sysid, man_control.roll * 1000, man_control.pitch * 1000, @@ -607,25 +560,25 @@ l_manual_control_setpoint(const struct listener *l) } void -l_vehicle_attitude_controls(const struct listener *l) +MavlinkOrbListener::l_vehicle_attitude_controls(const struct listener *l) { orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, mavlink_subs.actuators_sub, &actuators_0); if (gcs_link) { /* send, add spaces so that string buffer is at least 10 chars long */ - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + mavlink_msg_named_value_float_send(_mavlink->get_mavlink_chan(), last_sensor_timestamp / 1000, "ctrl0 ", actuators_0.control[0]); - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + mavlink_msg_named_value_float_send(_mavlink->get_mavlink_chan(), last_sensor_timestamp / 1000, "ctrl1 ", actuators_0.control[1]); - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + mavlink_msg_named_value_float_send(_mavlink->get_mavlink_chan(), last_sensor_timestamp / 1000, "ctrl2 ", actuators_0.control[2]); - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + mavlink_msg_named_value_float_send(_mavlink->get_mavlink_chan(), last_sensor_timestamp / 1000, "ctrl3 ", actuators_0.control[3]); @@ -633,7 +586,7 @@ l_vehicle_attitude_controls(const struct listener *l) } void -l_debug_key_value(const struct listener *l) +MavlinkOrbListener::l_debug_key_value(const struct listener *l) { struct debug_key_value_s debug; @@ -642,46 +595,46 @@ l_debug_key_value(const struct listener *l) /* Enforce null termination */ debug.key[sizeof(debug.key) - 1] = '\0'; - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + mavlink_msg_named_value_float_send(_mavlink->get_mavlink_chan(), last_sensor_timestamp / 1000, debug.key, debug.value); } void -l_optical_flow(const struct listener *l) +MavlinkOrbListener::l_optical_flow(const struct listener *l) { struct optical_flow_s flow; orb_copy(ORB_ID(optical_flow), mavlink_subs.optical_flow, &flow); - mavlink_msg_optical_flow_send(MAVLINK_COMM_0, flow.timestamp, flow.sensor_id, flow.flow_raw_x, flow.flow_raw_y, + mavlink_msg_optical_flow_send(_mavlink->get_mavlink_chan(), flow.timestamp, flow.sensor_id, flow.flow_raw_x, flow.flow_raw_y, flow.flow_comp_x_m, flow.flow_comp_y_m, flow.quality, flow.ground_distance_m); } void -l_home(const struct listener *l) +MavlinkOrbListener::l_home(const struct listener *l) { struct home_position_s home; orb_copy(ORB_ID(home_position), mavlink_subs.home_sub, &home); - mavlink_msg_gps_global_origin_send(MAVLINK_COMM_0, (int32_t)(home.lat*1e7d), (int32_t)(home.lon*1e7d), (int32_t)(home.altitude)*1e3f); + mavlink_msg_gps_global_origin_send(_mavlink->get_mavlink_chan(), (int32_t)(home.lat*1e7d), (int32_t)(home.lon*1e7d), (int32_t)(home.altitude)*1e3f); } void -l_airspeed(const struct listener *l) +MavlinkOrbListener::l_airspeed(const struct listener *l) { orb_copy(ORB_ID(airspeed), mavlink_subs.airspeed_sub, &airspeed); } void -l_nav_cap(const struct listener *l) +MavlinkOrbListener::l_nav_cap(const struct listener *l) { orb_copy(ORB_ID(navigation_capabilities), mavlink_subs.navigation_capabilities_sub, &nav_cap); - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + mavlink_msg_named_value_float_send(_mavlink->get_mavlink_chan(), hrt_absolute_time() / 1000, "turn dist", nav_cap.turn_distance); @@ -689,7 +642,7 @@ l_nav_cap(const struct listener *l) } void -l_control_mode(const struct listener *l) +MavlinkOrbListener::l_control_mode(const struct listener *l) { orb_copy(ORB_ID(vehicle_control_mode), mavlink_subs.control_mode_sub, &control_mode); @@ -725,9 +678,9 @@ uorb_receive_thread(void *arg) * * Might want to invoke each listener once to set initial state. */ - struct pollfd fds[n_listeners]; + struct pollfd fds[_n_listeners]; - for (unsigned i = 0; i < n_listeners; i++) { + for (unsigned i = 0; i < _n_listeners; i++) { fds[i].fd = *listeners[i].subp; fds[i].events = POLLIN; @@ -737,7 +690,7 @@ uorb_receive_thread(void *arg) while (!thread_should_exit) { - int poll_ret = poll(fds, n_listeners, timeout); + int poll_ret = poll(fds, _n_listeners, timeout); /* handle the poll result */ if (poll_ret == 0) { @@ -748,7 +701,7 @@ uorb_receive_thread(void *arg) } else { - for (unsigned i = 0; i < n_listeners; i++) { + for (unsigned i = 0; i < _n_listeners; i++) { if (fds[i].revents & POLLIN) listeners[i].callback(&listeners[i]); } diff --git a/src/modules/mavlink/mavlink_orb_listener.h b/src/modules/mavlink/mavlink_orb_listener.h new file mode 100644 index 0000000000..32a174fcd0 --- /dev/null +++ b/src/modules/mavlink/mavlink_orb_listener.h @@ -0,0 +1,143 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file mavlink_orb_listener.h + * MAVLink 1.0 protocol interface definition. + * + * @author Lorenz Meier + */ + +#pragma once + +class Mavlink; + +class MavlinkOrbListener +{ +public: + /** + * Constructor + */ + MavlinkOrbListener(Mavlink* parent); + + /** + * Destructor, also kills the mavlinks task. + */ + ~MavlinkOrbListener(); + + /** + * Start the mavlink task. + * + * @return OK on success. + */ + int start(); + + /** + * Display the mavlink status. + */ + void status(); + +private: + + bool _task_should_exit; /**< if true, sensor task should exit */ + + perf_counter_t _loop_perf; /**< loop performance counter */ + + Mavlink* _mavlink; + + unsigned _n_listeners; + + /** + * Shim for calling task_main from task_create. + */ + void task_main_trampoline(int argc, char *argv[]); + + /** + * Main sensor collection task. + */ + void task_main() __attribute__((noreturn)); + + void l_sensor_combined(const struct listener *l); + void l_vehicle_attitude(const struct listener *l); + void l_vehicle_gps_position(const struct listener *l); + void l_vehicle_status(const struct listener *l); + void l_rc_channels(const struct listener *l); + void l_input_rc(const struct listener *l); + void l_global_position(const struct listener *l); + void l_local_position(const struct listener *l); + void l_global_position_setpoint(const struct listener *l); + void l_local_position_setpoint(const struct listener *l); + void l_attitude_setpoint(const struct listener *l); + void l_actuator_outputs(const struct listener *l); + void l_actuator_armed(const struct listener *l); + void l_manual_control_setpoint(const struct listener *l); + void l_vehicle_attitude_controls(const struct listener *l); + void l_debug_key_value(const struct listener *l); + void l_optical_flow(const struct listener *l); + void l_vehicle_rates_setpoint(const struct listener *l); + void l_home(const struct listener *l); + void l_airspeed(const struct listener *l); + void l_nav_cap(const struct listener *l); + void l_control_mode(const struct listener *l); + + struct vehicle_global_position_s global_pos; + struct vehicle_local_position_s local_pos; + struct navigation_capabilities_s nav_cap; + struct vehicle_status_s v_status; + struct vehicle_control_mode_s control_mode; + struct rc_channels_s rc; + struct rc_input_values rc_raw; + struct actuator_armed_s armed; + struct actuator_controls_s actuators_0; + struct vehicle_attitude_s att; + struct airspeed_s airspeed; + + struct mavlink_subscriptions mavlink_subs; + + int status_sub; + int rc_sub; + + unsigned int sensors_raw_counter; + unsigned int attitude_counter; + unsigned int gps_counter; + + /* + * Last sensor loop time + * some outputs are better timestamped + * with this "global" reference. + */ + uint64_t last_sensor_timestamp; + + hrt_abstime last_sent_vfr = 0; + +}; diff --git a/src/modules/mavlink/mavlink_parameters.c b/src/modules/mavlink/mavlink_parameters.c deleted file mode 100644 index 18ca7a854b..0000000000 --- a/src/modules/mavlink/mavlink_parameters.c +++ /dev/null @@ -1,230 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file mavlink_parameters.c - * MAVLink parameter protocol implementation (BSD-relicensed). - * - * @author Lorenz Meier - */ - -#include "mavlink_bridge_header.h" -#include "mavlink_parameters.h" -#include -#include "math.h" /* isinf / isnan checks */ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -extern mavlink_system_t mavlink_system; - -extern int mavlink_missionlib_send_message(mavlink_message_t *msg); -extern int mavlink_missionlib_send_gcs_string(const char *string); - -/** - * If the queue index is not at 0, the queue sending - * logic will send parameters from the current index - * to len - 1, the end of the param list. - */ -static unsigned int mavlink_param_queue_index = 0; - -/** - * Callback for param interface. - */ -void mavlink_pm_callback(void *arg, param_t param); - -void mavlink_pm_callback(void *arg, param_t param) -{ - mavlink_pm_send_param(param); - usleep(*(unsigned int *)arg); -} - -void mavlink_pm_send_all_params(unsigned int delay) -{ - unsigned int dbuf = delay; - param_foreach(&mavlink_pm_callback, &dbuf, false); -} - -int mavlink_pm_queued_send() -{ - if (mavlink_param_queue_index < param_count()) { - mavlink_pm_send_param(param_for_index(mavlink_param_queue_index)); - mavlink_param_queue_index++; - return 0; - - } else { - return 1; - } -} - -void mavlink_pm_start_queued_send() -{ - mavlink_param_queue_index = 0; -} - -int mavlink_pm_send_param_for_index(uint16_t index) -{ - return mavlink_pm_send_param(param_for_index(index)); -} - -int mavlink_pm_send_param_for_name(const char *name) -{ - return mavlink_pm_send_param(param_find(name)); -} - -int mavlink_pm_send_param(param_t param) -{ - if (param == PARAM_INVALID) return 1; - - /* buffers for param transmission */ - static char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN]; - float val_buf; - static mavlink_message_t tx_msg; - - /* query parameter type */ - param_type_t type = param_type(param); - /* copy parameter name */ - strncpy((char *)name_buf, param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); - - /* - * Map onboard parameter type to MAVLink type, - * endianess matches (both little endian) - */ - uint8_t mavlink_type; - - if (type == PARAM_TYPE_INT32) { - mavlink_type = MAVLINK_TYPE_INT32_T; - - } else if (type == PARAM_TYPE_FLOAT) { - mavlink_type = MAVLINK_TYPE_FLOAT; - - } else { - mavlink_type = MAVLINK_TYPE_FLOAT; - } - - /* - * get param value, since MAVLink encodes float and int params in the same - * space during transmission, copy param onto float val_buf - */ - - int ret; - - if ((ret = param_get(param, &val_buf)) != OK) { - return ret; - } - - mavlink_msg_param_value_pack_chan(mavlink_system.sysid, - mavlink_system.compid, - MAVLINK_COMM_0, - &tx_msg, - name_buf, - val_buf, - mavlink_type, - param_count(), - param_get_index(param)); - ret = mavlink_missionlib_send_message(&tx_msg); - return ret; -} - -void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg) -{ - switch (msg->msgid) { - case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { - /* Start sending parameters */ - mavlink_pm_start_queued_send(); - mavlink_missionlib_send_gcs_string("[mavlink pm] sending list"); - } break; - - case MAVLINK_MSG_ID_PARAM_SET: { - - /* Handle parameter setting */ - - if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) { - mavlink_param_set_t mavlink_param_set; - mavlink_msg_param_set_decode(msg, &mavlink_param_set); - - if (mavlink_param_set.target_system == mavlink_system.sysid && ((mavlink_param_set.target_component == mavlink_system.compid) || (mavlink_param_set.target_component == MAV_COMP_ID_ALL))) { - /* local name buffer to enforce null-terminated string */ - char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; - strncpy(name, mavlink_param_set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); - /* enforce null termination */ - name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; - /* attempt to find parameter, set and send it */ - param_t param = param_find(name); - - if (param == PARAM_INVALID) { - char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; - sprintf(buf, "[mavlink pm] unknown: %s", name); - mavlink_missionlib_send_gcs_string(buf); - - } else { - /* set and send parameter */ - param_set(param, &(mavlink_param_set.param_value)); - mavlink_pm_send_param(param); - } - } - } - } break; - - case MAVLINK_MSG_ID_PARAM_REQUEST_READ: { - mavlink_param_request_read_t mavlink_param_request_read; - mavlink_msg_param_request_read_decode(msg, &mavlink_param_request_read); - - if (mavlink_param_request_read.target_system == mavlink_system.sysid && ((mavlink_param_request_read.target_component == mavlink_system.compid) || (mavlink_param_request_read.target_component == MAV_COMP_ID_ALL))) { - /* when no index is given, loop through string ids and compare them */ - if (mavlink_param_request_read.param_index == -1) { - /* local name buffer to enforce null-terminated string */ - char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; - strncpy(name, mavlink_param_request_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); - /* enforce null termination */ - name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; - /* attempt to find parameter and send it */ - mavlink_pm_send_param_for_name(name); - - } else { - /* when index is >= 0, send this parameter again */ - mavlink_pm_send_param_for_index(mavlink_param_request_read.param_index); - } - } - - } break; - } -} diff --git a/src/modules/mavlink/mavlink_parameters.h b/src/modules/mavlink/mavlink_parameters.h deleted file mode 100644 index b1e38bcc85..0000000000 --- a/src/modules/mavlink/mavlink_parameters.h +++ /dev/null @@ -1,104 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file mavlink_parameters.h - * MAVLink parameter protocol definitions (BSD-relicensed). - * - * @author Lorenz Meier - */ - -/* This assumes you have the mavlink headers on your include path - or in the same folder as this source file */ - - -#include -#include -#include - -/** - * Handle parameter related messages. - */ -void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg); - -/** - * Send all parameters at once. - * - * This function blocks until all parameters have been sent. - * it delays each parameter by the passed amount of microseconds. - * - * @param delay The delay in us between sending all parameters. - */ -void mavlink_pm_send_all_params(unsigned int delay); - -/** - * Send one parameter. - * - * @param param The parameter id to send. - * @return zero on success, nonzero on failure. - */ -int mavlink_pm_send_param(param_t param); - -/** - * Send one parameter identified by index. - * - * @param index The index of the parameter to send. - * @return zero on success, nonzero else. - */ -int mavlink_pm_send_param_for_index(uint16_t index); - -/** - * Send one parameter identified by name. - * - * @param name The index of the parameter to send. - * @return zero on success, nonzero else. - */ -int mavlink_pm_send_param_for_name(const char *name); - -/** - * Send a queue of parameters, one parameter per function call. - * - * @return zero on success, nonzero on failure - */ -int mavlink_pm_queued_send(void); - -/** - * Start sending the parameter queue. - * - * This function will not directly send parameters, but instead - * activate the sending of one parameter on each call of - * mavlink_pm_queued_send(). - * @see mavlink_pm_queued_send() - */ -void mavlink_pm_start_queued_send(void); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 771989430a..6881a22802 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -33,7 +32,7 @@ ****************************************************************************/ /** - * @file mavlink_receiver.c + * @file mavlink_receiver.cpp * MAVLink protocol message receive and dispatch * * @author Lorenz Meier @@ -79,12 +78,9 @@ __BEGIN_DECLS #include "mavlink_bridge_header.h" #include "waypoints.h" #include "orb_topics.h" -#include "mavlink_hil.h" #include "mavlink_parameters.h" #include "util.h" -extern bool gcs_link; - __END_DECLS /* XXX should be in a header somewhere */ @@ -272,7 +268,7 @@ handle_message(mavlink_message_t *msg) if (mavlink_system.sysid < 4) { /* switch to a receiving link mode */ - gcs_link = false; + _mavlink->set_mode(Mavlink::MODE_TX_HEARTBEAT_ONLY); /* * rate control mode - defined by MAVLink @@ -367,7 +363,7 @@ handle_message(mavlink_message_t *msg) * COMMAND_LONG message or a SET_MODE message */ - if (mavlink_hil_enabled) { + if (_mavlink->hil_enabled()) { uint64_t timestamp = hrt_absolute_time(); diff --git a/src/modules/mavlink/mavlink_hil.h b/src/modules/mavlink/mavlink_receiver.h similarity index 65% rename from src/modules/mavlink/mavlink_hil.h rename to src/modules/mavlink/mavlink_receiver.h index 744ed7d942..88ae4b1103 100644 --- a/src/modules/mavlink/mavlink_hil.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @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 @@ -33,20 +32,57 @@ ****************************************************************************/ /** - * @file mavlink_hil.h - * Hardware-in-the-loop simulation support. + * @file mavlink_orb_listener.h + * MAVLink 1.0 uORB listener definition + * + * @author Lorenz Meier */ #pragma once -extern bool mavlink_hil_enabled; +class Mavlink; -/** - * Enable / disable Hardware in the Loop simulation mode. - * - * @param hil_enabled The new HIL enable/disable state. - * @return OK if the HIL state changed, ERROR if the - * requested change could not be made or was - * redundant. - */ -extern int set_hil_on_off(bool hil_enabled); +class MavlinkReceiver +{ +public: + /** + * Constructor + */ + MavlinkReceiver(Mavlink *parent); + + /** + * Destructor, also kills the mavlinks task. + */ + ~MavlinkReceiver(); + + /** + * Start the mavlink task. + * + * @return OK on success. + */ + int start(); + + /** + * Display the mavlink status. + */ + void status(); + +private: + + bool _task_should_exit; /**< if true, sensor task should exit */ + + perf_counter_t _loop_perf; /**< loop performance counter */ + + Mavlink* _mavlink; + + /** + * Shim for calling task_main from task_create. + */ + void task_main_trampoline(int argc, char *argv[]); + + /** + * Main sensor collection task. + */ + void task_main() __attribute__((noreturn)); + +}; diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index 89a097c245..2a005565e0 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -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 @@ -36,10 +36,10 @@ # MODULE_COMMAND = mavlink -SRCS += mavlink.c \ - mavlink_parameters.c \ - mavlink_receiver.cpp \ - orb_listener.c \ - waypoints.c +SRCS += mavlink_main.cpp \ + mavlink.c \ + mavlink_receiver.cpp \ + mavlink_orb_listener.cpp \ + waypoints.cpp INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h deleted file mode 100644 index 4d428406ac..0000000000 --- a/src/modules/mavlink/orb_topics.h +++ /dev/null @@ -1,125 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file orb_topics.h - * Common sets of topics subscribed to or published by the MAVLink driver, - * and structures maintained by those subscriptions. - */ -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -struct mavlink_subscriptions { - int sensor_sub; - int att_sub; - int global_pos_sub; - int act_0_sub; - int act_1_sub; - int act_2_sub; - int act_3_sub; - int gps_sub; - int man_control_sp_sub; - int safety_sub; - int actuators_sub; - int armed_sub; - int actuators_effective_sub; - int local_pos_sub; - int spa_sub; - int spl_sub; - int triplet_sub; - int debug_key_value; - int input_rc_sub; - int optical_flow; - int rates_setpoint_sub; - int home_sub; - int airspeed_sub; - int navigation_capabilities_sub; - int control_mode_sub; -}; - -extern struct mavlink_subscriptions mavlink_subs; - -/** Global position */ -extern struct vehicle_global_position_s global_pos; - -/** Local position */ -extern struct vehicle_local_position_s local_pos; - -/** navigation capabilities */ -extern struct navigation_capabilities_s nav_cap; - -/** Vehicle status */ -extern struct vehicle_status_s v_status; - -/** Vehicle control mode */ -extern struct vehicle_control_mode_s control_mode; - -/** RC channels */ -extern struct rc_channels_s rc; - -/** Actuator armed state */ -extern struct actuator_armed_s armed; - -/** Worker thread starter */ -extern pthread_t uorb_receive_start(void); diff --git a/src/modules/mavlink/util.h b/src/modules/mavlink/util.h index 5e5ee8261c..5ca9a085d5 100644 --- a/src/modules/mavlink/util.h +++ b/src/modules/mavlink/util.h @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @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 diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c deleted file mode 100644 index 168666d4e1..0000000000 --- a/src/modules/mavlink/waypoints.c +++ /dev/null @@ -1,730 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. - * Author: @author Petri Tanskanen - * @author Lorenz Meier - * @author Thomas Gubler - * @author Julian Oes - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file waypoints.c - * MAVLink waypoint protocol implementation (BSD-relicensed). - */ - -#include -#include -#include -#include -#include "mavlink_bridge_header.h" -#include "waypoints.h" -#include "util.h" -#include -#include -#include -#include -#include -#include - -bool verbose = true; - -orb_advert_t mission_pub = -1; -struct mission_s mission; - -uint8_t mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER; - -void -mavlink_missionlib_send_message(mavlink_message_t *msg) -{ - uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg); - - mavlink_send_uart_bytes(chan, missionlib_msg_buf, len); -} - - - -int -mavlink_missionlib_send_gcs_string(const char *string) -{ - const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN; - mavlink_statustext_t statustext; - int i = 0; - - while (i < len - 1) { - statustext.text[i] = string[i]; - - if (string[i] == '\0') - break; - - i++; - } - - if (i > 1) { - /* Enforce null termination */ - statustext.text[i] = '\0'; - mavlink_message_t msg; - - mavlink_msg_statustext_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &statustext); - mavlink_missionlib_send_message(&msg); - return OK; - - } else { - return 1; - } -} - -void publish_mission() -{ - /* Initialize mission publication if necessary */ - if (mission_pub < 0) { - mission_pub = orb_advertise(ORB_ID(mission), &mission); - - } else { - orb_publish(ORB_ID(mission), mission_pub, &mission); - } -} - -int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item) -{ - /* only support global waypoints for now */ - switch (mavlink_mission_item->frame) { - case MAV_FRAME_GLOBAL: - mission_item->lat = (double)mavlink_mission_item->x; - mission_item->lon = (double)mavlink_mission_item->y; - mission_item->altitude = mavlink_mission_item->z; - mission_item->altitude_is_relative = false; - break; - - case MAV_FRAME_GLOBAL_RELATIVE_ALT: - mission_item->lat = (double)mavlink_mission_item->x; - mission_item->lon = (double)mavlink_mission_item->y; - mission_item->altitude = mavlink_mission_item->z; - mission_item->altitude_is_relative = true; - break; - - case MAV_FRAME_LOCAL_NED: - case MAV_FRAME_LOCAL_ENU: - return MAV_MISSION_UNSUPPORTED_FRAME; - case MAV_FRAME_MISSION: - default: - return MAV_MISSION_ERROR; - } - - switch (mavlink_mission_item->command) { - case MAV_CMD_NAV_TAKEOFF: - mission_item->pitch_min = mavlink_mission_item->param2; - break; - default: - mission_item->acceptance_radius = mavlink_mission_item->param2; - break; - } - - mission_item->yaw = _wrap_pi(mavlink_mission_item->param4*M_DEG_TO_RAD_F); - mission_item->loiter_radius = fabsf(mavlink_mission_item->param3); - mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */ - mission_item->nav_cmd = mavlink_mission_item->command; - - mission_item->time_inside = mavlink_mission_item->param1; - mission_item->autocontinue = mavlink_mission_item->autocontinue; - // mission_item->index = mavlink_mission_item->seq; - mission_item->origin = ORIGIN_MAVLINK; - - return OK; -} - -int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item) -{ - if (mission_item->altitude_is_relative) { - mavlink_mission_item->frame = MAV_FRAME_GLOBAL; - } else { - mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; - } - - switch (mission_item->nav_cmd) { - case NAV_CMD_TAKEOFF: - mavlink_mission_item->param2 = mission_item->pitch_min; - break; - default: - mavlink_mission_item->param2 = mission_item->acceptance_radius; - break; - } - - mavlink_mission_item->x = (float)mission_item->lat; - mavlink_mission_item->y = (float)mission_item->lon; - mavlink_mission_item->z = mission_item->altitude; - - mavlink_mission_item->param4 = mission_item->yaw*M_RAD_TO_DEG_F; - mavlink_mission_item->param3 = mission_item->loiter_radius*(float)mission_item->loiter_direction; - mavlink_mission_item->command = mission_item->nav_cmd; - mavlink_mission_item->param1 = mission_item->time_inside; - mavlink_mission_item->autocontinue = mission_item->autocontinue; - // mavlink_mission_item->seq = mission_item->index; - - return OK; -} - -void mavlink_wpm_init(mavlink_wpm_storage *state) -{ - state->size = 0; - state->max_size = MAVLINK_WPM_MAX_WP_COUNT; - state->current_state = MAVLINK_WPM_STATE_IDLE; - state->current_partner_sysid = 0; - state->current_partner_compid = 0; - state->timestamp_lastaction = 0; - state->timestamp_last_send_setpoint = 0; - state->timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT; - state->current_dataman_id = 0; -} - -/* - * @brief Sends an waypoint ack message - */ -void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type) -{ - mavlink_message_t msg; - mavlink_mission_ack_t wpa; - - wpa.target_system = sysid; - wpa.target_component = compid; - wpa.type = type; - - mavlink_msg_mission_ack_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpa); - mavlink_missionlib_send_message(&msg); - - if (verbose) warnx("Sent waypoint ack (%u) to ID %u", wpa.type, wpa.target_system); -} - -/* - * @brief Broadcasts the new target waypoint and directs the MAV to fly there - * - * This function broadcasts its new active waypoint sequence number and - * sends a message to the controller, advising it to fly to the coordinates - * of the waypoint with a given orientation - * - * @param seq The waypoint sequence number the MAV should fly to. - */ -void mavlink_wpm_send_waypoint_current(uint16_t seq) -{ - if (seq < wpm->size) { - mavlink_message_t msg; - mavlink_mission_current_t wpc; - - wpc.seq = seq; - - mavlink_msg_mission_current_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpc); - mavlink_missionlib_send_message(&msg); - - if (verbose) warnx("Broadcasted new current waypoint %u", wpc.seq); - - } else { - mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds"); - if (verbose) warnx("ERROR: index out of bounds"); - } -} - -void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count) -{ - mavlink_message_t msg; - mavlink_mission_count_t wpc; - - wpc.target_system = sysid; - wpc.target_component = compid; - wpc.count = mission.count; - - mavlink_msg_mission_count_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpc); - mavlink_missionlib_send_message(&msg); - - if (verbose) warnx("Sent waypoint count (%u) to ID %u", wpc.count, wpc.target_system); -} - -void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq) -{ - - struct mission_item_s mission_item; - ssize_t len = sizeof(struct mission_item_s); - - dm_item_t dm_current; - - if (wpm->current_dataman_id == 0) { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; - } else { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; - } - - if (dm_read(dm_current, seq, &mission_item, len) == len) { - - /* create mission_item_s from mavlink_mission_item_t */ - mavlink_mission_item_t wp; - map_mission_item_to_mavlink_mission_item(&mission_item, &wp); - - mavlink_message_t msg; - wp.target_system = sysid; - wp.target_component = compid; - wp.seq = seq; - mavlink_msg_mission_item_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wp); - mavlink_missionlib_send_message(&msg); - - if (verbose) warnx("Sent waypoint %u to ID %u", wp.seq, wp.target_system); - } else { - mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR); - if (verbose) warnx("ERROR: could not read WP%u", seq); - } -} - -void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq) -{ - if (seq < wpm->max_size) { - mavlink_message_t msg; - mavlink_mission_request_t wpr; - wpr.target_system = sysid; - wpr.target_component = compid; - wpr.seq = seq; - mavlink_msg_mission_request_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpr); - mavlink_missionlib_send_message(&msg); - - if (verbose) warnx("Sent waypoint request %u to ID %u", wpr.seq, wpr.target_system); - - } else { - mavlink_missionlib_send_gcs_string("ERROR: Waypoint index exceeds list capacity"); - if (verbose) warnx("ERROR: Waypoint index exceeds list capacity"); - } -} - -/* - * @brief emits a message that a waypoint reached - * - * This function broadcasts a message that a waypoint is reached. - * - * @param seq The waypoint sequence number the MAV has reached. - */ -void mavlink_wpm_send_waypoint_reached(uint16_t seq) -{ - mavlink_message_t msg; - mavlink_mission_item_reached_t wp_reached; - - wp_reached.seq = seq; - - mavlink_msg_mission_item_reached_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wp_reached); - mavlink_missionlib_send_message(&msg); - - if (verbose) warnx("Sent waypoint %u reached message", wp_reached.seq); -} - - -void mavlink_waypoint_eventloop(uint64_t now) -{ - /* check for timed-out operations */ - if (now - wpm->timestamp_lastaction > wpm->timeout && wpm->current_state != MAVLINK_WPM_STATE_IDLE) { - - mavlink_missionlib_send_gcs_string("Operation timeout"); - - if (verbose) warnx("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", wpm->current_state); - - wpm->current_state = MAVLINK_WPM_STATE_IDLE; - wpm->current_partner_sysid = 0; - wpm->current_partner_compid = 0; - } -} - - -void mavlink_wpm_message_handler(const mavlink_message_t *msg) -{ - uint64_t now = hrt_absolute_time(); - - switch (msg->msgid) { - - case MAVLINK_MSG_ID_MISSION_ACK: { - mavlink_mission_ack_t wpa; - mavlink_msg_mission_ack_decode(msg, &wpa); - - if ((msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid) && (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/)) { - wpm->timestamp_lastaction = now; - - if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { - if (wpm->current_wp_id == wpm->size - 1) { - - wpm->current_state = MAVLINK_WPM_STATE_IDLE; - wpm->current_wp_id = 0; - } - } - - } else { - mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch"); - if (verbose) warnx("REJ. WP CMD: curr partner id mismatch"); - } - - break; - } - - case MAVLINK_MSG_ID_MISSION_SET_CURRENT: { - 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*/) { - wpm->timestamp_lastaction = now; - - if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) { - if (wpc.seq < wpm->size) { - - mission.current_index = wpc.seq; - publish_mission(); - - mavlink_wpm_send_waypoint_current(wpc.seq); - - } else { - mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list"); - if (verbose) warnx("IGN WP CURR CMD: Not in list"); - } - - } else { - mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy"); - if (verbose) warnx("IGN WP CURR CMD: Busy"); - - } - - } else { - mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); - if (verbose) warnx("REJ. WP CMD: target id mismatch"); - } - - break; - } - - case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: { - 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*/) { - wpm->timestamp_lastaction = now; - - if (wpm->current_state == MAVLINK_WPM_STATE_IDLE || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { - if (wpm->size > 0) { - - wpm->current_state = MAVLINK_WPM_STATE_SENDLIST; - wpm->current_wp_id = 0; - wpm->current_partner_sysid = msg->sysid; - wpm->current_partner_compid = msg->compid; - - } else { - if (verbose) warnx("No waypoints send"); - } - - wpm->current_count = wpm->size; - mavlink_wpm_send_waypoint_count(msg->sysid, msg->compid, wpm->current_count); - - } else { - mavlink_missionlib_send_gcs_string("IGN REQUEST LIST: Busy"); - if (verbose) warnx("IGN REQUEST LIST: Busy"); - } - } else { - mavlink_missionlib_send_gcs_string("REJ. REQUEST LIST: target id mismatch"); - if (verbose) warnx("REJ. REQUEST LIST: target id mismatch"); - } - - break; - } - - case MAVLINK_MSG_ID_MISSION_REQUEST: { - mavlink_mission_request_t wpr; - mavlink_msg_mission_request_decode(msg, &wpr); - - if (msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid && wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) { - wpm->timestamp_lastaction = now; - - if (wpr.seq >= wpm->size) { - - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list"); - if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.", wpr.seq); - break; - } - - /* - * Ensure that we are in the correct state and that the first request has id 0 - * and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint) - */ - if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { - - if (wpr.seq == 0) { - if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); - wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; - } else { - mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0"); - if (verbose) warnx("REJ. WP CMD: First id != 0"); - break; - } - - } else if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { - - if (wpr.seq == wpm->current_wp_id) { - - if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); - - } else if (wpr.seq == wpm->current_wp_id + 1) { - - if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); - - } else { - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected"); - if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).", wpr.seq, wpm->current_wp_id, wpm->current_wp_id + 1); - break; - } - - } else { - - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); - if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).", wpm->current_state); - break; - } - - wpm->current_wp_id = wpr.seq; - wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; - - if (wpr.seq < wpm->size) { - - mavlink_wpm_send_waypoint(wpm->current_partner_sysid, wpm->current_partner_compid,wpm->current_wp_id); - - } else { - mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR); - if (verbose) warnx("ERROR: Waypoint %u out of bounds", wpr.seq); - } - - - } else { - //we we're target but already communicating with someone else - if ((wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) && !(msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid)) { - - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); - if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.", msg->sysid, wpm->current_partner_sysid); - - } else { - - mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); - if (verbose) warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); - } - } - break; - } - - case MAVLINK_MSG_ID_MISSION_COUNT: { - mavlink_mission_count_t wpc; - mavlink_msg_mission_count_decode(msg, &wpc); - - if (wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_wpm_comp_id*/) { - wpm->timestamp_lastaction = now; - - if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) { - - if (wpc.count > NUM_MISSIONS_SUPPORTED) { - if (verbose) warnx("Too many waypoints: %d, supported: %d", wpc.count, NUM_MISSIONS_SUPPORTED); - mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_NO_SPACE); - break; - } - - if (wpc.count == 0) { - mavlink_missionlib_send_gcs_string("COUNT 0"); - if (verbose) warnx("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE"); - break; - } - - if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid); - - wpm->current_state = MAVLINK_WPM_STATE_GETLIST; - wpm->current_wp_id = 0; - wpm->current_partner_sysid = msg->sysid; - wpm->current_partner_compid = msg->compid; - wpm->current_count = wpc.count; - - mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id); - - } else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST) { - - if (wpm->current_wp_id == 0) { - mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN"); - if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) again from %u", wpc.count, msg->sysid); - } else { - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); - if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.", wpm->current_wp_id); - } - } else { - mavlink_missionlib_send_gcs_string("IGN MISSION_COUNT CMD: Busy"); - if (verbose) warnx("IGN MISSION_COUNT CMD: Busy"); - } - } else { - - mavlink_missionlib_send_gcs_string("REJ. WP COUNT CMD: target id mismatch"); - if (verbose) warnx("IGNORED WAYPOINT COUNT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); - } - } - break; - - case MAVLINK_MSG_ID_MISSION_ITEM: { - mavlink_mission_item_t wp; - mavlink_msg_mission_item_decode(msg, &wp); - - if (wp.target_system == mavlink_system.sysid && wp.target_component == mavlink_wpm_comp_id) { - - wpm->timestamp_lastaction = now; - - /* - * ensure that we are in the correct state and that the first waypoint has id 0 - * and the following waypoints have the correct ids - */ - - if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST) { - - if (wp.seq != 0) { - mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP not 0"); - warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.", wp.seq); - break; - } - } else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { - - if (wp.seq >= wpm->current_count) { - mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP out of bounds"); - warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was out of bounds.", wp.seq); - break; - } - - if (wp.seq != wpm->current_wp_id) { - warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was not the expected %u.", wp.seq, wpm->current_wp_id); - mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id); - break; - } - } - - wpm->current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS; - - struct mission_item_s mission_item; - - int ret = map_mavlink_mission_item_to_mission_item(&wp, &mission_item); - - if (ret != OK) { - mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, ret); - wpm->current_state = MAVLINK_WPM_STATE_IDLE; - break; - } - - ssize_t len = sizeof(struct mission_item_s); - - dm_item_t dm_next; - - if (wpm->current_dataman_id == 0) { - dm_next = DM_KEY_WAYPOINTS_OFFBOARD_1; - mission.dataman_id = 1; - } else { - dm_next = DM_KEY_WAYPOINTS_OFFBOARD_0; - mission.dataman_id = 0; - } - - if (dm_write(dm_next, wp.seq, DM_PERSIST_IN_FLIGHT_RESET, &mission_item, len) != len) { - mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR); - wpm->current_state = MAVLINK_WPM_STATE_IDLE; - break; - } - - if (wp.current) { - mission.current_index = wp.seq; - } - - wpm->current_wp_id = wp.seq + 1; - - if (wpm->current_wp_id == wpm->current_count && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { - - if (verbose) warnx("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE", wpm->current_count); - - mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED); - - mission.count = wpm->current_count; - - publish_mission(); - - wpm->current_dataman_id = mission.dataman_id; - wpm->size = wpm->current_count; - - wpm->current_state = MAVLINK_WPM_STATE_IDLE; - - } else { - mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id); - } - - } else { - mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); - if (verbose) warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); - } - - break; - } - - case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: { - 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 (wpm->current_state == MAVLINK_WPM_STATE_IDLE) { - wpm->timestamp_lastaction = now; - - wpm->size = 0; - - /* prepare mission topic */ - mission.dataman_id = -1; - mission.count = 0; - mission.current_index = -1; - publish_mission(); - - if (dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_0) == OK && dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_1) == OK) { - mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED); - } else { - mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR); - } - - - } else { - mavlink_missionlib_send_gcs_string("IGN WP CLEAR CMD: Busy"); - if (verbose) warnx("IGN WP CLEAR CMD: Busy"); - } - - - } else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm->current_state != MAVLINK_WPM_STATE_IDLE) { - - mavlink_missionlib_send_gcs_string("REJ. WP CLERR CMD: target id mismatch"); - if (verbose) warnx("IGNORED WAYPOINT CLEAR COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); - } - - break; - } - - default: { - /* other messages might should get caught by mavlink and others */ - break; - } - } -} diff --git a/src/modules/mavlink/waypoints.h b/src/modules/mavlink/waypoints.h index f8b58c7d92..532eff7aa1 100644 --- a/src/modules/mavlink/waypoints.h +++ b/src/modules/mavlink/waypoints.h @@ -1,9 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * @author Thomas Gubler - * @author Julian Oes + * Copyright (c) 2009-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,11 @@ /** * @file waypoints.h * MAVLink waypoint protocol definition (BSD-relicensed). + * + * @author Petri Tanskanen + * @author Lorenz Meier + * @author Thomas Gubler + * @author Julian Oes */ #ifndef WAYPOINTS_H_ @@ -106,7 +108,4 @@ extern void mavlink_missionlib_current_waypoint_changed(uint16_t index, float pa static uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN]; -void mavlink_missionlib_send_message(mavlink_message_t *msg); -int mavlink_missionlib_send_gcs_string(const char *string); - #endif /* WAYPOINTS_H_ */ From e421260f7c3e2a3f7145c2f643f8440060a84777 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 12 Jan 2014 12:57:53 +0100 Subject: [PATCH 021/381] Removed bogus sensor counters, replaced them with proper timestamps --- .../attitude_estimator_ekf_main.cpp | 14 ++++----- .../attitude_estimator_so3_main.cpp | 13 ++++---- src/modules/mavlink/mavlink_receiver.cpp | 11 ++++--- src/modules/mavlink/orb_listener.c | 24 +++++++-------- .../position_estimator_inav_main.c | 16 +++++----- src/modules/sdlog2/sdlog2.c | 30 +++++++++---------- src/modules/sensors/sensors.cpp | 13 ++++---- src/modules/uORB/topics/sensor_combined.h | 17 +++++------ 8 files changed, 63 insertions(+), 75 deletions(-) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index a70a14fe4b..1741fab8b9 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -249,7 +249,6 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds // XXX write this out to perf regs /* keep track of sensor updates */ - uint32_t sensor_last_count[3] = {0, 0, 0}; uint64_t sensor_last_timestamp[3] = {0, 0, 0}; struct attitude_estimator_ekf_params ekf_params; @@ -333,9 +332,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds uint8_t update_vect[3] = {0, 0, 0}; /* Fill in gyro measurements */ - if (sensor_last_count[0] != raw.gyro_counter) { + if (sensor_last_timestamp[0] != raw.timestamp) { update_vect[0] = 1; - sensor_last_count[0] = raw.gyro_counter; sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]); sensor_last_timestamp[0] = raw.timestamp; } @@ -345,11 +343,10 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds z_k[2] = raw.gyro_rad_s[2] - gyro_offsets[2]; /* update accelerometer measurements */ - if (sensor_last_count[1] != raw.accelerometer_counter) { + if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) { update_vect[1] = 1; - sensor_last_count[1] = raw.accelerometer_counter; sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); - sensor_last_timestamp[1] = raw.timestamp; + sensor_last_timestamp[1] = raw.accelerometer_timestamp; } z_k[3] = raw.accelerometer_m_s2[0]; @@ -357,11 +354,10 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds z_k[5] = raw.accelerometer_m_s2[2]; /* update magnetometer measurements */ - if (sensor_last_count[2] != raw.magnetometer_counter) { + if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) { update_vect[2] = 1; - sensor_last_count[2] = raw.magnetometer_counter; sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); - sensor_last_timestamp[2] = raw.timestamp; + sensor_last_timestamp[2] = raw.magnetometer_timestamp; } z_k[6] = raw.magnetometer_ga[0]; diff --git a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp index e79726613d..e2d69cd615 100755 --- a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp +++ b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp @@ -526,9 +526,8 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[]) uint8_t update_vect[3] = {0, 0, 0}; /* Fill in gyro measurements */ - if (sensor_last_count[0] != raw.gyro_counter) { + if (sensor_last_timestamp[0] != raw.timestamp) { update_vect[0] = 1; - sensor_last_count[0] = raw.gyro_counter; sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]); sensor_last_timestamp[0] = raw.timestamp; } @@ -538,11 +537,10 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[]) gyro[2] = raw.gyro_rad_s[2] - gyro_offsets[2]; /* update accelerometer measurements */ - if (sensor_last_count[1] != raw.accelerometer_counter) { + if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) { update_vect[1] = 1; - sensor_last_count[1] = raw.accelerometer_counter; sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); - sensor_last_timestamp[1] = raw.timestamp; + sensor_last_timestamp[1] = raw.accelerometer_timestamp; } acc[0] = raw.accelerometer_m_s2[0]; @@ -550,11 +548,10 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[]) acc[2] = raw.accelerometer_m_s2[2]; /* update magnetometer measurements */ - if (sensor_last_count[2] != raw.magnetometer_counter) { + if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) { update_vect[2] = 1; - sensor_last_count[2] = raw.magnetometer_counter; sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); - sensor_last_timestamp[2] = raw.timestamp; + sensor_last_timestamp[2] = raw.magnetometer_timestamp; } mag[0] = raw.magnetometer_ga[0]; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 7b6fad6589..37e5b8c616 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -388,7 +388,6 @@ handle_message(mavlink_message_t *msg) hil_sensors.gyro_rad_s[0] = imu.xgyro; hil_sensors.gyro_rad_s[1] = imu.ygyro; hil_sensors.gyro_rad_s[2] = imu.zgyro; - hil_sensors.gyro_counter = hil_counter; /* accelerometer */ static const float mg2ms2 = 9.8f / 1000.0f; @@ -400,7 +399,7 @@ handle_message(mavlink_message_t *msg) hil_sensors.accelerometer_m_s2[2] = imu.zacc; hil_sensors.accelerometer_mode = 0; // TODO what is this? hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16 - hil_sensors.accelerometer_counter = hil_counter; + hil_sensors.accelerometer_timestamp = hil_sensors.timestamp; /* adc */ hil_sensors.adc_voltage_v[0] = 0.0f; @@ -418,17 +417,17 @@ handle_message(mavlink_message_t *msg) hil_sensors.magnetometer_range_ga = 32.7f; // int16 hil_sensors.magnetometer_mode = 0; // TODO what is this hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f; - hil_sensors.magnetometer_counter = hil_counter; + hil_sensors.magnetometer_timestamp = hil_sensors.timestamp; /* baro */ hil_sensors.baro_pres_mbar = imu.abs_pressure; hil_sensors.baro_alt_meter = imu.pressure_alt; hil_sensors.baro_temp_celcius = imu.temperature; - hil_sensors.baro_counter = hil_counter; + hil_sensors.baro_timestamp = hil_sensors.timestamp; /* differential pressure */ hil_sensors.differential_pressure_pa = imu.diff_pressure * 1e2f; //from hPa to Pa - hil_sensors.differential_pressure_counter = hil_counter; + hil_sensors.differential_pressure_timestamp = hil_sensors.timestamp; /* airspeed from differential pressure, ambient pressure and temp */ struct airspeed_s airspeed; @@ -829,7 +828,7 @@ receive_thread(void *arg) while (!thread_should_exit) { if (poll(fds, 1, timeout) > 0) { - if (nread < sizeof(buf)) { + if (nread < (ssize_t)sizeof(buf)) { /* to avoid reading very small chunks wait for data before reading */ usleep(1000); } diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 92b1b45be7..4a4ebf709d 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -181,33 +181,33 @@ l_sensor_combined(const struct listener *l) /* mark individual fields as changed */ uint16_t fields_updated = 0; - static unsigned accel_counter = 0; - static unsigned gyro_counter = 0; - static unsigned mag_counter = 0; - static unsigned baro_counter = 0; + static hrt_abstime accel_counter = 0; + static hrt_abstime gyro_counter = 0; + static hrt_abstime mag_counter = 0; + static hrt_abstime baro_counter = 0; - if (accel_counter != raw.accelerometer_counter) { + if (accel_counter != raw.accelerometer_timestamp) { /* mark first three dimensions as changed */ fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); - accel_counter = raw.accelerometer_counter; + accel_counter = raw.accelerometer_timestamp; } - if (gyro_counter != raw.gyro_counter) { + if (gyro_counter != raw.timestamp) { /* mark second group dimensions as changed */ fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); - gyro_counter = raw.gyro_counter; + gyro_counter = raw.timestamp; } - if (mag_counter != raw.magnetometer_counter) { + if (mag_counter != raw.magnetometer_timestamp) { /* mark third group dimensions as changed */ fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); - mag_counter = raw.magnetometer_counter; + mag_counter = raw.magnetometer_timestamp; } - if (baro_counter != raw.baro_counter) { + if (baro_counter != raw.baro_timestamp) { /* mark last group dimensions as changed */ fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); - baro_counter = raw.baro_counter; + baro_counter = raw.baro_timestamp; } if (gcs_link) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 3084b6d928..1270ff5cf1 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -177,8 +177,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) hrt_abstime landed_time = 0; bool flag_armed = false; - uint32_t accel_counter = 0; - uint32_t baro_counter = 0; + hrt_abstime accel_timestamp = 0; + hrt_abstime baro_timestamp = 0; /* declare and safely initialize all structs */ struct actuator_controls_s actuator; @@ -242,8 +242,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (fds_init[0].revents & POLLIN) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); - if (wait_baro && sensor.baro_counter != baro_counter) { - baro_counter = sensor.baro_counter; + if (wait_baro && sensor.baro_timestamp != baro_timestamp) { + baro_timestamp = sensor.baro_timestamp; /* mean calculation over several measurements */ if (baro_init_cnt < baro_init_num) { @@ -354,7 +354,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (fds[4].revents & POLLIN) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); - if (sensor.accelerometer_counter != accel_counter) { + if (sensor.accelerometer_timestamp != accel_timestamp) { if (att.R_valid) { /* correct accel bias, now only for Z */ sensor.accelerometer_m_s2[2] -= accel_bias[2]; @@ -376,13 +376,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) memset(accel_corr, 0, sizeof(accel_corr)); } - accel_counter = sensor.accelerometer_counter; + accel_timestamp = sensor.accelerometer_timestamp; accel_updates++; } - if (sensor.baro_counter != baro_counter) { + if (sensor.baro_timestamp != baro_timestamp) { baro_corr = - sensor.baro_alt_meter - z_est[0]; - baro_counter = sensor.baro_counter; + baro_timestamp = sensor.baro_timestamp; baro_updates++; } } diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index e94b1e13cb..1b91374629 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -903,11 +903,11 @@ int sdlog2_thread_main(int argc, char *argv[]) pthread_cond_init(&logbuffer_cond, NULL); /* track changes in sensor_combined topic */ - uint16_t gyro_counter = 0; - uint16_t accelerometer_counter = 0; - uint16_t magnetometer_counter = 0; - uint16_t baro_counter = 0; - uint16_t differential_pressure_counter = 0; + hrt_abstime gyro_timestamp = 0; + hrt_abstime accelerometer_timestamp = 0; + hrt_abstime magnetometer_timestamp = 0; + hrt_abstime barometer_timestamp = 0; + hrt_abstime differential_pressure_timestamp = 0; /* enable logging on start if needed */ if (log_on_start) @@ -1003,28 +1003,28 @@ int sdlog2_thread_main(int argc, char *argv[]) bool write_IMU = false; bool write_SENS = false; - if (buf.sensor.gyro_counter != gyro_counter) { - gyro_counter = buf.sensor.gyro_counter; + if (buf.sensor.timestamp != gyro_timestamp) { + gyro_timestamp = buf.sensor.timestamp; write_IMU = true; } - if (buf.sensor.accelerometer_counter != accelerometer_counter) { - accelerometer_counter = buf.sensor.accelerometer_counter; + if (buf.sensor.accelerometer_timestamp != accelerometer_timestamp) { + accelerometer_timestamp = buf.sensor.accelerometer_timestamp; write_IMU = true; } - if (buf.sensor.magnetometer_counter != magnetometer_counter) { - magnetometer_counter = buf.sensor.magnetometer_counter; + if (buf.sensor.magnetometer_timestamp != magnetometer_timestamp) { + magnetometer_timestamp = buf.sensor.magnetometer_timestamp; write_IMU = true; } - if (buf.sensor.baro_counter != baro_counter) { - baro_counter = buf.sensor.baro_counter; + if (buf.sensor.baro_timestamp != barometer_timestamp) { + barometer_timestamp = buf.sensor.baro_timestamp; write_SENS = true; } - if (buf.sensor.differential_pressure_counter != differential_pressure_counter) { - differential_pressure_counter = buf.sensor.differential_pressure_counter; + if (buf.sensor.differential_pressure_timestamp != differential_pressure_timestamp) { + differential_pressure_timestamp = buf.sensor.differential_pressure_timestamp; write_SENS = true; } diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index d9f037c271..a494cd08d6 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -931,7 +931,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) raw.accelerometer_raw[1] = accel_report.y_raw; raw.accelerometer_raw[2] = accel_report.z_raw; - raw.accelerometer_counter++; + raw.accelerometer_timestamp = accel_report.timestamp; } } @@ -957,7 +957,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) raw.gyro_raw[1] = gyro_report.y_raw; raw.gyro_raw[2] = gyro_report.z_raw; - raw.gyro_counter++; + raw.timestamp = gyro_report.timestamp; } } @@ -987,7 +987,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw) raw.magnetometer_raw[1] = mag_report.y_raw; raw.magnetometer_raw[2] = mag_report.z_raw; - raw.magnetometer_counter++; + raw.magnetometer_timestamp = mag_report.timestamp; } } @@ -1005,7 +1005,7 @@ Sensors::baro_poll(struct sensor_combined_s &raw) raw.baro_alt_meter = _barometer.altitude; // Altitude in meters raw.baro_temp_celcius = _barometer.temperature; // Temperature in degrees celcius - raw.baro_counter++; + raw.baro_timestamp = _barometer.timestamp; } } @@ -1019,7 +1019,7 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &_diff_pres); raw.differential_pressure_pa = _diff_pres.differential_pressure_pa; - raw.differential_pressure_counter++; + raw.differential_pressure_timestamp = _diff_pres.timestamp; _airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_pa); _airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_pa + raw.baro_pres_mbar * 1e2f, @@ -1560,9 +1560,6 @@ Sensors::task_main() /* check parameters for updates */ parameter_update_poll(); - /* store the time closest to all measurements (this is bogus, sensor timestamps should be propagated...) */ - raw.timestamp = hrt_absolute_time(); - /* copy most recent sensor data */ gyro_poll(raw); accel_poll(raw); diff --git a/src/modules/uORB/topics/sensor_combined.h b/src/modules/uORB/topics/sensor_combined.h index ad164555e2..92812efd49 100644 --- a/src/modules/uORB/topics/sensor_combined.h +++ b/src/modules/uORB/topics/sensor_combined.h @@ -77,34 +77,33 @@ struct sensor_combined_s { /* NOTE: Ordering of fields optimized to align to 32 bit / 4 bytes Change with consideration only */ - uint64_t timestamp; /**< Timestamp in microseconds since boot */ + uint64_t timestamp; /**< Timestamp in microseconds since boot, from gyro */ int16_t gyro_raw[3]; /**< Raw sensor values of angular velocity */ - uint16_t gyro_counter; /**< Number of raw measurments taken */ float gyro_rad_s[3]; /**< Angular velocity in radian per seconds */ - + int16_t accelerometer_raw[3]; /**< Raw acceleration in NED body frame */ - uint32_t accelerometer_counter; /**< Number of raw acc measurements taken */ float accelerometer_m_s2[3]; /**< Acceleration in NED body frame, in m/s^2 */ int accelerometer_mode; /**< Accelerometer measurement mode */ float accelerometer_range_m_s2; /**< Accelerometer measurement range in m/s^2 */ + uint64_t accelerometer_timestamp; /**< Accelerometer timestamp */ int16_t magnetometer_raw[3]; /**< Raw magnetic field in NED body frame */ float magnetometer_ga[3]; /**< Magnetic field in NED body frame, in Gauss */ int magnetometer_mode; /**< Magnetometer measurement mode */ float magnetometer_range_ga; /**< ± measurement range in Gauss */ float magnetometer_cuttoff_freq_hz; /**< Internal analog low pass frequency of sensor */ - uint32_t magnetometer_counter; /**< Number of raw mag measurements taken */ - + uint64_t magnetometer_timestamp; /**< Magnetometer timestamp */ + float baro_pres_mbar; /**< Barometric pressure, already temp. comp. */ float baro_alt_meter; /**< Altitude, already temp. comp. */ float baro_temp_celcius; /**< Temperature in degrees celsius */ float adc_voltage_v[4]; /**< ADC voltages of ADC Chan 10/11/12/13 or -1 */ float mcu_temp_celcius; /**< Internal temperature measurement of MCU */ - uint32_t baro_counter; /**< Number of raw baro measurements taken */ + uint64_t baro_timestamp; /**< Barometer timestamp */ - float differential_pressure_pa; /**< Airspeed sensor differential pressure */ - uint32_t differential_pressure_counter; /**< Number of raw differential pressure measurements taken */ + float differential_pressure_pa; /**< Airspeed sensor differential pressure */ + uint64_t differential_pressure_timestamp; /**< Last measurement timestamp */ }; /** From cd05dfcc7cb557575e62f5f93ddd3a86c073d517 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 12 Jan 2014 12:58:17 +0100 Subject: [PATCH 022/381] Moved to sensor_combined topic for estimator --- .../fw_att_pos_estimator_main.cpp | 150 +++++++++++++++--- 1 file changed, 130 insertions(+), 20 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index e6747604be..466f6d51f4 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -50,9 +50,17 @@ #include #include #include + +#define SENSOR_COMBINED_SUB + + #include #include #include +#include +#ifdef SENSOR_COMBINED_SUB +#include +#endif #include #include #include @@ -60,14 +68,9 @@ #include #include #include -#include -#include #include -#include -#include #include #include -#include #include #include #include @@ -77,6 +80,8 @@ #include "../../../InertialNav/code/estimator.h" + + /** * estimator app start / stop handling function * @@ -118,12 +123,15 @@ private: bool _task_should_exit; /**< if true, sensor task should exit */ int _estimator_task; /**< task handle for sensor task */ - +#ifndef SENSOR_COMBINED_SUB int _gyro_sub; /**< gyro sensor subscription */ int _accel_sub; /**< accel sensor subscription */ int _mag_sub; /**< mag sensor subscription */ - int _attitude_sub; /**< raw rc channels data subscription */ + #else + int _sensor_combined_sub; + #endif int _airspeed_sub; /**< airspeed subscription */ + int _baro_sub; /**< barometer subscription */ int _gps_sub; /**< GPS subscription */ int _vstatus_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ @@ -135,12 +143,11 @@ private: orb_advert_t _local_pos_pub; /**< position in local frame */ struct vehicle_attitude_s _att; /**< vehicle attitude */ - struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ - struct manual_control_setpoint_s _manual; /**< r/c channel data */ struct gyro_report _gyro; struct accel_report _accel; struct mag_report _mag; struct airspeed_s _airspeed; /**< airspeed */ + struct baro_report _baro; /**< baro readings */ struct vehicle_status_s _vstatus; /**< vehicle status */ struct vehicle_global_position_s _global_pos; /**< global vehicle position */ struct vehicle_local_position_s _local_pos; /**< local vehicle position */ @@ -150,6 +157,10 @@ private: struct accel_scale _accel_offsets; struct mag_scale _mag_offsets; +#ifdef SENSOR_COMBINED_SUB + struct sensor_combined_s _sensor_combined; +#endif + perf_counter_t _loop_perf; /**< loop performance counter */ perf_counter_t _perf_gyro; /// 1.0f) + deltaT = 0.01f; + + // Always store data, independent of init status + /* fill in last data set */ + dtIMU = deltaT; + + angRate.x = _sensor_combined.gyro_rad_s[0]; + angRate.y = _sensor_combined.gyro_rad_s[1]; + angRate.z = _sensor_combined.gyro_rad_s[2]; + + accel.x = _sensor_combined.accelerometer_m_s2[0]; + accel.y = _sensor_combined.accelerometer_m_s2[1]; + accel.z = _sensor_combined.accelerometer_m_s2[2]; + + dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU; + lastAngRate = angRate; + dVelIMU = 0.5f * (accel + lastAccel) * dtIMU; + lastAccel = accel; + + if (last_mag != _sensor_combined.magnetometer_timestamp) { + mag_updated = true; + } + last_mag = _sensor_combined.magnetometer_timestamp; + + #endif + if (_initialized) { /* predict states and covariances */ @@ -490,6 +572,14 @@ FixedwingEstimator::task_main() } } + bool baro_updated; + orb_check(_baro_sub, &baro_updated); + if (baro_updated) { + orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); + + /* XXX leverage baro */ + } + bool gps_updated; orb_check(_gps_sub, &gps_updated); if (gps_updated) { @@ -554,12 +644,16 @@ FixedwingEstimator::task_main() fuseHgtData = false; } - bool mag_updated; + #ifndef SENSOR_COMBINED_SUB orb_check(_mag_sub, &mag_updated); + #endif if (mag_updated) { - orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag); + perf_count(_perf_mag); + #ifndef SENSOR_COMBINED_SUB + orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag); + // XXX we compensate the offsets upfront - should be close to zero. // XXX the purpose of the 0.001 factor is unclear // 0.001f @@ -571,6 +665,22 @@ FixedwingEstimator::task_main() magData.z = 0.001f * _mag.z; magBias.z = 0.0f; // _mag_offsets.y_offset + + #else + + // XXX we compensate the offsets upfront - should be close to zero. + // XXX the purpose of the 0.001 factor is unclear + // 0.001f + magData.x = 0.001f * _sensor_combined.magnetometer_ga[0]; + magBias.x = 0.0f; // _mag_offsets.x_offset + + magData.y = 0.001f * _sensor_combined.magnetometer_ga[1]; + magBias.y = 0.0f; // _mag_offsets.y_offset + + magData.z = 0.001f * _sensor_combined.magnetometer_ga[2]; + magBias.z = 0.0f; // _mag_offsets.y_offset + + #endif if (_initialized) { From bc64391c538d85e6a1640dac0a76fe0f71a8efc4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 12 Jan 2014 12:59:15 +0100 Subject: [PATCH 023/381] AUTOMATED code-style fix on topics. NO MANUAL OR SEMANTIC EDITS --- src/modules/uORB/topics/esc_status.h | 9 +- .../uORB/topics/filtered_bottom_flow.h | 3 +- src/modules/uORB/topics/home_position.h | 5 +- src/modules/uORB/topics/mission.h | 6 +- .../uORB/topics/navigation_capabilities.h | 2 +- .../uORB/topics/offboard_control_setpoint.h | 5 +- src/modules/uORB/topics/rc_channels.h | 55 ++++++----- src/modules/uORB/topics/subsystem_info.h | 3 +- src/modules/uORB/topics/telemetry_status.h | 22 ++--- .../uORB/topics/vehicle_attitude_setpoint.h | 3 +- .../topics/vehicle_bodyframe_speed_setpoint.h | 3 +- src/modules/uORB/topics/vehicle_command.h | 91 +++++++++---------- .../uORB/topics/vehicle_control_debug.h | 9 +- .../uORB/topics/vehicle_control_mode.h | 5 +- .../uORB/topics/vehicle_global_position.h | 19 ++-- .../vehicle_global_position_set_triplet.h | 3 +- .../topics/vehicle_global_position_setpoint.h | 3 +- .../topics/vehicle_global_velocity_setpoint.h | 3 +- .../uORB/topics/vehicle_gps_position.h | 5 +- .../uORB/topics/vehicle_local_position.h | 3 +- .../topics/vehicle_local_position_setpoint.h | 3 +- .../uORB/topics/vehicle_rates_setpoint.h | 9 +- src/modules/uORB/topics/vehicle_status.h | 49 +++++----- .../uORB/topics/vehicle_vicon_position.h | 3 +- 24 files changed, 148 insertions(+), 173 deletions(-) diff --git a/src/modules/uORB/topics/esc_status.h b/src/modules/uORB/topics/esc_status.h index 11332d7a7a..628824efa8 100644 --- a/src/modules/uORB/topics/esc_status.h +++ b/src/modules/uORB/topics/esc_status.h @@ -60,7 +60,7 @@ enum ESC_VENDOR { ESC_VENDOR_GENERIC = 0, /**< generic ESC */ ESC_VENDOR_MIKROKOPTER, /**< Mikrokopter */ - ESC_VENDOR_GRAUPNER_HOTT /**< Graupner HoTT ESC */ + ESC_VENDOR_GRAUPNER_HOTT /**< Graupner HoTT ESC */ }; enum ESC_CONNECTION_TYPE { @@ -79,16 +79,15 @@ enum ESC_CONNECTION_TYPE { /** * Electronic speed controller status. */ -struct esc_status_s -{ +struct esc_status_s { /* use of a counter and timestamp recommended (but not necessary) */ uint16_t counter; /**< incremented by the writing thread everytime new data is stored */ uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ - + uint8_t esc_count; /**< number of connected ESCs */ 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 */ diff --git a/src/modules/uORB/topics/filtered_bottom_flow.h b/src/modules/uORB/topics/filtered_bottom_flow.h index ab6de26131..c5d5455422 100644 --- a/src/modules/uORB/topics/filtered_bottom_flow.h +++ b/src/modules/uORB/topics/filtered_bottom_flow.h @@ -53,8 +53,7 @@ /** * Filtered bottom flow in bodyframe. */ -struct filtered_bottom_flow_s -{ +struct filtered_bottom_flow_s { uint64_t timestamp; /**< time of this estimate, in microseconds since system start */ float sumx; /**< Integrated bodyframe x flow in meters */ diff --git a/src/modules/uORB/topics/home_position.h b/src/modules/uORB/topics/home_position.h index 7e1b82a0fb..09f5140b37 100644 --- a/src/modules/uORB/topics/home_position.h +++ b/src/modules/uORB/topics/home_position.h @@ -53,11 +53,10 @@ /** * GPS home position in WGS84 coordinates. */ -struct home_position_s -{ +struct home_position_s { uint64_t timestamp; /**< Timestamp (microseconds since system boot) */ uint64_t time_gps_usec; /**< Timestamp (microseconds in GPS format), this is the timestamp from the gps module */ - + int32_t lat; /**< Latitude in 1E7 degrees */ int32_t lon; /**< Longitude in 1E7 degrees */ int32_t alt; /**< Altitude in 1E3 meters (millimeters) above MSL */ diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index 978a3383a5..04cbf73e11 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -67,8 +67,7 @@ enum NAV_CMD { * This is the position the MAV is heading towards. If it of type loiter, * the MAV is circling around it with the given loiter radius in meters. */ -struct mission_item_s -{ +struct mission_item_s { bool altitude_is_relative; /**< true if altitude is relative from start point */ double lat; /**< latitude in degrees * 1E7 */ double lon; /**< longitude in degrees * 1E7 */ @@ -83,8 +82,7 @@ struct mission_item_s float param4; }; -struct mission_s -{ +struct mission_s { struct mission_item_s *items; unsigned count; }; diff --git a/src/modules/uORB/topics/navigation_capabilities.h b/src/modules/uORB/topics/navigation_capabilities.h index 6a3e811e3f..3a367b8cf6 100644 --- a/src/modules/uORB/topics/navigation_capabilities.h +++ b/src/modules/uORB/topics/navigation_capabilities.h @@ -52,7 +52,7 @@ * Airspeed */ struct navigation_capabilities_s { - float turn_distance; /**< the optimal distance to a waypoint to switch to the next */ + float turn_distance; /**< the optimal distance to a waypoint to switch to the next */ }; /** diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h index 7901b930af..68d3e494be 100644 --- a/src/modules/uORB/topics/offboard_control_setpoint.h +++ b/src/modules/uORB/topics/offboard_control_setpoint.h @@ -45,12 +45,11 @@ /** * Off-board control inputs. - * + * * Typically sent by a ground control station / joystick or by * some off-board controller via C or SIMULINK. */ -enum OFFBOARD_CONTROL_MODE -{ +enum OFFBOARD_CONTROL_MODE { OFFBOARD_CONTROL_MODE_DIRECT = 0, OFFBOARD_CONTROL_MODE_DIRECT_RATES = 1, OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE = 2, diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index 086b2ef150..6eb20efd1b 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -52,29 +52,28 @@ */ #define RC_CHANNELS_MAPPED_MAX 15 -/** +/** * This defines the mapping of the RC functions. * The value assigned to the specific function corresponds to the entry of * the channel array chan[]. */ -enum RC_CHANNELS_FUNCTION -{ - THROTTLE = 0, - ROLL = 1, - PITCH = 2, - YAW = 3, - MODE = 4, - RETURN = 5, - ASSISTED = 6, - MISSION = 7, - OFFBOARD_MODE = 8, - FLAPS = 9, - AUX_1 = 10, - AUX_2 = 11, - AUX_3 = 12, - AUX_4 = 13, - AUX_5 = 14, - RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */ +enum RC_CHANNELS_FUNCTION { + THROTTLE = 0, + ROLL = 1, + PITCH = 2, + YAW = 3, + MODE = 4, + RETURN = 5, + ASSISTED = 6, + MISSION = 7, + OFFBOARD_MODE = 8, + FLAPS = 9, + AUX_1 = 10, + AUX_2 = 11, + AUX_3 = 12, + AUX_4 = 13, + AUX_5 = 14, + RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */ }; /** @@ -85,16 +84,16 @@ enum RC_CHANNELS_FUNCTION struct rc_channels_s { uint64_t timestamp; /**< In microseconds since boot time. */ - uint64_t timestamp_last_valid; /**< timestamp of last valid RC signal. */ - struct { - float scaled; /**< Scaled to -1..1 (throttle: 0..1) */ - } chan[RC_CHANNELS_MAPPED_MAX]; - uint8_t chan_count; /**< number of valid channels */ + uint64_t timestamp_last_valid; /**< timestamp of last valid RC signal. */ + struct { + float scaled; /**< Scaled to -1..1 (throttle: 0..1) */ + } chan[RC_CHANNELS_MAPPED_MAX]; + uint8_t chan_count; /**< number of valid channels */ - /*String array to store the names of the functions*/ - char function_name[RC_CHANNELS_FUNCTION_MAX][20]; - int8_t function[RC_CHANNELS_FUNCTION_MAX]; - uint8_t rssi; /**< Overall receive signal strength */ + /*String array to store the names of the functions*/ + char function_name[RC_CHANNELS_FUNCTION_MAX][20]; + int8_t function[RC_CHANNELS_FUNCTION_MAX]; + uint8_t rssi; /**< Overall receive signal strength */ }; /**< radio control channels. */ /** diff --git a/src/modules/uORB/topics/subsystem_info.h b/src/modules/uORB/topics/subsystem_info.h index cfe0bf69e9..d3a7ce10d8 100644 --- a/src/modules/uORB/topics/subsystem_info.h +++ b/src/modules/uORB/topics/subsystem_info.h @@ -50,8 +50,7 @@ #include #include "../uORB.h" -enum SUBSYSTEM_TYPE -{ +enum SUBSYSTEM_TYPE { SUBSYSTEM_TYPE_GYRO = 1, SUBSYSTEM_TYPE_ACC = 2, SUBSYSTEM_TYPE_MAG = 4, diff --git a/src/modules/uORB/topics/telemetry_status.h b/src/modules/uORB/topics/telemetry_status.h index 828fb31cc7..c181ec04fb 100644 --- a/src/modules/uORB/topics/telemetry_status.h +++ b/src/modules/uORB/topics/telemetry_status.h @@ -44,10 +44,10 @@ #include "../uORB.h" enum TELEMETRY_STATUS_RADIO_TYPE { - TELEMETRY_STATUS_RADIO_TYPE_GENERIC = 0, - TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO, - TELEMETRY_STATUS_RADIO_TYPE_UBIQUITY_BULLET, - TELEMETRY_STATUS_RADIO_TYPE_WIRE + TELEMETRY_STATUS_RADIO_TYPE_GENERIC = 0, + TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO, + TELEMETRY_STATUS_RADIO_TYPE_UBIQUITY_BULLET, + TELEMETRY_STATUS_RADIO_TYPE_WIRE }; /** @@ -57,14 +57,14 @@ enum TELEMETRY_STATUS_RADIO_TYPE { struct telemetry_status_s { uint64_t timestamp; - enum TELEMETRY_STATUS_RADIO_TYPE type; /**< type of the radio hardware */ + enum TELEMETRY_STATUS_RADIO_TYPE type; /**< type of the radio hardware */ unsigned rssi; /**< local signal strength */ - unsigned remote_rssi; /**< remote signal strength */ - unsigned rxerrors; /**< receive errors */ - unsigned fixed; /**< count of error corrected packets */ - uint8_t noise; /**< background noise level */ - uint8_t remote_noise; /**< remote background noise level */ - uint8_t txbuf; /**< how full the tx buffer is as a percentage */ + unsigned remote_rssi; /**< remote signal strength */ + unsigned rxerrors; /**< receive errors */ + unsigned fixed; /**< count of error corrected packets */ + uint8_t noise; /**< background noise level */ + uint8_t remote_noise; /**< remote background noise level */ + uint8_t txbuf; /**< how full the tx buffer is as a percentage */ }; /** diff --git a/src/modules/uORB/topics/vehicle_attitude_setpoint.h b/src/modules/uORB/topics/vehicle_attitude_setpoint.h index 1a245132a8..fda3cd75e6 100644 --- a/src/modules/uORB/topics/vehicle_attitude_setpoint.h +++ b/src/modules/uORB/topics/vehicle_attitude_setpoint.h @@ -52,8 +52,7 @@ /** * vehicle attitude setpoint. */ -struct vehicle_attitude_setpoint_s -{ +struct vehicle_attitude_setpoint_s { uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ float roll_body; /**< body angle in NED frame */ diff --git a/src/modules/uORB/topics/vehicle_bodyframe_speed_setpoint.h b/src/modules/uORB/topics/vehicle_bodyframe_speed_setpoint.h index fbfab09f30..fd1ade671a 100644 --- a/src/modules/uORB/topics/vehicle_bodyframe_speed_setpoint.h +++ b/src/modules/uORB/topics/vehicle_bodyframe_speed_setpoint.h @@ -48,8 +48,7 @@ * @{ */ -struct vehicle_bodyframe_speed_setpoint_s -{ +struct vehicle_bodyframe_speed_setpoint_s { uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ float vx; /**< in m/s */ diff --git a/src/modules/uORB/topics/vehicle_command.h b/src/modules/uORB/topics/vehicle_command.h index e6e4743c6a..c21a29b130 100644 --- a/src/modules/uORB/topics/vehicle_command.h +++ b/src/modules/uORB/topics/vehicle_command.h @@ -51,43 +51,42 @@ * Should contain all commands from MAVLink's VEHICLE_CMD ENUM, * but can contain additional ones. */ -enum VEHICLE_CMD -{ - 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| */ - VEHICLE_CMD_NAV_LOITER_TIME=19, /* Loiter around this MISSION for X seconds |Seconds (decimal)| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ - VEHICLE_CMD_NAV_RETURN_TO_LAUNCH=20, /* Return to launch location |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_NAV_LAND=21, /* Land at location |Empty| Empty| Empty| Desired yaw angle.| Latitude| Longitude| Altitude| */ - 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_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| */ - VEHICLE_CMD_CONDITION_DISTANCE=114, /* Delay mission state machine until within desired distance of next NAV point. |Distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_CONDITION_YAW=115, /* Reach a certain target angle. |target angle: [0-360], 0 is north| speed during yaw change:[deg per second]| direction: negative: counter clockwise, positive: clockwise [-1,1]| relative offset or absolute angle: [ 1,0]| Empty| Empty| Empty| */ - VEHICLE_CMD_CONDITION_LAST=159, /* NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_SET_MODE=176, /* Set system mode. |Mode, as defined by ENUM MAV_MODE| Empty| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_JUMP=177, /* Jump to the desired command in the mission list. Repeat this action only the specified number of times |Sequence number| Repeat count| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_CHANGE_SPEED=178, /* Change speed and/or throttle set points. |Speed type (0=Airspeed, 1=Ground Speed)| Speed (m/s, -1 indicates no change)| Throttle ( Percent, -1 indicates no change)| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_SET_HOME=179, /* Changes the home location either to the current location or a specified location. |Use current (1=use current location, 0=use specified location)| Empty| Empty| Empty| Latitude| Longitude| Altitude| */ - VEHICLE_CMD_DO_SET_PARAMETER=180, /* Set a system parameter. Caution! Use of this command requires knowledge of the numeric enumeration value of the parameter. |Parameter number| Parameter value| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_SET_RELAY=181, /* Set a relay to a condition. |Relay number| Setting (1=on, 0=off, others possible depending on system hardware)| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_REPEAT_RELAY=182, /* Cycle a relay on and off for a desired number of cyles with a desired period. |Relay number| Cycle count| Cycle time (seconds, decimal)| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_SET_SERVO=183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ - VEHICLE_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - VEHICLE_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty| */ - VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS=242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| */ - VEHICLE_CMD_PREFLIGHT_STORAGE=245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty| */ - VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN=246, /* Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty| */ - VEHICLE_CMD_OVERRIDE_GOTO=252, /* Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position| */ - 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, /* | */ +enum VEHICLE_CMD { + 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| */ + VEHICLE_CMD_NAV_LOITER_TIME = 19, /* Loiter around this MISSION for X seconds |Seconds (decimal)| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ + VEHICLE_CMD_NAV_RETURN_TO_LAUNCH = 20, /* Return to launch location |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_NAV_LAND = 21, /* Land at location |Empty| Empty| Empty| Desired yaw angle.| Latitude| Longitude| Altitude| */ + 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_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| */ + VEHICLE_CMD_CONDITION_DISTANCE = 114, /* Delay mission state machine until within desired distance of next NAV point. |Distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_CONDITION_YAW = 115, /* Reach a certain target angle. |target angle: [0-360], 0 is north| speed during yaw change:[deg per second]| direction: negative: counter clockwise, positive: clockwise [-1,1]| relative offset or absolute angle: [ 1,0]| Empty| Empty| Empty| */ + VEHICLE_CMD_CONDITION_LAST = 159, /* NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_SET_MODE = 176, /* Set system mode. |Mode, as defined by ENUM MAV_MODE| Empty| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_JUMP = 177, /* Jump to the desired command in the mission list. Repeat this action only the specified number of times |Sequence number| Repeat count| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_CHANGE_SPEED = 178, /* Change speed and/or throttle set points. |Speed type (0=Airspeed, 1=Ground Speed)| Speed (m/s, -1 indicates no change)| Throttle ( Percent, -1 indicates no change)| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_SET_HOME = 179, /* Changes the home location either to the current location or a specified location. |Use current (1=use current location, 0=use specified location)| Empty| Empty| Empty| Latitude| Longitude| Altitude| */ + VEHICLE_CMD_DO_SET_PARAMETER = 180, /* Set a system parameter. Caution! Use of this command requires knowledge of the numeric enumeration value of the parameter. |Parameter number| Parameter value| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_SET_RELAY = 181, /* Set a relay to a condition. |Relay number| Setting (1=on, 0=off, others possible depending on system hardware)| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_REPEAT_RELAY = 182, /* Cycle a relay on and off for a desired number of cyles with a desired period. |Relay number| Cycle count| Cycle time (seconds, decimal)| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_SET_SERVO = 183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_REPEAT_SERVO = 184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_CONTROL_VIDEO = 200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ + VEHICLE_CMD_DO_LAST = 240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_PREFLIGHT_CALIBRATION = 241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty| */ + VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS = 242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| */ + VEHICLE_CMD_PREFLIGHT_STORAGE = 245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty| */ + VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN = 246, /* Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty| */ + VEHICLE_CMD_OVERRIDE_GOTO = 252, /* Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position| */ + 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, /* | */ }; /** @@ -96,14 +95,13 @@ enum VEHICLE_CMD * Should contain all of MAVLink's VEHICLE_CMD_RESULT values * but can contain additional ones. */ -enum VEHICLE_CMD_RESULT -{ - VEHICLE_CMD_RESULT_ACCEPTED=0, /* Command ACCEPTED and EXECUTED | */ - VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED=1, /* Command TEMPORARY REJECTED/DENIED | */ - VEHICLE_CMD_RESULT_DENIED=2, /* Command PERMANENTLY DENIED | */ - VEHICLE_CMD_RESULT_UNSUPPORTED=3, /* Command UNKNOWN/UNSUPPORTED | */ - VEHICLE_CMD_RESULT_FAILED=4, /* Command executed, but failed | */ - VEHICLE_CMD_RESULT_ENUM_END=5, /* | */ +enum VEHICLE_CMD_RESULT { + VEHICLE_CMD_RESULT_ACCEPTED = 0, /* Command ACCEPTED and EXECUTED | */ + VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED = 1, /* Command TEMPORARY REJECTED/DENIED | */ + VEHICLE_CMD_RESULT_DENIED = 2, /* Command PERMANENTLY DENIED | */ + VEHICLE_CMD_RESULT_UNSUPPORTED = 3, /* Command UNKNOWN/UNSUPPORTED | */ + VEHICLE_CMD_RESULT_FAILED = 4, /* Command executed, but failed | */ + VEHICLE_CMD_RESULT_ENUM_END = 5, /* | */ }; /** @@ -111,8 +109,7 @@ enum VEHICLE_CMD_RESULT * @{ */ -struct vehicle_command_s -{ +struct vehicle_command_s { float param1; /**< Parameter 1, as defined by MAVLink VEHICLE_CMD enum. */ float param2; /**< Parameter 2, as defined by MAVLink VEHICLE_CMD enum. */ float param3; /**< Parameter 3, as defined by MAVLink VEHICLE_CMD enum. */ diff --git a/src/modules/uORB/topics/vehicle_control_debug.h b/src/modules/uORB/topics/vehicle_control_debug.h index 6184284a43..2a45eaccc2 100644 --- a/src/modules/uORB/topics/vehicle_control_debug.h +++ b/src/modules/uORB/topics/vehicle_control_debug.h @@ -47,8 +47,7 @@ * @addtogroup topics * @{ */ -struct vehicle_control_debug_s -{ +struct vehicle_control_debug_s { uint64_t timestamp; /**< in microseconds since system start */ float roll_p; /**< roll P control part */ @@ -77,9 +76,9 @@ struct vehicle_control_debug_s }; /**< vehicle_control_debug */ - /** - * @} - */ +/** +* @} +*/ /* register this as object request broker structure */ ORB_DECLARE(vehicle_control_debug); diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 38fb74c9b9..4c31f92b5f 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -38,7 +38,7 @@ /** * @file vehicle_control_mode.h * Definition of the vehicle_control_mode uORB topic. - * + * * All control apps should depend their actions based on the flags set here. */ @@ -59,8 +59,7 @@ * * Encodes the complete system state and is set by the commander app. */ -struct vehicle_control_mode_s -{ +struct vehicle_control_mode_s { uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ bool flag_armed; diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h index 143734e372..4376fa1bab 100644 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ b/src/modules/uORB/topics/vehicle_global_position.h @@ -51,16 +51,15 @@ * @{ */ - /** - * Fused global position in WGS84. - * - * This struct contains the system's believ about its position. It is not the raw GPS - * measurement (@see vehicle_gps_position). This topic is usually published by the position - * estimator, which will take more sources of information into account than just GPS, - * e.g. control inputs of the vehicle in a Kalman-filter implementation. - */ -struct vehicle_global_position_s -{ +/** +* Fused global position in WGS84. +* +* This struct contains the system's believ about its position. It is not the raw GPS +* measurement (@see vehicle_gps_position). This topic is usually published by the position +* estimator, which will take more sources of information into account than just GPS, +* e.g. control inputs of the vehicle in a Kalman-filter implementation. +*/ +struct vehicle_global_position_s { uint64_t timestamp; /**< time of this estimate, in microseconds since system start */ uint64_t time_gps_usec; /**< GPS timestamp in microseconds */ bool valid; /**< true if position satisfies validity criteria of estimator */ diff --git a/src/modules/uORB/topics/vehicle_global_position_set_triplet.h b/src/modules/uORB/topics/vehicle_global_position_set_triplet.h index 8516b263fc..e8565d887a 100644 --- a/src/modules/uORB/topics/vehicle_global_position_set_triplet.h +++ b/src/modules/uORB/topics/vehicle_global_position_set_triplet.h @@ -58,8 +58,7 @@ * * This are the three next waypoints (or just the next two or one). */ -struct vehicle_global_position_set_triplet_s -{ +struct vehicle_global_position_set_triplet_s { bool previous_valid; /**< flag indicating previous position is valid */ bool next_valid; /**< flag indicating next position is valid */ diff --git a/src/modules/uORB/topics/vehicle_global_position_setpoint.h b/src/modules/uORB/topics/vehicle_global_position_setpoint.h index a56434d3b0..8102e367da 100644 --- a/src/modules/uORB/topics/vehicle_global_position_setpoint.h +++ b/src/modules/uORB/topics/vehicle_global_position_setpoint.h @@ -58,8 +58,7 @@ * This is the position the MAV is heading towards. If it of type loiter, * the MAV is circling around it with the given loiter radius in meters. */ -struct vehicle_global_position_setpoint_s -{ +struct vehicle_global_position_setpoint_s { bool altitude_is_relative; /**< true if altitude is relative from start point */ int32_t lat; /**< latitude in degrees * 1E7 */ int32_t lon; /**< longitude in degrees * 1E7 */ diff --git a/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h b/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h index 73961cdfed..5dac877d08 100644 --- a/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h +++ b/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h @@ -47,8 +47,7 @@ * @{ */ -struct vehicle_global_velocity_setpoint_s -{ +struct vehicle_global_velocity_setpoint_s { float vx; /**< in m/s NED */ float vy; /**< in m/s NED */ float vz; /**< in m/s NED */ diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h index 1639a08c2b..794c3f8bc7 100644 --- a/src/modules/uORB/topics/vehicle_gps_position.h +++ b/src/modules/uORB/topics/vehicle_gps_position.h @@ -53,13 +53,12 @@ /** * GPS position in WGS84 coordinates. */ -struct vehicle_gps_position_s -{ +struct vehicle_gps_position_s { uint64_t timestamp_position; /**< Timestamp for position information */ int32_t lat; /**< Latitude in 1E-7 degrees */ int32_t lon; /**< Longitude in 1E-7 degrees */ int32_t alt; /**< Altitude in 1E-3 meters (millimeters) above MSL */ - + uint64_t timestamp_variance; float s_variance_m_s; /**< speed accuracy estimate m/s */ float p_variance_m; /**< position accuracy estimate m */ diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h index 4271537829..44c339711d 100644 --- a/src/modules/uORB/topics/vehicle_local_position.h +++ b/src/modules/uORB/topics/vehicle_local_position.h @@ -52,8 +52,7 @@ /** * Fused local position in NED. */ -struct vehicle_local_position_s -{ +struct vehicle_local_position_s { uint64_t timestamp; /**< Time of this estimate, in microseconds since system start */ bool xy_valid; /**< true if x and y are valid */ bool z_valid; /**< true if z is valid */ diff --git a/src/modules/uORB/topics/vehicle_local_position_setpoint.h b/src/modules/uORB/topics/vehicle_local_position_setpoint.h index d24d81e3a4..8988a0330d 100644 --- a/src/modules/uORB/topics/vehicle_local_position_setpoint.h +++ b/src/modules/uORB/topics/vehicle_local_position_setpoint.h @@ -49,8 +49,7 @@ * @{ */ -struct vehicle_local_position_setpoint_s -{ +struct vehicle_local_position_setpoint_s { float x; /**< in meters NED */ float y; /**< in meters NED */ float z; /**< in meters NED */ diff --git a/src/modules/uORB/topics/vehicle_rates_setpoint.h b/src/modules/uORB/topics/vehicle_rates_setpoint.h index 46e62c4b71..9f8b412a7d 100644 --- a/src/modules/uORB/topics/vehicle_rates_setpoint.h +++ b/src/modules/uORB/topics/vehicle_rates_setpoint.h @@ -47,8 +47,7 @@ * @addtogroup topics * @{ */ -struct vehicle_rates_setpoint_s -{ +struct vehicle_rates_setpoint_s { uint64_t timestamp; /**< in microseconds since system start */ float roll; /**< body angular rates in NED frame */ @@ -58,9 +57,9 @@ struct vehicle_rates_setpoint_s }; /**< vehicle_rates_setpoint */ - /** - * @} - */ +/** +* @} +*/ /* register this as object request broker structure */ ORB_DECLARE(vehicle_rates_setpoint); diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 1c184d3a7a..7ac1a66604 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -131,31 +131,31 @@ enum VEHICLE_MODE_FLAG { * Should match 1:1 MAVLink's MAV_TYPE ENUM */ enum VEHICLE_TYPE { - VEHICLE_TYPE_GENERIC=0, /* Generic micro air vehicle. | */ - VEHICLE_TYPE_FIXED_WING=1, /* Fixed wing aircraft. | */ - VEHICLE_TYPE_QUADROTOR=2, /* Quadrotor | */ - VEHICLE_TYPE_COAXIAL=3, /* Coaxial helicopter | */ - VEHICLE_TYPE_HELICOPTER=4, /* Normal helicopter with tail rotor. | */ - VEHICLE_TYPE_ANTENNA_TRACKER=5, /* Ground installation | */ - VEHICLE_TYPE_GCS=6, /* Operator control unit / ground control station | */ - VEHICLE_TYPE_AIRSHIP=7, /* Airship, controlled | */ - VEHICLE_TYPE_FREE_BALLOON=8, /* Free balloon, uncontrolled | */ - VEHICLE_TYPE_ROCKET=9, /* Rocket | */ - VEHICLE_TYPE_GROUND_ROVER=10, /* Ground rover | */ - VEHICLE_TYPE_SURFACE_BOAT=11, /* Surface vessel, boat, ship | */ - VEHICLE_TYPE_SUBMARINE=12, /* Submarine | */ - VEHICLE_TYPE_HEXAROTOR=13, /* Hexarotor | */ - VEHICLE_TYPE_OCTOROTOR=14, /* Octorotor | */ - VEHICLE_TYPE_TRICOPTER=15, /* Octorotor | */ - VEHICLE_TYPE_FLAPPING_WING=16, /* Flapping wing | */ - VEHICLE_TYPE_KITE=17, /* Kite | */ - VEHICLE_TYPE_ENUM_END=18, /* | */ + VEHICLE_TYPE_GENERIC = 0, /* Generic micro air vehicle. | */ + VEHICLE_TYPE_FIXED_WING = 1, /* Fixed wing aircraft. | */ + VEHICLE_TYPE_QUADROTOR = 2, /* Quadrotor | */ + VEHICLE_TYPE_COAXIAL = 3, /* Coaxial helicopter | */ + VEHICLE_TYPE_HELICOPTER = 4, /* Normal helicopter with tail rotor. | */ + VEHICLE_TYPE_ANTENNA_TRACKER = 5, /* Ground installation | */ + VEHICLE_TYPE_GCS = 6, /* Operator control unit / ground control station | */ + VEHICLE_TYPE_AIRSHIP = 7, /* Airship, controlled | */ + VEHICLE_TYPE_FREE_BALLOON = 8, /* Free balloon, uncontrolled | */ + VEHICLE_TYPE_ROCKET = 9, /* Rocket | */ + VEHICLE_TYPE_GROUND_ROVER = 10, /* Ground rover | */ + VEHICLE_TYPE_SURFACE_BOAT = 11, /* Surface vessel, boat, ship | */ + VEHICLE_TYPE_SUBMARINE = 12, /* Submarine | */ + VEHICLE_TYPE_HEXAROTOR = 13, /* Hexarotor | */ + VEHICLE_TYPE_OCTOROTOR = 14, /* Octorotor | */ + VEHICLE_TYPE_TRICOPTER = 15, /* Octorotor | */ + VEHICLE_TYPE_FLAPPING_WING = 16, /* Flapping wing | */ + VEHICLE_TYPE_KITE = 17, /* Kite | */ + VEHICLE_TYPE_ENUM_END = 18, /* | */ }; enum VEHICLE_BATTERY_WARNING { - VEHICLE_BATTERY_WARNING_NONE = 0, /**< no battery low voltage warning active */ - VEHICLE_BATTERY_WARNING_LOW, /**< warning of low voltage */ - VEHICLE_BATTERY_WARNING_CRITICAL /**< alerting of critical voltage */ + VEHICLE_BATTERY_WARNING_NONE = 0, /**< no battery low voltage warning active */ + VEHICLE_BATTERY_WARNING_LOW, /**< warning of low voltage */ + VEHICLE_BATTERY_WARNING_CRITICAL /**< alerting of critical voltage */ }; /** @@ -168,8 +168,7 @@ enum VEHICLE_BATTERY_WARNING { * * Encodes the complete system state and is set by the commander app. */ -struct vehicle_status_s -{ +struct vehicle_status_s { /* use of a counter and timestamp recommended (but not necessary) */ uint16_t counter; /**< incremented by the writing thread everytime new data is stored */ @@ -216,7 +215,7 @@ struct vehicle_status_s uint32_t onboard_control_sensors_present; uint32_t onboard_control_sensors_enabled; uint32_t onboard_control_sensors_health; - + float load; /**< processor load from 0 to 1 */ float battery_voltage; float battery_current; diff --git a/src/modules/uORB/topics/vehicle_vicon_position.h b/src/modules/uORB/topics/vehicle_vicon_position.h index 0822fa89a1..e19a34a5d7 100644 --- a/src/modules/uORB/topics/vehicle_vicon_position.h +++ b/src/modules/uORB/topics/vehicle_vicon_position.h @@ -52,8 +52,7 @@ /** * Fused local position in NED. */ -struct vehicle_vicon_position_s -{ +struct vehicle_vicon_position_s { uint64_t timestamp; /**< time of this estimate, in microseconds since system start */ bool valid; /**< true if position satisfies validity criteria of estimator */ From 6b55baad7853670165a93135416b6024397d9955 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 12 Jan 2014 23:41:52 +0100 Subject: [PATCH 024/381] Tool command to print full filter states --- .../fw_att_pos_estimator_main.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 466f6d51f4..142abc50df 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -875,12 +875,16 @@ int fw_att_pos_estimator_main(int argc, char *argv[]) math::Quaternion q(states[0], states[1], states[2], states[3]); math::EulerAngles euler(q); - printf("attitude: roll: %8.4f, pitch %8.4f, yaw: %8.4f degrees", + printf("attitude: roll: %8.4f, pitch %8.4f, yaw: %8.4f degrees\n", (double)math::degrees(euler.getPhi()), (double)math::degrees(euler.getTheta()), (double)math::degrees(euler.getPsi())); - printf("states [1-3]: %8.4f, %8.4f, %8.4f\n", (double)states[0], (double)states[1], (double)states[2]); - printf("states [4-6]: %8.4f, %8.4f, %8.4f\n", (double)states[3], (double)states[4], (double)states[5]); - printf("states [7-9]: %8.4f, %8.4f, %8.4f\n", (double)states[6], (double)states[7], (double)states[8]); + printf("states [1-4]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)states[0], (double)states[1], (double)states[2], (double)states[3]); + printf("states [5-7]: %8.4f, %8.4f, %8.4f\n", (double)states[4], (double)states[5], (double)states[6]); + printf("states [8-10]: %8.4f, %8.4f, %8.4f\n", (double)states[7], (double)states[8], (double)states[9]); + printf("states [11-13]: %8.4f, %8.4f, %8.4f\n", (double)states[10], (double)states[11], (double)states[12]); + printf("states [14-16]: %8.4f, %8.4f, %8.4f\n", (double)states[13], (double)states[14], (double)states[15]); + printf("states [17-19]: %8.4f, %8.4f, %8.4f\n", (double)states[16], (double)states[17], (double)states[18]); + printf("states [20-21]: %8.4f, %8.4f\n", (double)states[19], (double)states[20]); exit(0); } else { From 8a37633d2d48dde675a2dda6d5c8f8af39197da0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 13 Jan 2014 08:32:44 +0100 Subject: [PATCH 025/381] Copyright cleanup --- src/modules/sensors/sensors.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index a494cd08d6..e13fc4a919 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -37,6 +36,8 @@ * Sensor readout process. * * @author Lorenz Meier + * @author Julian Oes + * @author Thomas Gubler */ #include From 1bf7e10b9fc76327fa8d7322eca36ec4fc4935e6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 13 Jan 2014 21:21:45 +0100 Subject: [PATCH 026/381] Better output --- .../fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 142abc50df..9ea4e93b82 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -878,9 +878,9 @@ int fw_att_pos_estimator_main(int argc, char *argv[]) printf("attitude: roll: %8.4f, pitch %8.4f, yaw: %8.4f degrees\n", (double)math::degrees(euler.getPhi()), (double)math::degrees(euler.getTheta()), (double)math::degrees(euler.getPsi())); - printf("states [1-4]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)states[0], (double)states[1], (double)states[2], (double)states[3]); - printf("states [5-7]: %8.4f, %8.4f, %8.4f\n", (double)states[4], (double)states[5], (double)states[6]); - printf("states [8-10]: %8.4f, %8.4f, %8.4f\n", (double)states[7], (double)states[8], (double)states[9]); + printf("states (quat) [1-4]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)states[0], (double)states[1], (double)states[2], (double)states[3]); + printf("states (vel) [5-7]: %8.4f, %8.4f, %8.4f\n", (double)states[4], (double)states[5], (double)states[6]); + printf("states (pos) [8-10]: %8.4f, %8.4f, %8.4f\n", (double)states[7], (double)states[8], (double)states[9]); printf("states [11-13]: %8.4f, %8.4f, %8.4f\n", (double)states[10], (double)states[11], (double)states[12]); printf("states [14-16]: %8.4f, %8.4f, %8.4f\n", (double)states[13], (double)states[14], (double)states[15]); printf("states [17-19]: %8.4f, %8.4f, %8.4f\n", (double)states[16], (double)states[17], (double)states[18]); From b7b1c1428f672ccfec78e2e9ef4e2542a53f8821 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 14 Jan 2014 07:52:40 +0100 Subject: [PATCH 027/381] Fixed perf init --- .../fw_att_pos_estimator_main.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 9ea4e93b82..dc05540631 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -251,13 +251,13 @@ FixedwingEstimator::FixedwingEstimator() : _local_pos_pub(-1), /* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "fw_att_pos_estimator")), - _perf_gyro(perf_alloc(PC_ELAPSED, "fw_ekf_gyro_upd")), - _perf_accel(perf_alloc(PC_ELAPSED, "fw_ekf_accel_upd")), - _perf_mag(perf_alloc(PC_ELAPSED, "fw_ekf_mag_upd")), - _perf_gps(perf_alloc(PC_ELAPSED, "fw_ekf_gps_upd")), - _perf_baro(perf_alloc(PC_ELAPSED, "fw_ekf_baro_upd")), - _perf_airspeed(perf_alloc(PC_ELAPSED, "fw_ekf_aspd_upd")), + _loop_perf(perf_alloc(PC_COUNT, "fw_att_pos_estimator")), + _perf_gyro(perf_alloc(PC_COUNT, "fw_ekf_gyro_upd")), + _perf_accel(perf_alloc(PC_COUNT, "fw_ekf_accel_upd")), + _perf_mag(perf_alloc(PC_COUNT, "fw_ekf_mag_upd")), + _perf_gps(perf_alloc(PC_COUNT, "fw_ekf_gps_upd")), + _perf_baro(perf_alloc(PC_COUNT, "fw_ekf_baro_upd")), + _perf_airspeed(perf_alloc(PC_COUNT, "fw_ekf_aspd_upd")), /* states */ _initialized(false) From fb446c01b815b00f5da098d340557164fdbf78c9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 17 Jan 2014 14:39:01 +0100 Subject: [PATCH 028/381] Embedded estimator code locally --- .../fw_att_pos_estimator/estimator.cpp | 1843 +++++++++++++++++ src/modules/fw_att_pos_estimator/estimator.h | 172 ++ .../fw_att_pos_estimator_main.cpp | 2 +- src/modules/fw_att_pos_estimator/module.mk | 2 +- 4 files changed, 2017 insertions(+), 2 deletions(-) create mode 100644 src/modules/fw_att_pos_estimator/estimator.cpp create mode 100644 src/modules/fw_att_pos_estimator/estimator.h diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp new file mode 100644 index 0000000000..388cea8646 --- /dev/null +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -0,0 +1,1843 @@ +#include "estimator.h" + +// Global variables +float KH[n_states][n_states]; // intermediate result used for covariance updates +float KHP[n_states][n_states]; // intermediate result used for covariance updates +float P[n_states][n_states]; // covariance matrix +float Kfusion[n_states]; // Kalman gains +float states[n_states]; // state matrix +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 +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) +Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad) +Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s) +float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2) +Vector3f earthRateNED; // earths angular rate vector in NED (rad/s) +Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s) +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 dt; // time lapsed since last covariance prediction +bool onGround = true; // boolean true when the flight vehicle is on the ground (not flying) +bool useAirspeed = true; // boolean true if airspeed data is being used +bool useCompass = true; // boolean true if magnetometer data is being used +uint8_t fusionModeGPS = 0; // 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 +bool fuseVelData = false; // this boolean causes the posNE and velNED obs to be fused +bool fusePosData = false; // this boolean causes the posNE and velNED obs to be fused +bool fuseHgtData = false; // this boolean causes the hgtMea obs to be fused +bool fuseMagData = false; // boolean true when magnetometer data is to be fused + +float velNED[3]; // North, East, Down velocity obs (m/s) +float posNE[2]; // North, East position obs (m) +float hgtMea; // measured height (m) +float posNED[3]; // North, East Down position (m) + +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 +float innovMag[3]; // innovation output +float varInnovMag[3]; // innovation variance output +Vector3f magData; // magnetometer flux radings in X,Y,Z body axes +float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time +float innovVtas; // innovation output +float varInnovVtas; // innovation variance output +bool fuseVtasData = false; // boolean true when airspeed data is to be fused +float VtasMeas; // true airspeed measurement (m/s) +float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time +float latRef; // WGS-84 latitude of reference point (rad) +float lonRef; // WGS-84 longitude of reference point (rad) +float hgtRef; // WGS-84 height of reference point (m) +Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes +uint8_t covSkipCount = 0; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction +float EAS2TAS = 1.0f; // ratio f true to equivalent airspeed + +// GPS input data variables +float gpsCourse; +float gpsGndSpd; +float gpsVelD; +float gpsLat; +float gpsLon; +float gpsHgt; +uint8_t GPSstatus; + + +bool statesInitialised = false; + + +float Vector3f::length(void) const +{ + return sqrt(x*x + y*y + z*z); +} + +Vector3f Vector3f::zero(void) const +{ + Vector3f ret = *this; + ret.x = 0.0; + ret.y = 0.0; + ret.z = 0.0; + return ret; +} + +Mat3f Mat3f::transpose(void) const +{ + Mat3f ret = *this; + swap_var(ret.x.y, ret.y.x); + swap_var(ret.x.z, ret.z.x); + swap_var(ret.y.z, ret.z.y); + return ret; +} + +// overload + operator to provide a vector addition +Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2) +{ + Vector3f vecOut; + vecOut.x = vecIn1.x + vecIn2.x; + vecOut.y = vecIn1.y + vecIn2.y; + vecOut.z = vecIn1.z + vecIn2.z; + return vecOut; +} + +// overload - operator to provide a vector subtraction +Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2) +{ + Vector3f vecOut; + vecOut.x = vecIn1.x - vecIn2.x; + vecOut.y = vecIn1.y - vecIn2.y; + vecOut.z = vecIn1.z - vecIn2.z; + return vecOut; +} + +// overload * operator to provide a matrix vector product +Vector3f operator*( Mat3f matIn, Vector3f vecIn) +{ + Vector3f vecOut; + vecOut.x = matIn.x.x*vecIn.x + matIn.x.y*vecIn.y + matIn.x.z*vecIn.z; + vecOut.y = matIn.y.x*vecIn.x + matIn.y.y*vecIn.y + matIn.y.z*vecIn.z; + vecOut.z = matIn.x.x*vecIn.x + matIn.z.y*vecIn.y + matIn.z.z*vecIn.z; + return vecOut; +} + +// overload % operator to provide a vector cross product +Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2) +{ + Vector3f vecOut; + vecOut.x = vecIn1.y*vecIn2.z - vecIn1.z*vecIn2.y; + vecOut.y = vecIn1.z*vecIn2.x - vecIn1.x*vecIn2.z; + vecOut.z = vecIn1.x*vecIn2.y - vecIn1.y*vecIn2.x; + return vecOut; +} + +// overload * operator to provide a vector scaler product +Vector3f operator*(Vector3f vecIn1, float sclIn1) +{ + Vector3f vecOut; + vecOut.x = vecIn1.x * sclIn1; + vecOut.y = vecIn1.y * sclIn1; + vecOut.z = vecIn1.z * sclIn1; + return vecOut; +} + +// overload * operator to provide a vector scaler product +Vector3f operator*(float sclIn1, Vector3f vecIn1) +{ + Vector3f vecOut; + vecOut.x = vecIn1.x * sclIn1; + vecOut.y = vecIn1.y * sclIn1; + vecOut.z = vecIn1.z * sclIn1; + return vecOut; +} + +void swap_var(float &d1, float &d2) +{ + float tmp = d1; + d1 = d2; + d2 = tmp; +} + +void UpdateStrapdownEquationsNED() +{ + static Vector3f prevDelAng; + Vector3f delVelNav; + float q00; + float q11; + float q22; + float q33; + float q01; + float q02; + float q03; + float q12; + float q13; + float q23; + static Mat3f Tbn; + static Mat3f Tnb; + float rotationMag; + float rotScaler; + float qUpdated[4]; + float quatMag; + float quatMagInv; + float deltaQuat[4]; + static float lastVelocity[3]; + const Vector3f gravityNED = {0.0,0.0,GRAVITY_MSS}; + + // Remove sensor bias errors + correctedDelAng.x = dAngIMU.x - states[10]; + correctedDelAng.y = dAngIMU.y - states[11]; + correctedDelAng.z = dAngIMU.z - states[12]; + dVelIMU.x = dVelIMU.x; + dVelIMU.y = dVelIMU.y; + dVelIMU.z = dVelIMU.z; + + // Save current measurements + prevDelAng = correctedDelAng; + + // Apply corrections for earths rotation rate and coning errors + // * and + operators have been overloaded + correctedDelAng = correctedDelAng - Tnb*earthRateNED*dtIMU + 8.333333333333333e-2f*(prevDelAng % correctedDelAng); + + // Convert the rotation vector to its equivalent quaternion + rotationMag = correctedDelAng.length(); + if (rotationMag < 1e-12f) + { + deltaQuat[0] = 1.0; + deltaQuat[1] = 0.0; + deltaQuat[2] = 0.0; + deltaQuat[3] = 0.0; + } + else + { + deltaQuat[0] = cos(0.5f*rotationMag); + rotScaler = (sin(0.5f*rotationMag))/rotationMag; + deltaQuat[1] = correctedDelAng.x*rotScaler; + deltaQuat[2] = correctedDelAng.y*rotScaler; + deltaQuat[3] = correctedDelAng.z*rotScaler; + } + + // Update the quaternions by rotating from the previous attitude through + // the delta angle rotation quaternion + qUpdated[0] = states[0]*deltaQuat[0] - states[1]*deltaQuat[1] - states[2]*deltaQuat[2] - states[3]*deltaQuat[3]; + qUpdated[1] = states[0]*deltaQuat[1] + states[1]*deltaQuat[0] + states[2]*deltaQuat[3] - states[3]*deltaQuat[2]; + qUpdated[2] = states[0]*deltaQuat[2] + states[2]*deltaQuat[0] + states[3]*deltaQuat[1] - states[1]*deltaQuat[3]; + qUpdated[3] = states[0]*deltaQuat[3] + states[3]*deltaQuat[0] + states[1]*deltaQuat[2] - states[2]*deltaQuat[1]; + + // Normalise the quaternions and update the quaternion states + quatMag = sqrt(sq(qUpdated[0]) + sq(qUpdated[1]) + sq(qUpdated[2]) + sq(qUpdated[3])); + if (quatMag > 1e-16f) + { + quatMagInv = 1.0f/quatMag; + states[0] = quatMagInv*qUpdated[0]; + states[1] = quatMagInv*qUpdated[1]; + states[2] = quatMagInv*qUpdated[2]; + states[3] = quatMagInv*qUpdated[3]; + } + + // Calculate the body to nav cosine matrix + q00 = sq(states[0]); + q11 = sq(states[1]); + q22 = sq(states[2]); + q33 = sq(states[3]); + q01 = states[0]*states[1]; + q02 = states[0]*states[2]; + q03 = states[0]*states[3]; + q12 = states[1]*states[2]; + q13 = states[1]*states[3]; + q23 = states[2]*states[3]; + + Tbn.x.x = q00 + q11 - q22 - q33; + Tbn.y.y = q00 - q11 + q22 - q33; + Tbn.z.z = q00 - q11 - q22 + q33; + Tbn.x.y = 2*(q12 - q03); + Tbn.x.z = 2*(q13 + q02); + Tbn.y.x = 2*(q12 + q03); + Tbn.y.z = 2*(q23 - q01); + Tbn.z.x = 2*(q13 - q02); + Tbn.z.y = 2*(q23 + q01); + + Tnb = Tbn.transpose(); + + // transform body delta velocities to delta velocities in the nav frame + // * and + operators have been overloaded + //delVelNav = Tbn*dVelIMU + gravityNED*dtIMU; + delVelNav.x = Tbn.x.x*dVelIMU.x + Tbn.x.y*dVelIMU.y + Tbn.x.z*dVelIMU.z + gravityNED.x*dtIMU; + delVelNav.y = Tbn.y.x*dVelIMU.x + Tbn.y.y*dVelIMU.y + Tbn.y.z*dVelIMU.z + gravityNED.y*dtIMU; + delVelNav.z = Tbn.z.x*dVelIMU.x + Tbn.z.y*dVelIMU.y + Tbn.z.z*dVelIMU.z + gravityNED.z*dtIMU; + + // calculate the magnitude of the nav acceleration (required for GPS + // variance estimation) + accNavMag = delVelNav.length()/dtIMU; + + // If calculating position save previous velocity + lastVelocity[0] = states[4]; + lastVelocity[1] = states[5]; + lastVelocity[2] = states[6]; + + // Sum delta velocities to get velocity + states[4] = states[4] + delVelNav.x; + states[5] = states[5] + delVelNav.y; + states[6] = states[6] + delVelNav.z; + + // If calculating postions, do a trapezoidal integration for position + states[7] = states[7] + 0.5f*(states[4] + lastVelocity[0])*dtIMU; + states[8] = states[8] + 0.5f*(states[5] + lastVelocity[1])*dtIMU; + states[9] = states[9] + 0.5f*(states[6] + lastVelocity[2])*dtIMU; + +} + +void CovariancePrediction() +{ + // scalars + float windVelSigma; + float dAngBiasSigma; + float dVelBiasSigma; + float magEarthSigma; + float magBodySigma; + float daxCov; + float dayCov; + float dazCov; + float dvxCov; + float dvyCov; + float dvzCov; + float dvx; + float dvy; + float dvz; + float dax; + float day; + float daz; + float q0; + float q1; + float q2; + float q3; + float dax_b; + float day_b; + float daz_b; + + // arrays + float processNoise[n_states]; + float SF[14]; + float SG[8]; + float SQ[11]; + float SPP[13]; + float nextP[n_states][n_states]; + + // calculate covariance prediction process noise + const float yawVarScale = 1.0f; + windVelSigma = dt*0.1f; + dAngBiasSigma = dt*5.0e-7f; + magEarthSigma = dt*3.0e-4f; + magBodySigma = dt*3.0e-4f; + for (uint8_t i= 0; i<=9; i++) processNoise[i] = 1.0e-9f; + for (uint8_t i=10; i<=12; i++) processNoise[i] = dAngBiasSigma; + if (onGround) processNoise[12] = dAngBiasSigma * yawVarScale; + for (uint8_t i=13; i<=14; i++) processNoise[i] = windVelSigma; + for (uint8_t i=15; i<=17; i++) processNoise[i] = magEarthSigma; + for (uint8_t i=18; i<=20; i++) processNoise[i] = magBodySigma; + for (uint8_t i= 0; i<=20; i++) processNoise[i] = sq(processNoise[i]); + + // set variables used to calculate covariance growth + dvx = summedDelVel.x; + dvy = summedDelVel.y; + dvz = summedDelVel.z; + dax = summedDelAng.x; + day = summedDelAng.y; + daz = summedDelAng.z; + q0 = states[0]; + q1 = states[1]; + q2 = states[2]; + q3 = states[3]; + dax_b = states[10]; + day_b = states[11]; + daz_b = states[12]; + daxCov = sq(dt*1.4544411e-2f); + dayCov = sq(dt*1.4544411e-2f); + dazCov = sq(dt*1.4544411e-2f); + if (onGround) dazCov = dazCov * sq(yawVarScale); + dvxCov = sq(dt*0.5f); + dvyCov = sq(dt*0.5f); + dvzCov = sq(dt*0.5f); + + // Predicted covariance calculation + SF[0] = 2*dvx*q1 + 2*dvy*q2 + 2*dvz*q3; + SF[1] = 2*dvx*q3 + 2*dvy*q0 - 2*dvz*q1; + SF[2] = 2*dvx*q0 - 2*dvy*q3 + 2*dvz*q2; + SF[3] = day/2 - day_b/2; + SF[4] = daz/2 - daz_b/2; + SF[5] = dax/2 - dax_b/2; + SF[6] = dax_b/2 - dax/2; + SF[7] = daz_b/2 - daz/2; + SF[8] = day_b/2 - day/2; + SF[9] = q1/2; + SF[10] = q2/2; + SF[11] = q3/2; + SF[12] = 2*dvz*q0; + SF[13] = 2*dvy*q1; + + SG[0] = q0/2; + SG[1] = sq(q3); + SG[2] = sq(q2); + SG[3] = sq(q1); + SG[4] = sq(q0); + SG[5] = 2*q2*q3; + SG[6] = 2*q1*q3; + SG[7] = 2*q1*q2; + + SQ[0] = dvzCov*(SG[5] - 2*q0*q1)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvyCov*(SG[5] + 2*q0*q1)*(SG[1] - SG[2] + SG[3] - SG[4]) + dvxCov*(SG[6] - 2*q0*q2)*(SG[7] + 2*q0*q3); + SQ[1] = dvzCov*(SG[6] + 2*q0*q2)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvxCov*(SG[6] - 2*q0*q2)*(SG[1] + SG[2] - SG[3] - SG[4]) + dvyCov*(SG[5] + 2*q0*q1)*(SG[7] - 2*q0*q3); + SQ[2] = dvzCov*(SG[5] - 2*q0*q1)*(SG[6] + 2*q0*q2) - dvyCov*(SG[7] - 2*q0*q3)*(SG[1] - SG[2] + SG[3] - SG[4]) - dvxCov*(SG[7] + 2*q0*q3)*(SG[1] + SG[2] - SG[3] - SG[4]); + SQ[3] = (dayCov*q1*SG[0])/2 - (dazCov*q1*SG[0])/2 - (daxCov*q2*q3)/4; + SQ[4] = (dazCov*q2*SG[0])/2 - (daxCov*q2*SG[0])/2 - (dayCov*q1*q3)/4; + SQ[5] = (daxCov*q3*SG[0])/2 - (dayCov*q3*SG[0])/2 - (dazCov*q1*q2)/4; + SQ[6] = (daxCov*q1*q2)/4 - (dazCov*q3*SG[0])/2 - (dayCov*q1*q2)/4; + SQ[7] = (dazCov*q1*q3)/4 - (daxCov*q1*q3)/4 - (dayCov*q2*SG[0])/2; + SQ[8] = (dayCov*q2*q3)/4 - (daxCov*q1*SG[0])/2 - (dazCov*q2*q3)/4; + SQ[9] = sq(SG[0]); + SQ[10] = sq(q1); + + SPP[0] = SF[12] + SF[13] - 2*dvx*q2; + SPP[1] = 2*dvx*q0 - 2*dvy*q3 + 2*dvz*q2; + SPP[2] = 2*dvx*q3 + 2*dvy*q0 - 2*dvz*q1; + SPP[3] = SF[11]; + SPP[4] = SF[10]; + SPP[5] = SF[9]; + SPP[6] = SF[7]; + SPP[7] = SF[8]; + + nextP[0][0] = P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3] + (daxCov*SQ[10])/4 + SF[6]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) + SPP[7]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SPP[6]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[5]*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]) + SPP[4]*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]) + SPP[3]*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]) + (dayCov*sq(q2))/4 + (dazCov*sq(q3))/4; + nextP[0][1] = P[0][1] + SQ[8] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3] + SF[5]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[4]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SPP[7]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[3]*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]) - SPP[4]*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]) - (q0*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]))/2; + nextP[0][2] = P[0][2] + SQ[7] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3] + SF[3]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[5]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[6]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) - SPP[3]*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]) + SPP[5]*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]) - (q0*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]))/2; + nextP[0][3] = P[0][3] + SQ[6] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3] + SF[4]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[3]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) + SF[6]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SPP[4]*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]) - SPP[5]*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]) - (q0*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]))/2; + nextP[0][4] = P[0][4] + P[1][4]*SF[6] + P[2][4]*SPP[7] + P[3][4]*SPP[6] + P[10][4]*SPP[5] + P[11][4]*SPP[4] + P[12][4]*SPP[3] + SF[2]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[0]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) + SPP[0]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) - SPP[2]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]); + nextP[0][5] = P[0][5] + P[1][5]*SF[6] + P[2][5]*SPP[7] + P[3][5]*SPP[6] + P[10][5]*SPP[5] + P[11][5]*SPP[4] + P[12][5]*SPP[3] + SF[1]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[0]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SF[2]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) - SPP[0]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]); + nextP[0][6] = P[0][6] + P[1][6]*SF[6] + P[2][6]*SPP[7] + P[3][6]*SPP[6] + P[10][6]*SPP[5] + P[11][6]*SPP[4] + P[12][6]*SPP[3] + SF[1]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) + SF[0]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[0]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) - SPP[1]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]); + nextP[0][7] = P[0][7] + P[1][7]*SF[6] + P[2][7]*SPP[7] + P[3][7]*SPP[6] + P[10][7]*SPP[5] + P[11][7]*SPP[4] + P[12][7]*SPP[3] + dt*(P[0][4] + P[1][4]*SF[6] + P[2][4]*SPP[7] + P[3][4]*SPP[6] + P[10][4]*SPP[5] + P[11][4]*SPP[4] + P[12][4]*SPP[3]); + nextP[0][8] = P[0][8] + P[1][8]*SF[6] + P[2][8]*SPP[7] + P[3][8]*SPP[6] + P[10][8]*SPP[5] + P[11][8]*SPP[4] + P[12][8]*SPP[3] + dt*(P[0][5] + P[1][5]*SF[6] + P[2][5]*SPP[7] + P[3][5]*SPP[6] + P[10][5]*SPP[5] + P[11][5]*SPP[4] + P[12][5]*SPP[3]); + nextP[0][9] = P[0][9] + P[1][9]*SF[6] + P[2][9]*SPP[7] + P[3][9]*SPP[6] + P[10][9]*SPP[5] + P[11][9]*SPP[4] + P[12][9]*SPP[3] + dt*(P[0][6] + P[1][6]*SF[6] + P[2][6]*SPP[7] + P[3][6]*SPP[6] + P[10][6]*SPP[5] + P[11][6]*SPP[4] + P[12][6]*SPP[3]); + nextP[0][10] = P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]; + nextP[0][11] = P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]; + nextP[0][12] = P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]; + nextP[0][13] = P[0][13] + P[1][13]*SF[6] + P[2][13]*SPP[7] + P[3][13]*SPP[6] + P[10][13]*SPP[5] + P[11][13]*SPP[4] + P[12][13]*SPP[3]; + nextP[0][14] = P[0][14] + P[1][14]*SF[6] + P[2][14]*SPP[7] + P[3][14]*SPP[6] + P[10][14]*SPP[5] + P[11][14]*SPP[4] + P[12][14]*SPP[3]; + nextP[0][15] = P[0][15] + P[1][15]*SF[6] + P[2][15]*SPP[7] + P[3][15]*SPP[6] + P[10][15]*SPP[5] + P[11][15]*SPP[4] + P[12][15]*SPP[3]; + nextP[0][16] = P[0][16] + P[1][16]*SF[6] + P[2][16]*SPP[7] + P[3][16]*SPP[6] + P[10][16]*SPP[5] + P[11][16]*SPP[4] + P[12][16]*SPP[3]; + nextP[0][17] = P[0][17] + P[1][17]*SF[6] + P[2][17]*SPP[7] + P[3][17]*SPP[6] + P[10][17]*SPP[5] + P[11][17]*SPP[4] + P[12][17]*SPP[3]; + nextP[0][18] = P[0][18] + P[1][18]*SF[6] + P[2][18]*SPP[7] + P[3][18]*SPP[6] + P[10][18]*SPP[5] + P[11][18]*SPP[4] + P[12][18]*SPP[3]; + nextP[0][19] = P[0][19] + P[1][19]*SF[6] + P[2][19]*SPP[7] + P[3][19]*SPP[6] + P[10][19]*SPP[5] + P[11][19]*SPP[4] + P[12][19]*SPP[3]; + nextP[0][20] = P[0][20] + P[1][20]*SF[6] + P[2][20]*SPP[7] + P[3][20]*SPP[6] + P[10][20]*SPP[5] + P[11][20]*SPP[4] + P[12][20]*SPP[3]; + nextP[1][0] = P[1][0] + SQ[8] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2 + SF[6]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) + SPP[7]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) + SPP[6]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) + SPP[5]*(P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2) + SPP[4]*(P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2) + SPP[3]*(P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2); + nextP[1][1] = P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] + daxCov*SQ[9] - (P[10][1]*q0)/2 + SF[5]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[4]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) + SPP[7]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) + SPP[3]*(P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2) - SPP[4]*(P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2) + (dayCov*sq(q3))/4 + (dazCov*sq(q2))/4 - (q0*(P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2))/2; + nextP[1][2] = P[1][2] + SQ[5] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2 + SF[3]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[5]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) + SPP[6]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) - SPP[3]*(P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2) + SPP[5]*(P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2) - (q0*(P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2))/2; + nextP[1][3] = P[1][3] + SQ[4] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2 + SF[4]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[3]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) + SF[6]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) + SPP[4]*(P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2) - SPP[5]*(P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2) - (q0*(P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2))/2; + nextP[1][4] = P[1][4] + P[0][4]*SF[5] + P[2][4]*SF[4] + P[3][4]*SPP[7] + P[11][4]*SPP[3] - P[12][4]*SPP[4] - (P[10][4]*q0)/2 + SF[2]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[0]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) + SPP[0]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) - SPP[2]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2); + nextP[1][5] = P[1][5] + P[0][5]*SF[5] + P[2][5]*SF[4] + P[3][5]*SPP[7] + P[11][5]*SPP[3] - P[12][5]*SPP[4] - (P[10][5]*q0)/2 + SF[1]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[0]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) + SF[2]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) - SPP[0]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2); + nextP[1][6] = P[1][6] + P[0][6]*SF[5] + P[2][6]*SF[4] + P[3][6]*SPP[7] + P[11][6]*SPP[3] - P[12][6]*SPP[4] - (P[10][6]*q0)/2 + SF[1]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) + SF[0]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) + SPP[0]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) - SPP[1]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2); + nextP[1][7] = P[1][7] + P[0][7]*SF[5] + P[2][7]*SF[4] + P[3][7]*SPP[7] + P[11][7]*SPP[3] - P[12][7]*SPP[4] - (P[10][7]*q0)/2 + dt*(P[1][4] + P[0][4]*SF[5] + P[2][4]*SF[4] + P[3][4]*SPP[7] + P[11][4]*SPP[3] - P[12][4]*SPP[4] - (P[10][4]*q0)/2); + nextP[1][8] = P[1][8] + P[0][8]*SF[5] + P[2][8]*SF[4] + P[3][8]*SPP[7] + P[11][8]*SPP[3] - P[12][8]*SPP[4] - (P[10][8]*q0)/2 + dt*(P[1][5] + P[0][5]*SF[5] + P[2][5]*SF[4] + P[3][5]*SPP[7] + P[11][5]*SPP[3] - P[12][5]*SPP[4] - (P[10][5]*q0)/2); + nextP[1][9] = P[1][9] + P[0][9]*SF[5] + P[2][9]*SF[4] + P[3][9]*SPP[7] + P[11][9]*SPP[3] - P[12][9]*SPP[4] - (P[10][9]*q0)/2 + dt*(P[1][6] + P[0][6]*SF[5] + P[2][6]*SF[4] + P[3][6]*SPP[7] + P[11][6]*SPP[3] - P[12][6]*SPP[4] - (P[10][6]*q0)/2); + nextP[1][10] = P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2; + nextP[1][11] = P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2; + nextP[1][12] = P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2; + nextP[1][13] = P[1][13] + P[0][13]*SF[5] + P[2][13]*SF[4] + P[3][13]*SPP[7] + P[11][13]*SPP[3] - P[12][13]*SPP[4] - (P[10][13]*q0)/2; + nextP[1][14] = P[1][14] + P[0][14]*SF[5] + P[2][14]*SF[4] + P[3][14]*SPP[7] + P[11][14]*SPP[3] - P[12][14]*SPP[4] - (P[10][14]*q0)/2; + nextP[1][15] = P[1][15] + P[0][15]*SF[5] + P[2][15]*SF[4] + P[3][15]*SPP[7] + P[11][15]*SPP[3] - P[12][15]*SPP[4] - (P[10][15]*q0)/2; + nextP[1][16] = P[1][16] + P[0][16]*SF[5] + P[2][16]*SF[4] + P[3][16]*SPP[7] + P[11][16]*SPP[3] - P[12][16]*SPP[4] - (P[10][16]*q0)/2; + nextP[1][17] = P[1][17] + P[0][17]*SF[5] + P[2][17]*SF[4] + P[3][17]*SPP[7] + P[11][17]*SPP[3] - P[12][17]*SPP[4] - (P[10][17]*q0)/2; + nextP[1][18] = P[1][18] + P[0][18]*SF[5] + P[2][18]*SF[4] + P[3][18]*SPP[7] + P[11][18]*SPP[3] - P[12][18]*SPP[4] - (P[10][18]*q0)/2; + nextP[1][19] = P[1][19] + P[0][19]*SF[5] + P[2][19]*SF[4] + P[3][19]*SPP[7] + P[11][19]*SPP[3] - P[12][19]*SPP[4] - (P[10][19]*q0)/2; + nextP[1][20] = P[1][20] + P[0][20]*SF[5] + P[2][20]*SF[4] + P[3][20]*SPP[7] + P[11][20]*SPP[3] - P[12][20]*SPP[4] - (P[10][20]*q0)/2; + nextP[2][0] = P[2][0] + SQ[7] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2 + SF[6]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) + SPP[7]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) + SPP[6]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) + SPP[5]*(P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2) + SPP[4]*(P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2) + SPP[3]*(P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2); + nextP[2][1] = P[2][1] + SQ[5] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2 + SF[5]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[4]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) + SPP[7]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) + SPP[3]*(P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2) - SPP[4]*(P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2) - (q0*(P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2))/2; + nextP[2][2] = P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] + dayCov*SQ[9] + (dazCov*SQ[10])/4 - (P[11][2]*q0)/2 + SF[3]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[5]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) + SPP[6]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) - SPP[3]*(P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2) + SPP[5]*(P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2) + (daxCov*sq(q3))/4 - (q0*(P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2))/2; + nextP[2][3] = P[2][3] + SQ[3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2 + SF[4]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[3]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) + SF[6]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) + SPP[4]*(P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2) - SPP[5]*(P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2) - (q0*(P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2))/2; + nextP[2][4] = P[2][4] + P[0][4]*SF[3] + P[3][4]*SF[5] + P[1][4]*SPP[6] - P[10][4]*SPP[3] + P[12][4]*SPP[5] - (P[11][4]*q0)/2 + SF[2]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[0]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) + SPP[0]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) - SPP[2]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2); + nextP[2][5] = P[2][5] + P[0][5]*SF[3] + P[3][5]*SF[5] + P[1][5]*SPP[6] - P[10][5]*SPP[3] + P[12][5]*SPP[5] - (P[11][5]*q0)/2 + SF[1]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[0]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) + SF[2]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) - SPP[0]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2); + nextP[2][6] = P[2][6] + P[0][6]*SF[3] + P[3][6]*SF[5] + P[1][6]*SPP[6] - P[10][6]*SPP[3] + P[12][6]*SPP[5] - (P[11][6]*q0)/2 + SF[1]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) + SF[0]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) + SPP[0]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) - SPP[1]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2); + nextP[2][7] = P[2][7] + P[0][7]*SF[3] + P[3][7]*SF[5] + P[1][7]*SPP[6] - P[10][7]*SPP[3] + P[12][7]*SPP[5] - (P[11][7]*q0)/2 + dt*(P[2][4] + P[0][4]*SF[3] + P[3][4]*SF[5] + P[1][4]*SPP[6] - P[10][4]*SPP[3] + P[12][4]*SPP[5] - (P[11][4]*q0)/2); + nextP[2][8] = P[2][8] + P[0][8]*SF[3] + P[3][8]*SF[5] + P[1][8]*SPP[6] - P[10][8]*SPP[3] + P[12][8]*SPP[5] - (P[11][8]*q0)/2 + dt*(P[2][5] + P[0][5]*SF[3] + P[3][5]*SF[5] + P[1][5]*SPP[6] - P[10][5]*SPP[3] + P[12][5]*SPP[5] - (P[11][5]*q0)/2); + nextP[2][9] = P[2][9] + P[0][9]*SF[3] + P[3][9]*SF[5] + P[1][9]*SPP[6] - P[10][9]*SPP[3] + P[12][9]*SPP[5] - (P[11][9]*q0)/2 + dt*(P[2][6] + P[0][6]*SF[3] + P[3][6]*SF[5] + P[1][6]*SPP[6] - P[10][6]*SPP[3] + P[12][6]*SPP[5] - (P[11][6]*q0)/2); + nextP[2][10] = P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2; + nextP[2][11] = P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2; + nextP[2][12] = P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2; + nextP[2][13] = P[2][13] + P[0][13]*SF[3] + P[3][13]*SF[5] + P[1][13]*SPP[6] - P[10][13]*SPP[3] + P[12][13]*SPP[5] - (P[11][13]*q0)/2; + nextP[2][14] = P[2][14] + P[0][14]*SF[3] + P[3][14]*SF[5] + P[1][14]*SPP[6] - P[10][14]*SPP[3] + P[12][14]*SPP[5] - (P[11][14]*q0)/2; + nextP[2][15] = P[2][15] + P[0][15]*SF[3] + P[3][15]*SF[5] + P[1][15]*SPP[6] - P[10][15]*SPP[3] + P[12][15]*SPP[5] - (P[11][15]*q0)/2; + nextP[2][16] = P[2][16] + P[0][16]*SF[3] + P[3][16]*SF[5] + P[1][16]*SPP[6] - P[10][16]*SPP[3] + P[12][16]*SPP[5] - (P[11][16]*q0)/2; + nextP[2][17] = P[2][17] + P[0][17]*SF[3] + P[3][17]*SF[5] + P[1][17]*SPP[6] - P[10][17]*SPP[3] + P[12][17]*SPP[5] - (P[11][17]*q0)/2; + nextP[2][18] = P[2][18] + P[0][18]*SF[3] + P[3][18]*SF[5] + P[1][18]*SPP[6] - P[10][18]*SPP[3] + P[12][18]*SPP[5] - (P[11][18]*q0)/2; + nextP[2][19] = P[2][19] + P[0][19]*SF[3] + P[3][19]*SF[5] + P[1][19]*SPP[6] - P[10][19]*SPP[3] + P[12][19]*SPP[5] - (P[11][19]*q0)/2; + nextP[2][20] = P[2][20] + P[0][20]*SF[3] + P[3][20]*SF[5] + P[1][20]*SPP[6] - P[10][20]*SPP[3] + P[12][20]*SPP[5] - (P[11][20]*q0)/2; + nextP[3][0] = P[3][0] + SQ[6] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2 + SF[6]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) + SPP[7]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) + SPP[6]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) + SPP[5]*(P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2) + SPP[4]*(P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2) + SPP[3]*(P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2); + nextP[3][1] = P[3][1] + SQ[4] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2 + SF[5]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[4]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) + SPP[7]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) + SPP[3]*(P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2) - SPP[4]*(P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2) - (q0*(P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2))/2; + nextP[3][2] = P[3][2] + SQ[3] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2 + SF[3]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[5]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) + SPP[6]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) - SPP[3]*(P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2) + SPP[5]*(P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2) - (q0*(P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2))/2; + nextP[3][3] = P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] + (dayCov*SQ[10])/4 + dazCov*SQ[9] - (P[12][3]*q0)/2 + SF[4]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[3]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) + SF[6]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) + SPP[4]*(P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2) - SPP[5]*(P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2) + (daxCov*sq(q2))/4 - (q0*(P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2))/2; + nextP[3][4] = P[3][4] + P[0][4]*SF[4] + P[1][4]*SF[3] + P[2][4]*SF[6] + P[10][4]*SPP[4] - P[11][4]*SPP[5] - (P[12][4]*q0)/2 + SF[2]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[0]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) + SPP[0]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) - SPP[2]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2); + nextP[3][5] = P[3][5] + P[0][5]*SF[4] + P[1][5]*SF[3] + P[2][5]*SF[6] + P[10][5]*SPP[4] - P[11][5]*SPP[5] - (P[12][5]*q0)/2 + SF[1]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[0]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) + SF[2]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) - SPP[0]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2); + nextP[3][6] = P[3][6] + P[0][6]*SF[4] + P[1][6]*SF[3] + P[2][6]*SF[6] + P[10][6]*SPP[4] - P[11][6]*SPP[5] - (P[12][6]*q0)/2 + SF[1]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) + SF[0]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) + SPP[0]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) - SPP[1]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2); + nextP[3][7] = P[3][7] + P[0][7]*SF[4] + P[1][7]*SF[3] + P[2][7]*SF[6] + P[10][7]*SPP[4] - P[11][7]*SPP[5] - (P[12][7]*q0)/2 + dt*(P[3][4] + P[0][4]*SF[4] + P[1][4]*SF[3] + P[2][4]*SF[6] + P[10][4]*SPP[4] - P[11][4]*SPP[5] - (P[12][4]*q0)/2); + nextP[3][8] = P[3][8] + P[0][8]*SF[4] + P[1][8]*SF[3] + P[2][8]*SF[6] + P[10][8]*SPP[4] - P[11][8]*SPP[5] - (P[12][8]*q0)/2 + dt*(P[3][5] + P[0][5]*SF[4] + P[1][5]*SF[3] + P[2][5]*SF[6] + P[10][5]*SPP[4] - P[11][5]*SPP[5] - (P[12][5]*q0)/2); + nextP[3][9] = P[3][9] + P[0][9]*SF[4] + P[1][9]*SF[3] + P[2][9]*SF[6] + P[10][9]*SPP[4] - P[11][9]*SPP[5] - (P[12][9]*q0)/2 + dt*(P[3][6] + P[0][6]*SF[4] + P[1][6]*SF[3] + P[2][6]*SF[6] + P[10][6]*SPP[4] - P[11][6]*SPP[5] - (P[12][6]*q0)/2); + nextP[3][10] = P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2; + nextP[3][11] = P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2; + nextP[3][12] = P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2; + nextP[3][13] = P[3][13] + P[0][13]*SF[4] + P[1][13]*SF[3] + P[2][13]*SF[6] + P[10][13]*SPP[4] - P[11][13]*SPP[5] - (P[12][13]*q0)/2; + nextP[3][14] = P[3][14] + P[0][14]*SF[4] + P[1][14]*SF[3] + P[2][14]*SF[6] + P[10][14]*SPP[4] - P[11][14]*SPP[5] - (P[12][14]*q0)/2; + nextP[3][15] = P[3][15] + P[0][15]*SF[4] + P[1][15]*SF[3] + P[2][15]*SF[6] + P[10][15]*SPP[4] - P[11][15]*SPP[5] - (P[12][15]*q0)/2; + nextP[3][16] = P[3][16] + P[0][16]*SF[4] + P[1][16]*SF[3] + P[2][16]*SF[6] + P[10][16]*SPP[4] - P[11][16]*SPP[5] - (P[12][16]*q0)/2; + nextP[3][17] = P[3][17] + P[0][17]*SF[4] + P[1][17]*SF[3] + P[2][17]*SF[6] + P[10][17]*SPP[4] - P[11][17]*SPP[5] - (P[12][17]*q0)/2; + nextP[3][18] = P[3][18] + P[0][18]*SF[4] + P[1][18]*SF[3] + P[2][18]*SF[6] + P[10][18]*SPP[4] - P[11][18]*SPP[5] - (P[12][18]*q0)/2; + nextP[3][19] = P[3][19] + P[0][19]*SF[4] + P[1][19]*SF[3] + P[2][19]*SF[6] + P[10][19]*SPP[4] - P[11][19]*SPP[5] - (P[12][19]*q0)/2; + nextP[3][20] = P[3][20] + P[0][20]*SF[4] + P[1][20]*SF[3] + P[2][20]*SF[6] + P[10][20]*SPP[4] - P[11][20]*SPP[5] - (P[12][20]*q0)/2; + nextP[4][0] = P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2] + SF[6]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) + SPP[7]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) + SPP[6]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + SPP[5]*(P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]) + SPP[4]*(P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]) + SPP[3]*(P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]); + nextP[4][1] = P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2] + SF[5]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[4]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) + SPP[7]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + SPP[3]*(P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]) - SPP[4]*(P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]) - (q0*(P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]))/2; + nextP[4][2] = P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2] + SF[3]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[5]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + SPP[6]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) - SPP[3]*(P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]) + SPP[5]*(P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]) - (q0*(P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]))/2; + nextP[4][3] = P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2] + SF[4]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[3]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) + SF[6]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) + SPP[4]*(P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]) - SPP[5]*(P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]) - (q0*(P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]))/2; + nextP[4][4] = P[4][4] + P[0][4]*SF[2] + P[1][4]*SF[0] + P[2][4]*SPP[0] - P[3][4]*SPP[2] + dvyCov*sq(SG[7] - 2*q0*q3) + dvzCov*sq(SG[6] + 2*q0*q2) + SF[2]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[0]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) + SPP[0]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) - SPP[2]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + dvxCov*sq(SG[1] + SG[2] - SG[3] - SG[4]); + nextP[4][5] = P[4][5] + SQ[2] + P[0][5]*SF[2] + P[1][5]*SF[0] + P[2][5]*SPP[0] - P[3][5]*SPP[2] + SF[1]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[0]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) + SF[2]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) - SPP[0]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]); + nextP[4][6] = P[4][6] + SQ[1] + P[0][6]*SF[2] + P[1][6]*SF[0] + P[2][6]*SPP[0] - P[3][6]*SPP[2] + SF[1]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) + SF[0]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + SPP[0]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) - SPP[1]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]); + nextP[4][7] = P[4][7] + P[0][7]*SF[2] + P[1][7]*SF[0] + P[2][7]*SPP[0] - P[3][7]*SPP[2] + dt*(P[4][4] + P[0][4]*SF[2] + P[1][4]*SF[0] + P[2][4]*SPP[0] - P[3][4]*SPP[2]); + nextP[4][8] = P[4][8] + P[0][8]*SF[2] + P[1][8]*SF[0] + P[2][8]*SPP[0] - P[3][8]*SPP[2] + dt*(P[4][5] + P[0][5]*SF[2] + P[1][5]*SF[0] + P[2][5]*SPP[0] - P[3][5]*SPP[2]); + nextP[4][9] = P[4][9] + P[0][9]*SF[2] + P[1][9]*SF[0] + P[2][9]*SPP[0] - P[3][9]*SPP[2] + dt*(P[4][6] + P[0][6]*SF[2] + P[1][6]*SF[0] + P[2][6]*SPP[0] - P[3][6]*SPP[2]); + nextP[4][10] = P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]; + nextP[4][11] = P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]; + nextP[4][12] = P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]; + nextP[4][13] = P[4][13] + P[0][13]*SF[2] + P[1][13]*SF[0] + P[2][13]*SPP[0] - P[3][13]*SPP[2]; + nextP[4][14] = P[4][14] + P[0][14]*SF[2] + P[1][14]*SF[0] + P[2][14]*SPP[0] - P[3][14]*SPP[2]; + nextP[4][15] = P[4][15] + P[0][15]*SF[2] + P[1][15]*SF[0] + P[2][15]*SPP[0] - P[3][15]*SPP[2]; + nextP[4][16] = P[4][16] + P[0][16]*SF[2] + P[1][16]*SF[0] + P[2][16]*SPP[0] - P[3][16]*SPP[2]; + nextP[4][17] = P[4][17] + P[0][17]*SF[2] + P[1][17]*SF[0] + P[2][17]*SPP[0] - P[3][17]*SPP[2]; + nextP[4][18] = P[4][18] + P[0][18]*SF[2] + P[1][18]*SF[0] + P[2][18]*SPP[0] - P[3][18]*SPP[2]; + nextP[4][19] = P[4][19] + P[0][19]*SF[2] + P[1][19]*SF[0] + P[2][19]*SPP[0] - P[3][19]*SPP[2]; + nextP[4][20] = P[4][20] + P[0][20]*SF[2] + P[1][20]*SF[0] + P[2][20]*SPP[0] - P[3][20]*SPP[2]; + nextP[5][0] = P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0] + SF[6]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + SPP[7]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) + SPP[6]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) + SPP[5]*(P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]) + SPP[4]*(P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]) + SPP[3]*(P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]); + nextP[5][1] = P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0] + SF[5]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[4]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) + SPP[7]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) + SPP[3]*(P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]) - SPP[4]*(P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]) - (q0*(P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]))/2; + nextP[5][2] = P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0] + SF[3]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[5]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) + SPP[6]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) - SPP[3]*(P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]) + SPP[5]*(P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]) - (q0*(P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]))/2; + nextP[5][3] = P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0] + SF[4]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[3]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + SF[6]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) + SPP[4]*(P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]) - SPP[5]*(P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]) - (q0*(P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]))/2; + nextP[5][4] = P[5][4] + SQ[2] + P[0][4]*SF[1] + P[2][4]*SF[0] + P[3][4]*SF[2] - P[1][4]*SPP[0] + SF[2]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[0]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + SPP[0]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) - SPP[2]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]); + nextP[5][5] = P[5][5] + P[0][5]*SF[1] + P[2][5]*SF[0] + P[3][5]*SF[2] - P[1][5]*SPP[0] + dvxCov*sq(SG[7] + 2*q0*q3) + dvzCov*sq(SG[5] - 2*q0*q1) + SF[1]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[0]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) + SF[2]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) - SPP[0]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + dvyCov*sq(SG[1] - SG[2] + SG[3] - SG[4]); + nextP[5][6] = P[5][6] + SQ[0] + P[0][6]*SF[1] + P[2][6]*SF[0] + P[3][6]*SF[2] - P[1][6]*SPP[0] + SF[1]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + SF[0]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) + SPP[0]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) - SPP[1]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]); + nextP[5][7] = P[5][7] + P[0][7]*SF[1] + P[2][7]*SF[0] + P[3][7]*SF[2] - P[1][7]*SPP[0] + dt*(P[5][4] + P[0][4]*SF[1] + P[2][4]*SF[0] + P[3][4]*SF[2] - P[1][4]*SPP[0]); + nextP[5][8] = P[5][8] + P[0][8]*SF[1] + P[2][8]*SF[0] + P[3][8]*SF[2] - P[1][8]*SPP[0] + dt*(P[5][5] + P[0][5]*SF[1] + P[2][5]*SF[0] + P[3][5]*SF[2] - P[1][5]*SPP[0]); + nextP[5][9] = P[5][9] + P[0][9]*SF[1] + P[2][9]*SF[0] + P[3][9]*SF[2] - P[1][9]*SPP[0] + dt*(P[5][6] + P[0][6]*SF[1] + P[2][6]*SF[0] + P[3][6]*SF[2] - P[1][6]*SPP[0]); + nextP[5][10] = P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]; + nextP[5][11] = P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]; + nextP[5][12] = P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]; + nextP[5][13] = P[5][13] + P[0][13]*SF[1] + P[2][13]*SF[0] + P[3][13]*SF[2] - P[1][13]*SPP[0]; + nextP[5][14] = P[5][14] + P[0][14]*SF[1] + P[2][14]*SF[0] + P[3][14]*SF[2] - P[1][14]*SPP[0]; + nextP[5][15] = P[5][15] + P[0][15]*SF[1] + P[2][15]*SF[0] + P[3][15]*SF[2] - P[1][15]*SPP[0]; + nextP[5][16] = P[5][16] + P[0][16]*SF[1] + P[2][16]*SF[0] + P[3][16]*SF[2] - P[1][16]*SPP[0]; + nextP[5][17] = P[5][17] + P[0][17]*SF[1] + P[2][17]*SF[0] + P[3][17]*SF[2] - P[1][17]*SPP[0]; + nextP[5][18] = P[5][18] + P[0][18]*SF[1] + P[2][18]*SF[0] + P[3][18]*SF[2] - P[1][18]*SPP[0]; + nextP[5][19] = P[5][19] + P[0][19]*SF[1] + P[2][19]*SF[0] + P[3][19]*SF[2] - P[1][19]*SPP[0]; + nextP[5][20] = P[5][20] + P[0][20]*SF[1] + P[2][20]*SF[0] + P[3][20]*SF[2] - P[1][20]*SPP[0]; + nextP[6][0] = P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1] + SF[6]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) + SPP[7]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + SPP[6]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) + SPP[5]*(P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]) + SPP[4]*(P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]) + SPP[3]*(P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]); + nextP[6][1] = P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1] + SF[5]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[4]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + SPP[7]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) + SPP[3]*(P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]) - SPP[4]*(P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]) - (q0*(P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]))/2; + nextP[6][2] = P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1] + SF[3]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[5]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) + SPP[6]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) - SPP[3]*(P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]) + SPP[5]*(P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]) - (q0*(P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]))/2; + nextP[6][3] = P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1] + SF[4]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[3]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) + SF[6]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + SPP[4]*(P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]) - SPP[5]*(P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]) - (q0*(P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]))/2; + nextP[6][4] = P[6][4] + SQ[1] + P[1][4]*SF[1] + P[3][4]*SF[0] + P[0][4]*SPP[0] - P[2][4]*SPP[1] + SF[2]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[0]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) + SPP[0]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) - SPP[2]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]); + nextP[6][5] = P[6][5] + SQ[0] + P[1][5]*SF[1] + P[3][5]*SF[0] + P[0][5]*SPP[0] - P[2][5]*SPP[1] + SF[1]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[0]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + SF[2]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) - SPP[0]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]); + nextP[6][6] = P[6][6] + P[1][6]*SF[1] + P[3][6]*SF[0] + P[0][6]*SPP[0] - P[2][6]*SPP[1] + dvxCov*sq(SG[6] - 2*q0*q2) + dvyCov*sq(SG[5] + 2*q0*q1) + SF[1]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) + SF[0]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) + SPP[0]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) - SPP[1]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + dvzCov*sq(SG[1] - SG[2] - SG[3] + SG[4]); + nextP[6][7] = P[6][7] + P[1][7]*SF[1] + P[3][7]*SF[0] + P[0][7]*SPP[0] - P[2][7]*SPP[1] + dt*(P[6][4] + P[1][4]*SF[1] + P[3][4]*SF[0] + P[0][4]*SPP[0] - P[2][4]*SPP[1]); + nextP[6][8] = P[6][8] + P[1][8]*SF[1] + P[3][8]*SF[0] + P[0][8]*SPP[0] - P[2][8]*SPP[1] + dt*(P[6][5] + P[1][5]*SF[1] + P[3][5]*SF[0] + P[0][5]*SPP[0] - P[2][5]*SPP[1]); + nextP[6][9] = P[6][9] + P[1][9]*SF[1] + P[3][9]*SF[0] + P[0][9]*SPP[0] - P[2][9]*SPP[1] + dt*(P[6][6] + P[1][6]*SF[1] + P[3][6]*SF[0] + P[0][6]*SPP[0] - P[2][6]*SPP[1]); + nextP[6][10] = P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]; + nextP[6][11] = P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]; + nextP[6][12] = P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]; + nextP[6][13] = P[6][13] + P[1][13]*SF[1] + P[3][13]*SF[0] + P[0][13]*SPP[0] - P[2][13]*SPP[1]; + nextP[6][14] = P[6][14] + P[1][14]*SF[1] + P[3][14]*SF[0] + P[0][14]*SPP[0] - P[2][14]*SPP[1]; + nextP[6][15] = P[6][15] + P[1][15]*SF[1] + P[3][15]*SF[0] + P[0][15]*SPP[0] - P[2][15]*SPP[1]; + nextP[6][16] = P[6][16] + P[1][16]*SF[1] + P[3][16]*SF[0] + P[0][16]*SPP[0] - P[2][16]*SPP[1]; + nextP[6][17] = P[6][17] + P[1][17]*SF[1] + P[3][17]*SF[0] + P[0][17]*SPP[0] - P[2][17]*SPP[1]; + nextP[6][18] = P[6][18] + P[1][18]*SF[1] + P[3][18]*SF[0] + P[0][18]*SPP[0] - P[2][18]*SPP[1]; + nextP[6][19] = P[6][19] + P[1][19]*SF[1] + P[3][19]*SF[0] + P[0][19]*SPP[0] - P[2][19]*SPP[1]; + nextP[6][20] = P[6][20] + P[1][20]*SF[1] + P[3][20]*SF[0] + P[0][20]*SPP[0] - P[2][20]*SPP[1]; + nextP[7][0] = P[7][0] + P[4][0]*dt + SF[6]*(P[7][1] + P[4][1]*dt) + SPP[7]*(P[7][2] + P[4][2]*dt) + SPP[6]*(P[7][3] + P[4][3]*dt) + SPP[5]*(P[7][10] + P[4][10]*dt) + SPP[4]*(P[7][11] + P[4][11]*dt) + SPP[3]*(P[7][12] + P[4][12]*dt); + nextP[7][1] = P[7][1] + P[4][1]*dt + SF[5]*(P[7][0] + P[4][0]*dt) + SF[4]*(P[7][2] + P[4][2]*dt) + SPP[7]*(P[7][3] + P[4][3]*dt) + SPP[3]*(P[7][11] + P[4][11]*dt) - SPP[4]*(P[7][12] + P[4][12]*dt) - (q0*(P[7][10] + P[4][10]*dt))/2; + nextP[7][2] = P[7][2] + P[4][2]*dt + SF[3]*(P[7][0] + P[4][0]*dt) + SF[5]*(P[7][3] + P[4][3]*dt) + SPP[6]*(P[7][1] + P[4][1]*dt) - SPP[3]*(P[7][10] + P[4][10]*dt) + SPP[5]*(P[7][12] + P[4][12]*dt) - (q0*(P[7][11] + P[4][11]*dt))/2; + nextP[7][3] = P[7][3] + P[4][3]*dt + SF[4]*(P[7][0] + P[4][0]*dt) + SF[3]*(P[7][1] + P[4][1]*dt) + SF[6]*(P[7][2] + P[4][2]*dt) + SPP[4]*(P[7][10] + P[4][10]*dt) - SPP[5]*(P[7][11] + P[4][11]*dt) - (q0*(P[7][12] + P[4][12]*dt))/2; + nextP[7][4] = P[7][4] + P[4][4]*dt + SF[0]*(P[7][1] + P[4][1]*dt) + SF[2]*(P[7][0] + P[4][0]*dt) + SPP[0]*(P[7][2] + P[4][2]*dt) - SPP[2]*(P[7][3] + P[4][3]*dt); + nextP[7][5] = P[7][5] + P[4][5]*dt + SF[1]*(P[7][0] + P[4][0]*dt) + SF[0]*(P[7][2] + P[4][2]*dt) + SF[2]*(P[7][3] + P[4][3]*dt) - SPP[0]*(P[7][1] + P[4][1]*dt); + nextP[7][6] = P[7][6] + P[4][6]*dt + SF[1]*(P[7][1] + P[4][1]*dt) + SF[0]*(P[7][3] + P[4][3]*dt) + SPP[0]*(P[7][0] + P[4][0]*dt) - SPP[1]*(P[7][2] + P[4][2]*dt); + nextP[7][7] = P[7][7] + P[4][7]*dt + dt*(P[7][4] + P[4][4]*dt); + nextP[7][8] = P[7][8] + P[4][8]*dt + dt*(P[7][5] + P[4][5]*dt); + nextP[7][9] = P[7][9] + P[4][9]*dt + dt*(P[7][6] + P[4][6]*dt); + nextP[7][10] = P[7][10] + P[4][10]*dt; + nextP[7][11] = P[7][11] + P[4][11]*dt; + nextP[7][12] = P[7][12] + P[4][12]*dt; + nextP[7][13] = P[7][13] + P[4][13]*dt; + nextP[7][14] = P[7][14] + P[4][14]*dt; + nextP[7][15] = P[7][15] + P[4][15]*dt; + nextP[7][16] = P[7][16] + P[4][16]*dt; + nextP[7][17] = P[7][17] + P[4][17]*dt; + nextP[7][18] = P[7][18] + P[4][18]*dt; + nextP[7][19] = P[7][19] + P[4][19]*dt; + nextP[7][20] = P[7][20] + P[4][20]*dt; + nextP[8][0] = P[8][0] + P[5][0]*dt + SF[6]*(P[8][1] + P[5][1]*dt) + SPP[7]*(P[8][2] + P[5][2]*dt) + SPP[6]*(P[8][3] + P[5][3]*dt) + SPP[5]*(P[8][10] + P[5][10]*dt) + SPP[4]*(P[8][11] + P[5][11]*dt) + SPP[3]*(P[8][12] + P[5][12]*dt); + nextP[8][1] = P[8][1] + P[5][1]*dt + SF[5]*(P[8][0] + P[5][0]*dt) + SF[4]*(P[8][2] + P[5][2]*dt) + SPP[7]*(P[8][3] + P[5][3]*dt) + SPP[3]*(P[8][11] + P[5][11]*dt) - SPP[4]*(P[8][12] + P[5][12]*dt) - (q0*(P[8][10] + P[5][10]*dt))/2; + nextP[8][2] = P[8][2] + P[5][2]*dt + SF[3]*(P[8][0] + P[5][0]*dt) + SF[5]*(P[8][3] + P[5][3]*dt) + SPP[6]*(P[8][1] + P[5][1]*dt) - SPP[3]*(P[8][10] + P[5][10]*dt) + SPP[5]*(P[8][12] + P[5][12]*dt) - (q0*(P[8][11] + P[5][11]*dt))/2; + nextP[8][3] = P[8][3] + P[5][3]*dt + SF[4]*(P[8][0] + P[5][0]*dt) + SF[3]*(P[8][1] + P[5][1]*dt) + SF[6]*(P[8][2] + P[5][2]*dt) + SPP[4]*(P[8][10] + P[5][10]*dt) - SPP[5]*(P[8][11] + P[5][11]*dt) - (q0*(P[8][12] + P[5][12]*dt))/2; + nextP[8][4] = P[8][4] + P[5][4]*dt + SF[0]*(P[8][1] + P[5][1]*dt) + SF[2]*(P[8][0] + P[5][0]*dt) + SPP[0]*(P[8][2] + P[5][2]*dt) - SPP[2]*(P[8][3] + P[5][3]*dt); + nextP[8][5] = P[8][5] + P[5][5]*dt + SF[1]*(P[8][0] + P[5][0]*dt) + SF[0]*(P[8][2] + P[5][2]*dt) + SF[2]*(P[8][3] + P[5][3]*dt) - SPP[0]*(P[8][1] + P[5][1]*dt); + nextP[8][6] = P[8][6] + P[5][6]*dt + SF[1]*(P[8][1] + P[5][1]*dt) + SF[0]*(P[8][3] + P[5][3]*dt) + SPP[0]*(P[8][0] + P[5][0]*dt) - SPP[1]*(P[8][2] + P[5][2]*dt); + nextP[8][7] = P[8][7] + P[5][7]*dt + dt*(P[8][4] + P[5][4]*dt); + nextP[8][8] = P[8][8] + P[5][8]*dt + dt*(P[8][5] + P[5][5]*dt); + nextP[8][9] = P[8][9] + P[5][9]*dt + dt*(P[8][6] + P[5][6]*dt); + nextP[8][10] = P[8][10] + P[5][10]*dt; + nextP[8][11] = P[8][11] + P[5][11]*dt; + nextP[8][12] = P[8][12] + P[5][12]*dt; + nextP[8][13] = P[8][13] + P[5][13]*dt; + nextP[8][14] = P[8][14] + P[5][14]*dt; + nextP[8][15] = P[8][15] + P[5][15]*dt; + nextP[8][16] = P[8][16] + P[5][16]*dt; + nextP[8][17] = P[8][17] + P[5][17]*dt; + nextP[8][18] = P[8][18] + P[5][18]*dt; + nextP[8][19] = P[8][19] + P[5][19]*dt; + nextP[8][20] = P[8][20] + P[5][20]*dt; + nextP[9][0] = P[9][0] + P[6][0]*dt + SF[6]*(P[9][1] + P[6][1]*dt) + SPP[7]*(P[9][2] + P[6][2]*dt) + SPP[6]*(P[9][3] + P[6][3]*dt) + SPP[5]*(P[9][10] + P[6][10]*dt) + SPP[4]*(P[9][11] + P[6][11]*dt) + SPP[3]*(P[9][12] + P[6][12]*dt); + nextP[9][1] = P[9][1] + P[6][1]*dt + SF[5]*(P[9][0] + P[6][0]*dt) + SF[4]*(P[9][2] + P[6][2]*dt) + SPP[7]*(P[9][3] + P[6][3]*dt) + SPP[3]*(P[9][11] + P[6][11]*dt) - SPP[4]*(P[9][12] + P[6][12]*dt) - (q0*(P[9][10] + P[6][10]*dt))/2; + nextP[9][2] = P[9][2] + P[6][2]*dt + SF[3]*(P[9][0] + P[6][0]*dt) + SF[5]*(P[9][3] + P[6][3]*dt) + SPP[6]*(P[9][1] + P[6][1]*dt) - SPP[3]*(P[9][10] + P[6][10]*dt) + SPP[5]*(P[9][12] + P[6][12]*dt) - (q0*(P[9][11] + P[6][11]*dt))/2; + nextP[9][3] = P[9][3] + P[6][3]*dt + SF[4]*(P[9][0] + P[6][0]*dt) + SF[3]*(P[9][1] + P[6][1]*dt) + SF[6]*(P[9][2] + P[6][2]*dt) + SPP[4]*(P[9][10] + P[6][10]*dt) - SPP[5]*(P[9][11] + P[6][11]*dt) - (q0*(P[9][12] + P[6][12]*dt))/2; + nextP[9][4] = P[9][4] + P[6][4]*dt + SF[0]*(P[9][1] + P[6][1]*dt) + SF[2]*(P[9][0] + P[6][0]*dt) + SPP[0]*(P[9][2] + P[6][2]*dt) - SPP[2]*(P[9][3] + P[6][3]*dt); + nextP[9][5] = P[9][5] + P[6][5]*dt + SF[1]*(P[9][0] + P[6][0]*dt) + SF[0]*(P[9][2] + P[6][2]*dt) + SF[2]*(P[9][3] + P[6][3]*dt) - SPP[0]*(P[9][1] + P[6][1]*dt); + nextP[9][6] = P[9][6] + P[6][6]*dt + SF[1]*(P[9][1] + P[6][1]*dt) + SF[0]*(P[9][3] + P[6][3]*dt) + SPP[0]*(P[9][0] + P[6][0]*dt) - SPP[1]*(P[9][2] + P[6][2]*dt); + nextP[9][7] = P[9][7] + P[6][7]*dt + dt*(P[9][4] + P[6][4]*dt); + nextP[9][8] = P[9][8] + P[6][8]*dt + dt*(P[9][5] + P[6][5]*dt); + nextP[9][9] = P[9][9] + P[6][9]*dt + dt*(P[9][6] + P[6][6]*dt); + nextP[9][10] = P[9][10] + P[6][10]*dt; + nextP[9][11] = P[9][11] + P[6][11]*dt; + nextP[9][12] = P[9][12] + P[6][12]*dt; + nextP[9][13] = P[9][13] + P[6][13]*dt; + nextP[9][14] = P[9][14] + P[6][14]*dt; + nextP[9][15] = P[9][15] + P[6][15]*dt; + nextP[9][16] = P[9][16] + P[6][16]*dt; + nextP[9][17] = P[9][17] + P[6][17]*dt; + nextP[9][18] = P[9][18] + P[6][18]*dt; + nextP[9][19] = P[9][19] + P[6][19]*dt; + nextP[9][20] = P[9][20] + P[6][20]*dt; + nextP[10][0] = P[10][0] + P[10][1]*SF[6] + P[10][2]*SPP[7] + P[10][3]*SPP[6] + P[10][10]*SPP[5] + P[10][11]*SPP[4] + P[10][12]*SPP[3]; + nextP[10][1] = P[10][1] + P[10][0]*SF[5] + P[10][2]*SF[4] + P[10][3]*SPP[7] + P[10][11]*SPP[3] - P[10][12]*SPP[4] - (P[10][10]*q0)/2; + nextP[10][2] = P[10][2] + P[10][0]*SF[3] + P[10][3]*SF[5] + P[10][1]*SPP[6] - P[10][10]*SPP[3] + P[10][12]*SPP[5] - (P[10][11]*q0)/2; + nextP[10][3] = P[10][3] + P[10][0]*SF[4] + P[10][1]*SF[3] + P[10][2]*SF[6] + P[10][10]*SPP[4] - P[10][11]*SPP[5] - (P[10][12]*q0)/2; + nextP[10][4] = P[10][4] + P[10][1]*SF[0] + P[10][0]*SF[2] + P[10][2]*SPP[0] - P[10][3]*SPP[2]; + nextP[10][5] = P[10][5] + P[10][0]*SF[1] + P[10][2]*SF[0] + P[10][3]*SF[2] - P[10][1]*SPP[0]; + nextP[10][6] = P[10][6] + P[10][1]*SF[1] + P[10][3]*SF[0] + P[10][0]*SPP[0] - P[10][2]*SPP[1]; + nextP[10][7] = P[10][7] + P[10][4]*dt; + nextP[10][8] = P[10][8] + P[10][5]*dt; + nextP[10][9] = P[10][9] + P[10][6]*dt; + nextP[10][10] = P[10][10]; + nextP[10][11] = P[10][11]; + nextP[10][12] = P[10][12]; + nextP[10][13] = P[10][13]; + nextP[10][14] = P[10][14]; + nextP[10][15] = P[10][15]; + nextP[10][16] = P[10][16]; + nextP[10][17] = P[10][17]; + nextP[10][18] = P[10][18]; + nextP[10][19] = P[10][19]; + nextP[10][20] = P[10][20]; + nextP[11][0] = P[11][0] + P[11][1]*SF[6] + P[11][2]*SPP[7] + P[11][3]*SPP[6] + P[11][10]*SPP[5] + P[11][11]*SPP[4] + P[11][12]*SPP[3]; + nextP[11][1] = P[11][1] + P[11][0]*SF[5] + P[11][2]*SF[4] + P[11][3]*SPP[7] + P[11][11]*SPP[3] - P[11][12]*SPP[4] - (P[11][10]*q0)/2; + nextP[11][2] = P[11][2] + P[11][0]*SF[3] + P[11][3]*SF[5] + P[11][1]*SPP[6] - P[11][10]*SPP[3] + P[11][12]*SPP[5] - (P[11][11]*q0)/2; + nextP[11][3] = P[11][3] + P[11][0]*SF[4] + P[11][1]*SF[3] + P[11][2]*SF[6] + P[11][10]*SPP[4] - P[11][11]*SPP[5] - (P[11][12]*q0)/2; + nextP[11][4] = P[11][4] + P[11][1]*SF[0] + P[11][0]*SF[2] + P[11][2]*SPP[0] - P[11][3]*SPP[2]; + nextP[11][5] = P[11][5] + P[11][0]*SF[1] + P[11][2]*SF[0] + P[11][3]*SF[2] - P[11][1]*SPP[0]; + nextP[11][6] = P[11][6] + P[11][1]*SF[1] + P[11][3]*SF[0] + P[11][0]*SPP[0] - P[11][2]*SPP[1]; + nextP[11][7] = P[11][7] + P[11][4]*dt; + nextP[11][8] = P[11][8] + P[11][5]*dt; + nextP[11][9] = P[11][9] + P[11][6]*dt; + nextP[11][10] = P[11][10]; + nextP[11][11] = P[11][11]; + nextP[11][12] = P[11][12]; + nextP[11][13] = P[11][13]; + nextP[11][14] = P[11][14]; + nextP[11][15] = P[11][15]; + nextP[11][16] = P[11][16]; + nextP[11][17] = P[11][17]; + nextP[11][18] = P[11][18]; + nextP[11][19] = P[11][19]; + nextP[11][20] = P[11][20]; + nextP[12][0] = P[12][0] + P[12][1]*SF[6] + P[12][2]*SPP[7] + P[12][3]*SPP[6] + P[12][10]*SPP[5] + P[12][11]*SPP[4] + P[12][12]*SPP[3]; + nextP[12][1] = P[12][1] + P[12][0]*SF[5] + P[12][2]*SF[4] + P[12][3]*SPP[7] + P[12][11]*SPP[3] - P[12][12]*SPP[4] - (P[12][10]*q0)/2; + nextP[12][2] = P[12][2] + P[12][0]*SF[3] + P[12][3]*SF[5] + P[12][1]*SPP[6] - P[12][10]*SPP[3] + P[12][12]*SPP[5] - (P[12][11]*q0)/2; + nextP[12][3] = P[12][3] + P[12][0]*SF[4] + P[12][1]*SF[3] + P[12][2]*SF[6] + P[12][10]*SPP[4] - P[12][11]*SPP[5] - (P[12][12]*q0)/2; + nextP[12][4] = P[12][4] + P[12][1]*SF[0] + P[12][0]*SF[2] + P[12][2]*SPP[0] - P[12][3]*SPP[2]; + nextP[12][5] = P[12][5] + P[12][0]*SF[1] + P[12][2]*SF[0] + P[12][3]*SF[2] - P[12][1]*SPP[0]; + nextP[12][6] = P[12][6] + P[12][1]*SF[1] + P[12][3]*SF[0] + P[12][0]*SPP[0] - P[12][2]*SPP[1]; + nextP[12][7] = P[12][7] + P[12][4]*dt; + nextP[12][8] = P[12][8] + P[12][5]*dt; + nextP[12][9] = P[12][9] + P[12][6]*dt; + nextP[12][10] = P[12][10]; + nextP[12][11] = P[12][11]; + nextP[12][12] = P[12][12]; + nextP[12][13] = P[12][13]; + nextP[12][14] = P[12][14]; + nextP[12][15] = P[12][15]; + nextP[12][16] = P[12][16]; + nextP[12][17] = P[12][17]; + nextP[12][18] = P[12][18]; + nextP[12][19] = P[12][19]; + nextP[12][20] = P[12][20]; + nextP[13][0] = P[13][0] + P[13][1]*SF[6] + P[13][2]*SPP[7] + P[13][3]*SPP[6] + P[13][10]*SPP[5] + P[13][11]*SPP[4] + P[13][12]*SPP[3]; + nextP[13][1] = P[13][1] + P[13][0]*SF[5] + P[13][2]*SF[4] + P[13][3]*SPP[7] + P[13][11]*SPP[3] - P[13][12]*SPP[4] - (P[13][10]*q0)/2; + nextP[13][2] = P[13][2] + P[13][0]*SF[3] + P[13][3]*SF[5] + P[13][1]*SPP[6] - P[13][10]*SPP[3] + P[13][12]*SPP[5] - (P[13][11]*q0)/2; + nextP[13][3] = P[13][3] + P[13][0]*SF[4] + P[13][1]*SF[3] + P[13][2]*SF[6] + P[13][10]*SPP[4] - P[13][11]*SPP[5] - (P[13][12]*q0)/2; + nextP[13][4] = P[13][4] + P[13][1]*SF[0] + P[13][0]*SF[2] + P[13][2]*SPP[0] - P[13][3]*SPP[2]; + nextP[13][5] = P[13][5] + P[13][0]*SF[1] + P[13][2]*SF[0] + P[13][3]*SF[2] - P[13][1]*SPP[0]; + nextP[13][6] = P[13][6] + P[13][1]*SF[1] + P[13][3]*SF[0] + P[13][0]*SPP[0] - P[13][2]*SPP[1]; + nextP[13][7] = P[13][7] + P[13][4]*dt; + nextP[13][8] = P[13][8] + P[13][5]*dt; + nextP[13][9] = P[13][9] + P[13][6]*dt; + nextP[13][10] = P[13][10]; + nextP[13][11] = P[13][11]; + nextP[13][12] = P[13][12]; + nextP[13][13] = P[13][13]; + nextP[13][14] = P[13][14]; + nextP[13][15] = P[13][15]; + nextP[13][16] = P[13][16]; + nextP[13][17] = P[13][17]; + nextP[13][18] = P[13][18]; + nextP[13][19] = P[13][19]; + nextP[13][20] = P[13][20]; + nextP[14][0] = P[14][0] + P[14][1]*SF[6] + P[14][2]*SPP[7] + P[14][3]*SPP[6] + P[14][10]*SPP[5] + P[14][11]*SPP[4] + P[14][12]*SPP[3]; + nextP[14][1] = P[14][1] + P[14][0]*SF[5] + P[14][2]*SF[4] + P[14][3]*SPP[7] + P[14][11]*SPP[3] - P[14][12]*SPP[4] - (P[14][10]*q0)/2; + nextP[14][2] = P[14][2] + P[14][0]*SF[3] + P[14][3]*SF[5] + P[14][1]*SPP[6] - P[14][10]*SPP[3] + P[14][12]*SPP[5] - (P[14][11]*q0)/2; + nextP[14][3] = P[14][3] + P[14][0]*SF[4] + P[14][1]*SF[3] + P[14][2]*SF[6] + P[14][10]*SPP[4] - P[14][11]*SPP[5] - (P[14][12]*q0)/2; + nextP[14][4] = P[14][4] + P[14][1]*SF[0] + P[14][0]*SF[2] + P[14][2]*SPP[0] - P[14][3]*SPP[2]; + nextP[14][5] = P[14][5] + P[14][0]*SF[1] + P[14][2]*SF[0] + P[14][3]*SF[2] - P[14][1]*SPP[0]; + nextP[14][6] = P[14][6] + P[14][1]*SF[1] + P[14][3]*SF[0] + P[14][0]*SPP[0] - P[14][2]*SPP[1]; + nextP[14][7] = P[14][7] + P[14][4]*dt; + nextP[14][8] = P[14][8] + P[14][5]*dt; + nextP[14][9] = P[14][9] + P[14][6]*dt; + nextP[14][10] = P[14][10]; + nextP[14][11] = P[14][11]; + nextP[14][12] = P[14][12]; + nextP[14][13] = P[14][13]; + nextP[14][14] = P[14][14]; + nextP[14][15] = P[14][15]; + nextP[14][16] = P[14][16]; + nextP[14][17] = P[14][17]; + nextP[14][18] = P[14][18]; + nextP[14][19] = P[14][19]; + nextP[14][20] = P[14][20]; + nextP[15][0] = P[15][0] + P[15][1]*SF[6] + P[15][2]*SPP[7] + P[15][3]*SPP[6] + P[15][10]*SPP[5] + P[15][11]*SPP[4] + P[15][12]*SPP[3]; + nextP[15][1] = P[15][1] + P[15][0]*SF[5] + P[15][2]*SF[4] + P[15][3]*SPP[7] + P[15][11]*SPP[3] - P[15][12]*SPP[4] - (P[15][10]*q0)/2; + nextP[15][2] = P[15][2] + P[15][0]*SF[3] + P[15][3]*SF[5] + P[15][1]*SPP[6] - P[15][10]*SPP[3] + P[15][12]*SPP[5] - (P[15][11]*q0)/2; + nextP[15][3] = P[15][3] + P[15][0]*SF[4] + P[15][1]*SF[3] + P[15][2]*SF[6] + P[15][10]*SPP[4] - P[15][11]*SPP[5] - (P[15][12]*q0)/2; + nextP[15][4] = P[15][4] + P[15][1]*SF[0] + P[15][0]*SF[2] + P[15][2]*SPP[0] - P[15][3]*SPP[2]; + nextP[15][5] = P[15][5] + P[15][0]*SF[1] + P[15][2]*SF[0] + P[15][3]*SF[2] - P[15][1]*SPP[0]; + nextP[15][6] = P[15][6] + P[15][1]*SF[1] + P[15][3]*SF[0] + P[15][0]*SPP[0] - P[15][2]*SPP[1]; + nextP[15][7] = P[15][7] + P[15][4]*dt; + nextP[15][8] = P[15][8] + P[15][5]*dt; + nextP[15][9] = P[15][9] + P[15][6]*dt; + nextP[15][10] = P[15][10]; + nextP[15][11] = P[15][11]; + nextP[15][12] = P[15][12]; + nextP[15][13] = P[15][13]; + nextP[15][14] = P[15][14]; + nextP[15][15] = P[15][15]; + nextP[15][16] = P[15][16]; + nextP[15][17] = P[15][17]; + nextP[15][18] = P[15][18]; + nextP[15][19] = P[15][19]; + nextP[15][20] = P[15][20]; + nextP[16][0] = P[16][0] + P[16][1]*SF[6] + P[16][2]*SPP[7] + P[16][3]*SPP[6] + P[16][10]*SPP[5] + P[16][11]*SPP[4] + P[16][12]*SPP[3]; + nextP[16][1] = P[16][1] + P[16][0]*SF[5] + P[16][2]*SF[4] + P[16][3]*SPP[7] + P[16][11]*SPP[3] - P[16][12]*SPP[4] - (P[16][10]*q0)/2; + nextP[16][2] = P[16][2] + P[16][0]*SF[3] + P[16][3]*SF[5] + P[16][1]*SPP[6] - P[16][10]*SPP[3] + P[16][12]*SPP[5] - (P[16][11]*q0)/2; + nextP[16][3] = P[16][3] + P[16][0]*SF[4] + P[16][1]*SF[3] + P[16][2]*SF[6] + P[16][10]*SPP[4] - P[16][11]*SPP[5] - (P[16][12]*q0)/2; + nextP[16][4] = P[16][4] + P[16][1]*SF[0] + P[16][0]*SF[2] + P[16][2]*SPP[0] - P[16][3]*SPP[2]; + nextP[16][5] = P[16][5] + P[16][0]*SF[1] + P[16][2]*SF[0] + P[16][3]*SF[2] - P[16][1]*SPP[0]; + nextP[16][6] = P[16][6] + P[16][1]*SF[1] + P[16][3]*SF[0] + P[16][0]*SPP[0] - P[16][2]*SPP[1]; + nextP[16][7] = P[16][7] + P[16][4]*dt; + nextP[16][8] = P[16][8] + P[16][5]*dt; + nextP[16][9] = P[16][9] + P[16][6]*dt; + nextP[16][10] = P[16][10]; + nextP[16][11] = P[16][11]; + nextP[16][12] = P[16][12]; + nextP[16][13] = P[16][13]; + nextP[16][14] = P[16][14]; + nextP[16][15] = P[16][15]; + nextP[16][16] = P[16][16]; + nextP[16][17] = P[16][17]; + nextP[16][18] = P[16][18]; + nextP[16][19] = P[16][19]; + nextP[16][20] = P[16][20]; + nextP[17][0] = P[17][0] + P[17][1]*SF[6] + P[17][2]*SPP[7] + P[17][3]*SPP[6] + P[17][10]*SPP[5] + P[17][11]*SPP[4] + P[17][12]*SPP[3]; + nextP[17][1] = P[17][1] + P[17][0]*SF[5] + P[17][2]*SF[4] + P[17][3]*SPP[7] + P[17][11]*SPP[3] - P[17][12]*SPP[4] - (P[17][10]*q0)/2; + nextP[17][2] = P[17][2] + P[17][0]*SF[3] + P[17][3]*SF[5] + P[17][1]*SPP[6] - P[17][10]*SPP[3] + P[17][12]*SPP[5] - (P[17][11]*q0)/2; + nextP[17][3] = P[17][3] + P[17][0]*SF[4] + P[17][1]*SF[3] + P[17][2]*SF[6] + P[17][10]*SPP[4] - P[17][11]*SPP[5] - (P[17][12]*q0)/2; + nextP[17][4] = P[17][4] + P[17][1]*SF[0] + P[17][0]*SF[2] + P[17][2]*SPP[0] - P[17][3]*SPP[2]; + nextP[17][5] = P[17][5] + P[17][0]*SF[1] + P[17][2]*SF[0] + P[17][3]*SF[2] - P[17][1]*SPP[0]; + nextP[17][6] = P[17][6] + P[17][1]*SF[1] + P[17][3]*SF[0] + P[17][0]*SPP[0] - P[17][2]*SPP[1]; + nextP[17][7] = P[17][7] + P[17][4]*dt; + nextP[17][8] = P[17][8] + P[17][5]*dt; + nextP[17][9] = P[17][9] + P[17][6]*dt; + nextP[17][10] = P[17][10]; + nextP[17][11] = P[17][11]; + nextP[17][12] = P[17][12]; + nextP[17][13] = P[17][13]; + nextP[17][14] = P[17][14]; + nextP[17][15] = P[17][15]; + nextP[17][16] = P[17][16]; + nextP[17][17] = P[17][17]; + nextP[17][18] = P[17][18]; + nextP[17][19] = P[17][19]; + nextP[17][20] = P[17][20]; + nextP[18][0] = P[18][0] + P[18][1]*SF[6] + P[18][2]*SPP[7] + P[18][3]*SPP[6] + P[18][10]*SPP[5] + P[18][11]*SPP[4] + P[18][12]*SPP[3]; + nextP[18][1] = P[18][1] + P[18][0]*SF[5] + P[18][2]*SF[4] + P[18][3]*SPP[7] + P[18][11]*SPP[3] - P[18][12]*SPP[4] - (P[18][10]*q0)/2; + nextP[18][2] = P[18][2] + P[18][0]*SF[3] + P[18][3]*SF[5] + P[18][1]*SPP[6] - P[18][10]*SPP[3] + P[18][12]*SPP[5] - (P[18][11]*q0)/2; + nextP[18][3] = P[18][3] + P[18][0]*SF[4] + P[18][1]*SF[3] + P[18][2]*SF[6] + P[18][10]*SPP[4] - P[18][11]*SPP[5] - (P[18][12]*q0)/2; + nextP[18][4] = P[18][4] + P[18][1]*SF[0] + P[18][0]*SF[2] + P[18][2]*SPP[0] - P[18][3]*SPP[2]; + nextP[18][5] = P[18][5] + P[18][0]*SF[1] + P[18][2]*SF[0] + P[18][3]*SF[2] - P[18][1]*SPP[0]; + nextP[18][6] = P[18][6] + P[18][1]*SF[1] + P[18][3]*SF[0] + P[18][0]*SPP[0] - P[18][2]*SPP[1]; + nextP[18][7] = P[18][7] + P[18][4]*dt; + nextP[18][8] = P[18][8] + P[18][5]*dt; + nextP[18][9] = P[18][9] + P[18][6]*dt; + nextP[18][10] = P[18][10]; + nextP[18][11] = P[18][11]; + nextP[18][12] = P[18][12]; + nextP[18][13] = P[18][13]; + nextP[18][14] = P[18][14]; + nextP[18][15] = P[18][15]; + nextP[18][16] = P[18][16]; + nextP[18][17] = P[18][17]; + nextP[18][18] = P[18][18]; + nextP[18][19] = P[18][19]; + nextP[18][20] = P[18][20]; + nextP[19][0] = P[19][0] + P[19][1]*SF[6] + P[19][2]*SPP[7] + P[19][3]*SPP[6] + P[19][10]*SPP[5] + P[19][11]*SPP[4] + P[19][12]*SPP[3]; + nextP[19][1] = P[19][1] + P[19][0]*SF[5] + P[19][2]*SF[4] + P[19][3]*SPP[7] + P[19][11]*SPP[3] - P[19][12]*SPP[4] - (P[19][10]*q0)/2; + nextP[19][2] = P[19][2] + P[19][0]*SF[3] + P[19][3]*SF[5] + P[19][1]*SPP[6] - P[19][10]*SPP[3] + P[19][12]*SPP[5] - (P[19][11]*q0)/2; + nextP[19][3] = P[19][3] + P[19][0]*SF[4] + P[19][1]*SF[3] + P[19][2]*SF[6] + P[19][10]*SPP[4] - P[19][11]*SPP[5] - (P[19][12]*q0)/2; + nextP[19][4] = P[19][4] + P[19][1]*SF[0] + P[19][0]*SF[2] + P[19][2]*SPP[0] - P[19][3]*SPP[2]; + nextP[19][5] = P[19][5] + P[19][0]*SF[1] + P[19][2]*SF[0] + P[19][3]*SF[2] - P[19][1]*SPP[0]; + nextP[19][6] = P[19][6] + P[19][1]*SF[1] + P[19][3]*SF[0] + P[19][0]*SPP[0] - P[19][2]*SPP[1]; + nextP[19][7] = P[19][7] + P[19][4]*dt; + nextP[19][8] = P[19][8] + P[19][5]*dt; + nextP[19][9] = P[19][9] + P[19][6]*dt; + nextP[19][10] = P[19][10]; + nextP[19][11] = P[19][11]; + nextP[19][12] = P[19][12]; + nextP[19][13] = P[19][13]; + nextP[19][14] = P[19][14]; + nextP[19][15] = P[19][15]; + nextP[19][16] = P[19][16]; + nextP[19][17] = P[19][17]; + nextP[19][18] = P[19][18]; + nextP[19][19] = P[19][19]; + nextP[19][20] = P[19][20]; + nextP[20][0] = P[20][0] + P[20][1]*SF[6] + P[20][2]*SPP[7] + P[20][3]*SPP[6] + P[20][10]*SPP[5] + P[20][11]*SPP[4] + P[20][12]*SPP[3]; + nextP[20][1] = P[20][1] + P[20][0]*SF[5] + P[20][2]*SF[4] + P[20][3]*SPP[7] + P[20][11]*SPP[3] - P[20][12]*SPP[4] - (P[20][10]*q0)/2; + nextP[20][2] = P[20][2] + P[20][0]*SF[3] + P[20][3]*SF[5] + P[20][1]*SPP[6] - P[20][10]*SPP[3] + P[20][12]*SPP[5] - (P[20][11]*q0)/2; + nextP[20][3] = P[20][3] + P[20][0]*SF[4] + P[20][1]*SF[3] + P[20][2]*SF[6] + P[20][10]*SPP[4] - P[20][11]*SPP[5] - (P[20][12]*q0)/2; + nextP[20][4] = P[20][4] + P[20][1]*SF[0] + P[20][0]*SF[2] + P[20][2]*SPP[0] - P[20][3]*SPP[2]; + nextP[20][5] = P[20][5] + P[20][0]*SF[1] + P[20][2]*SF[0] + P[20][3]*SF[2] - P[20][1]*SPP[0]; + nextP[20][6] = P[20][6] + P[20][1]*SF[1] + P[20][3]*SF[0] + P[20][0]*SPP[0] - P[20][2]*SPP[1]; + nextP[20][7] = P[20][7] + P[20][4]*dt; + nextP[20][8] = P[20][8] + P[20][5]*dt; + nextP[20][9] = P[20][9] + P[20][6]*dt; + nextP[20][10] = P[20][10]; + nextP[20][11] = P[20][11]; + nextP[20][12] = P[20][12]; + nextP[20][13] = P[20][13]; + nextP[20][14] = P[20][14]; + nextP[20][15] = P[20][15]; + nextP[20][16] = P[20][16]; + nextP[20][17] = P[20][17]; + nextP[20][18] = P[20][18]; + nextP[20][19] = P[20][19]; + nextP[20][20] = P[20][20]; + + for (uint8_t i=0; i< n_states; i++) + { + nextP[i][i] = nextP[i][i] + processNoise[i]; + } + + // If on ground or no magnetometer fitted, inhibit magnetometer bias updates by + // setting the coresponding covariance terms to zero. + if (onGround || !useCompass) + { + zeroRows(nextP,15,20); + zeroCols(nextP,15,20); + } + + // If on ground or not using airspeed sensing, inhibit wind velocity + // covariance growth. + if (onGround || !useAirspeed) + { + zeroRows(nextP,13,14); + zeroCols(nextP,13,14); + } + + // If the total position variance exceds 1E6 (1000m), then stop covariance + // growth by setting the predicted to the previous values + // This prevent an ill conditioned matrix from occurring for long periods + // without GPS + if ((P[7][7] + P[8][8]) > 1E6f) + { + for (uint8_t i=7; i<=8; i++) + { + for (uint8_t j=0; j<=20; j++) + { + nextP[i][j] = P[i][j]; + nextP[j][i] = P[j][i]; + } + } + } + + // Force symmetry on the covariance matrix to prevent ill-conditioning + // of the matrix which would cause the filter to blow-up + for (uint8_t i=0; i< n_states; i++) P[i][i] = nextP[i][i]; + for (uint8_t i=1; i< n_states; i++) + { + for (uint8_t j=0; j<=i-1; j++) + { + P[i][j] = 0.5f*(nextP[i][j] + nextP[j][i]); + P[j][i] = P[i][j]; + } + } + + // +} + +void FuseVelposNED() +{ + + // declare variables used by fault isolation logic + uint32_t gpsRetryTime = 30000; // time in msec before GPS fusion will be retried following innovation consistency failure + uint32_t gpsRetryTimeNoTAS = 5000; // retry time if no TAS measurement available + uint32_t hgtRetryTime = 5000; // height measurement retry time + uint32_t horizRetryTime; + static uint32_t velFailTime; + static uint32_t posFailTime; + static uint32_t hgtFailTime; + bool velHealth; + bool posHealth; + bool hgtHealth; + bool velTimeout; + bool posTimeout; + bool hgtTimeout; + + // declare variables used to check measurement errors + float velInnov[3] = {0.0,0.0,0.0}; + float posInnov[2] = {0.0,0.0}; + float hgtInnov = 0.0; + + // declare variables used to control access to arrays + bool fuseData[6] = {false,false,false,false,false,false}; + uint8_t stateIndex; + unsigned obsIndex; + unsigned indexLimit; + + // declare variables used by state and covariance update calculations + float velErr; + float posErr; + float R_OBS[6]; + float observation[6]; + float SK; + float quatMag; + + // Perform sequential fusion of GPS measurements. This assumes that the + // errors in the different velocity and position components are + // uncorrelated which is not true, however in the absence of covariance + // data from the GPS receiver it is the only assumption we can make + // so we might as well take advantage of the computational efficiencies + // associated with sequential fusion + if (fuseVelData || fusePosData || fuseHgtData) + { + // set the GPS data timeout depending on whether airspeed data is present + if (useAirspeed) horizRetryTime = gpsRetryTime; + else horizRetryTime = gpsRetryTimeNoTAS; + + // Form the observation vector + for (uint8_t i=0; i<=2; i++) observation[i] = velNED[i]; + for (uint8_t i=3; i<=4; i++) observation[i] = posNE[i-3]; + observation[5] = -(hgtMea); + + // Estimate the GPS Velocity, GPS horiz position and height measurement variances. + velErr = 0.2*accNavMag; // additional error in GPS velocities caused by manoeuvring + posErr = 0.2*accNavMag; // additional error in GPS position caused by manoeuvring + R_OBS[0] = 0.04f + sq(velErr); + R_OBS[1] = R_OBS[0]; + R_OBS[2] = 0.08f + sq(velErr); + R_OBS[3] = R_OBS[2]; + R_OBS[4] = 4.0f + sq(posErr); + R_OBS[5] = 4.0f; + + // Set innovation variances to zero default + for (uint8_t i = 0; i<=5; i++) + { + varInnovVelPos[i] = 0.0f; + } + // calculate innovations and check GPS data validity using an innovation consistency check + if (fuseVelData) + { + // test velocity measurements + uint8_t imax = 2; + if (fusionModeGPS == 1) imax = 1; + for (uint8_t i = 0; i<=imax; i++) + { + velInnov[i] = statesAtVelTime[i+4] - velNED[i]; + stateIndex = 4 + i; + varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS[i]; + } + // apply a 5-sigma threshold + velHealth = (sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) < 25.0*(varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2]); + velTimeout = (millis() - velFailTime) > horizRetryTime; + if (velHealth || velTimeout) + { + velHealth = true; + velFailTime = millis(); + } + else + { + velHealth = false; + } + } + if (fusePosData) + { + // test horizontal position measurements + posInnov[0] = statesAtPosTime[7] - posNE[0]; + posInnov[1] = statesAtPosTime[8] - posNE[1]; + varInnovVelPos[3] = P[7][7] + R_OBS[3]; + varInnovVelPos[4] = P[8][8] + R_OBS[4]; + // apply a 10-sigma threshold + posHealth = (sq(posInnov[0]) + sq(posInnov[1])) < 100.0f*(varInnovVelPos[3] + varInnovVelPos[4]); + posTimeout = (millis() - posFailTime) > horizRetryTime; + if (posHealth || posTimeout) + { + posHealth = true; + posFailTime = millis(); + } + else + { + posHealth = false; + } + } + // test height measurements + if (fuseHgtData) + { + hgtInnov = statesAtHgtTime[9] + hgtMea; + varInnovVelPos[5] = P[9][9] + R_OBS[5]; + // apply a 10-sigma threshold + hgtHealth = sq(hgtInnov) < 100.0f*varInnovVelPos[5]; + hgtTimeout = (millis() - hgtFailTime) > hgtRetryTime; + if (hgtHealth || hgtTimeout) + { + hgtHealth = true; + hgtFailTime = millis(); + } + else + { + hgtHealth = false; + } + } + // Set range for sequential fusion of velocity and position measurements depending + // on which data is available and its health + if (fuseVelData && fusionModeGPS == 0 && velHealth) + { + fuseData[0] = true; + fuseData[1] = true; + fuseData[2] = true; + } + if (fuseVelData && fusionModeGPS == 1 && velHealth) + { + fuseData[0] = true; + fuseData[1] = true; + } + if (fusePosData && fusionModeGPS <= 2 && posHealth) + { + fuseData[3] = true; + fuseData[4] = true; + } + if (fuseHgtData && hgtHealth) + { + fuseData[5] = true; + } + // Limit range of states modified when on ground + if(!onGround) + { + indexLimit = 20; + } + else + { + indexLimit = 12; + } + // Fuse measurements sequentially + for (obsIndex=0; obsIndex<=5; obsIndex++) + { + if (fuseData[obsIndex]) + { + stateIndex = 4 + obsIndex; + // Calculate the measurement innovation, using states from a + // different time coordinate if fusing height data + if (obsIndex <= 2) + { + innovVelPos[obsIndex] = statesAtVelTime[stateIndex] - observation[obsIndex]; + } + else if (obsIndex == 3 || obsIndex == 4) + { + innovVelPos[obsIndex] = statesAtPosTime[stateIndex] - observation[obsIndex]; + } + else if (obsIndex == 5) + { + innovVelPos[obsIndex] = statesAtHgtTime[stateIndex] - observation[obsIndex]; + } + // Calculate the Kalman Gain + // Calculate innovation variances - also used for data logging + varInnovVelPos[obsIndex] = P[stateIndex][stateIndex] + R_OBS[obsIndex]; + SK = 1.0/varInnovVelPos[obsIndex]; + for (uint8_t i= 0; i<=indexLimit; i++) + { + Kfusion[i] = P[i][stateIndex]*SK; + } + // Calculate state corrections and re-normalise the quaternions + for (uint8_t i = 0; i<=indexLimit; i++) + { + states[i] = states[i] - Kfusion[i] * innovVelPos[obsIndex]; + } + quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); + if (quatMag > 1e-12f) // divide by 0 protection + { + for (uint8_t i = 0; i<=3; i++) + { + states[i] = states[i] / quatMag; + } + } + // Update the covariance - take advantage of direct observation of a + // single state at index = stateIndex to reduce computations + // Optimised implementation of standard equation P = (I - K*H)*P; + for (uint8_t i= 0; i<=indexLimit; i++) + { + for (uint8_t j= 0; j<=indexLimit; j++) + { + KHP[i][j] = Kfusion[i] * P[stateIndex][j]; + } + } + for (uint8_t i= 0; i<=indexLimit; i++) + { + for (uint8_t j= 0; j<=indexLimit; j++) + { + P[i][j] = P[i][j] - KHP[i][j]; + } + } + } + } + } +} + +void FuseMagnetometer() +{ + + static float q0 = 1.0; + static float q1 = 0.0; + static float q2 = 0.0; + static float q3 = 0.0; + static float magN = 0.0; + static float magE = 0.0; + static float magD = 0.0; + static float magXbias = 0.0; + static float magYbias = 0.0; + static float magZbias = 0.0; + static unsigned obsIndex; + uint8_t indexLimit; + float DCM[3][3] = + { + {1.0,0.0,0.0} , + {0.0,1.0,0.0} , + {0.0,0.0,1.0} + }; + static float MagPred[3] = {0.0,0.0,0.0}; + static float R_MAG; + static float SH_MAG[9] = {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0}; + float H_MAG[n_states]; + float SK_MX[6]; + float SK_MY[5]; + float SK_MZ[6]; + + // Perform sequential fusion of Magnetometer measurements. + // This assumes that the errors in the different components are + // uncorrelated which is not true, however in the absence of covariance + // data fit is the only assumption we can make + // so we might as well take advantage of the computational efficiencies + // associated with sequential fusion + if (useCompass && (fuseMagData || obsIndex == 1 || obsIndex == 2)) + { + // Limit range of states modified when on ground + if(!onGround) + { + indexLimit = 20; + } + else + { + indexLimit = 12; + } + + // Sequential fusion of XYZ components to spread processing load across + // three prediction time steps. + + // Calculate observation jacobians and Kalman gains + if (fuseMagData) + { + // Copy required states to local variable names + q0 = statesAtMagMeasTime[0]; + q1 = statesAtMagMeasTime[1]; + q2 = statesAtMagMeasTime[2]; + q3 = statesAtMagMeasTime[3]; + magN = statesAtMagMeasTime[15]; + magE = statesAtMagMeasTime[16]; + magD = statesAtMagMeasTime[17]; + magXbias = statesAtMagMeasTime[18]; + magYbias = statesAtMagMeasTime[19]; + magZbias = statesAtMagMeasTime[20]; + + // rotate predicted earth components into body axes and calculate + // predicted measurments + DCM[0][0] = q0*q0 + q1*q1 - q2*q2 - q3*q3; + DCM[0][1] = 2*(q1*q2 + q0*q3); + DCM[0][2] = 2*(q1*q3-q0*q2); + DCM[1][0] = 2*(q1*q2 - q0*q3); + DCM[1][1] = q0*q0 - q1*q1 + q2*q2 - q3*q3; + DCM[1][2] = 2*(q2*q3 + q0*q1); + DCM[2][0] = 2*(q1*q3 + q0*q2); + DCM[2][1] = 2*(q2*q3 - q0*q1); + DCM[2][2] = q0*q0 - q1*q1 - q2*q2 + q3*q3; + MagPred[0] = DCM[0][0]*magN + DCM[0][1]*magE + DCM[0][2]*magD + magXbias; + MagPred[1] = DCM[1][0]*magN + DCM[1][1]*magE + DCM[1][2]*magD + magYbias; + MagPred[2] = DCM[2][0]*magN + DCM[2][1]*magE + DCM[2][2]*magD + magZbias; + + // scale magnetometer observation error with total angular rate + R_MAG = 0.0025f + sq(0.05f*dAngIMU.length()/dtIMU); + + // Calculate observation jacobians + SH_MAG[0] = 2*magD*q3 + 2*magE*q2 + 2*magN*q1; + SH_MAG[1] = 2*magD*q0 - 2*magE*q1 + 2*magN*q2; + SH_MAG[2] = 2*magD*q1 + 2*magE*q0 - 2*magN*q3; + SH_MAG[3] = sq(q3); + SH_MAG[4] = sq(q2); + SH_MAG[5] = sq(q1); + SH_MAG[6] = sq(q0); + SH_MAG[7] = 2*magN*q0; + SH_MAG[8] = 2*magE*q3; + for (uint8_t i=0; i<=20; i++) H_MAG[i] = 0; + H_MAG[0] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; + H_MAG[1] = SH_MAG[0]; + H_MAG[2] = 2*magE*q1 - 2*magD*q0 - 2*magN*q2; + H_MAG[3] = SH_MAG[2]; + H_MAG[15] = SH_MAG[5] - SH_MAG[4] - SH_MAG[3] + SH_MAG[6]; + H_MAG[16] = 2*q0*q3 + 2*q1*q2; + H_MAG[17] = 2*q1*q3 - 2*q0*q2; + H_MAG[18] = 1; + + // Calculate Kalman gain + SK_MX[0] = 1/(P[18][18] + R_MAG + P[1][18]*SH_MAG[0] + P[3][18]*SH_MAG[2] - P[15][18]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) - (2*magD*q0 - 2*magE*q1 + 2*magN*q2)*(P[18][2] + P[1][2]*SH_MAG[0] + P[3][2]*SH_MAG[2] - P[15][2]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][2]*(2*q0*q3 + 2*q1*q2) - P[17][2]*(2*q0*q2 - 2*q1*q3) - P[2][2]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[18][0] + P[1][0]*SH_MAG[0] + P[3][0]*SH_MAG[2] - P[15][0]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][0]*(2*q0*q3 + 2*q1*q2) - P[17][0]*(2*q0*q2 - 2*q1*q3) - P[2][0]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[18][1] + P[1][1]*SH_MAG[0] + P[3][1]*SH_MAG[2] - P[15][1]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][1]*(2*q0*q3 + 2*q1*q2) - P[17][1]*(2*q0*q2 - 2*q1*q3) - P[2][1]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[2]*(P[18][3] + P[1][3]*SH_MAG[0] + P[3][3]*SH_MAG[2] - P[15][3]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][3]*(2*q0*q3 + 2*q1*q2) - P[17][3]*(2*q0*q2 - 2*q1*q3) - P[2][3]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6])*(P[18][15] + P[1][15]*SH_MAG[0] + P[3][15]*SH_MAG[2] - P[15][15]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][15]*(2*q0*q3 + 2*q1*q2) - P[17][15]*(2*q0*q2 - 2*q1*q3) - P[2][15]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][15]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[16][18]*(2*q0*q3 + 2*q1*q2) - P[17][18]*(2*q0*q2 - 2*q1*q3) - P[2][18]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + (2*q0*q3 + 2*q1*q2)*(P[18][16] + P[1][16]*SH_MAG[0] + P[3][16]*SH_MAG[2] - P[15][16]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][16]*(2*q0*q3 + 2*q1*q2) - P[17][16]*(2*q0*q2 - 2*q1*q3) - P[2][16]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q2 - 2*q1*q3)*(P[18][17] + P[1][17]*SH_MAG[0] + P[3][17]*SH_MAG[2] - P[15][17]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][17]*(2*q0*q3 + 2*q1*q2) - P[17][17]*(2*q0*q2 - 2*q1*q3) - P[2][17]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[0][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); + SK_MX[1] = SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]; + SK_MX[2] = 2*magD*q0 - 2*magE*q1 + 2*magN*q2; + SK_MX[3] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; + SK_MX[4] = 2*q0*q2 - 2*q1*q3; + SK_MX[5] = 2*q0*q3 + 2*q1*q2; + Kfusion[0] = SK_MX[0]*(P[0][18] + P[0][1]*SH_MAG[0] + P[0][3]*SH_MAG[2] + P[0][0]*SK_MX[3] - P[0][2]*SK_MX[2] - P[0][15]*SK_MX[1] + P[0][16]*SK_MX[5] - P[0][17]*SK_MX[4]); + Kfusion[1] = SK_MX[0]*(P[1][18] + P[1][1]*SH_MAG[0] + P[1][3]*SH_MAG[2] + P[1][0]*SK_MX[3] - P[1][2]*SK_MX[2] - P[1][15]*SK_MX[1] + P[1][16]*SK_MX[5] - P[1][17]*SK_MX[4]); + Kfusion[2] = SK_MX[0]*(P[2][18] + P[2][1]*SH_MAG[0] + P[2][3]*SH_MAG[2] + P[2][0]*SK_MX[3] - P[2][2]*SK_MX[2] - P[2][15]*SK_MX[1] + P[2][16]*SK_MX[5] - P[2][17]*SK_MX[4]); + Kfusion[3] = SK_MX[0]*(P[3][18] + P[3][1]*SH_MAG[0] + P[3][3]*SH_MAG[2] + P[3][0]*SK_MX[3] - P[3][2]*SK_MX[2] - P[3][15]*SK_MX[1] + P[3][16]*SK_MX[5] - P[3][17]*SK_MX[4]); + Kfusion[4] = SK_MX[0]*(P[4][18] + P[4][1]*SH_MAG[0] + P[4][3]*SH_MAG[2] + P[4][0]*SK_MX[3] - P[4][2]*SK_MX[2] - P[4][15]*SK_MX[1] + P[4][16]*SK_MX[5] - P[4][17]*SK_MX[4]); + Kfusion[5] = SK_MX[0]*(P[5][18] + P[5][1]*SH_MAG[0] + P[5][3]*SH_MAG[2] + P[5][0]*SK_MX[3] - P[5][2]*SK_MX[2] - P[5][15]*SK_MX[1] + P[5][16]*SK_MX[5] - P[5][17]*SK_MX[4]); + Kfusion[6] = SK_MX[0]*(P[6][18] + P[6][1]*SH_MAG[0] + P[6][3]*SH_MAG[2] + P[6][0]*SK_MX[3] - P[6][2]*SK_MX[2] - P[6][15]*SK_MX[1] + P[6][16]*SK_MX[5] - P[6][17]*SK_MX[4]); + Kfusion[7] = SK_MX[0]*(P[7][18] + P[7][1]*SH_MAG[0] + P[7][3]*SH_MAG[2] + P[7][0]*SK_MX[3] - P[7][2]*SK_MX[2] - P[7][15]*SK_MX[1] + P[7][16]*SK_MX[5] - P[7][17]*SK_MX[4]); + Kfusion[8] = SK_MX[0]*(P[8][18] + P[8][1]*SH_MAG[0] + P[8][3]*SH_MAG[2] + P[8][0]*SK_MX[3] - P[8][2]*SK_MX[2] - P[8][15]*SK_MX[1] + P[8][16]*SK_MX[5] - P[8][17]*SK_MX[4]); + Kfusion[9] = SK_MX[0]*(P[9][18] + P[9][1]*SH_MAG[0] + P[9][3]*SH_MAG[2] + P[9][0]*SK_MX[3] - P[9][2]*SK_MX[2] - P[9][15]*SK_MX[1] + P[9][16]*SK_MX[5] - P[9][17]*SK_MX[4]); + Kfusion[10] = SK_MX[0]*(P[10][18] + P[10][1]*SH_MAG[0] + P[10][3]*SH_MAG[2] + P[10][0]*SK_MX[3] - P[10][2]*SK_MX[2] - P[10][15]*SK_MX[1] + P[10][16]*SK_MX[5] - P[10][17]*SK_MX[4]); + Kfusion[11] = SK_MX[0]*(P[11][18] + P[11][1]*SH_MAG[0] + P[11][3]*SH_MAG[2] + P[11][0]*SK_MX[3] - P[11][2]*SK_MX[2] - P[11][15]*SK_MX[1] + P[11][16]*SK_MX[5] - P[11][17]*SK_MX[4]); + Kfusion[12] = SK_MX[0]*(P[12][18] + P[12][1]*SH_MAG[0] + P[12][3]*SH_MAG[2] + P[12][0]*SK_MX[3] - P[12][2]*SK_MX[2] - P[12][15]*SK_MX[1] + P[12][16]*SK_MX[5] - P[12][17]*SK_MX[4]); + Kfusion[13] = SK_MX[0]*(P[13][18] + P[13][1]*SH_MAG[0] + P[13][3]*SH_MAG[2] + P[13][0]*SK_MX[3] - P[13][2]*SK_MX[2] - P[13][15]*SK_MX[1] + P[13][16]*SK_MX[5] - P[13][17]*SK_MX[4]); + Kfusion[14] = SK_MX[0]*(P[14][18] + P[14][1]*SH_MAG[0] + P[14][3]*SH_MAG[2] + P[14][0]*SK_MX[3] - P[14][2]*SK_MX[2] - P[14][15]*SK_MX[1] + P[14][16]*SK_MX[5] - P[14][17]*SK_MX[4]); + Kfusion[15] = SK_MX[0]*(P[15][18] + P[15][1]*SH_MAG[0] + P[15][3]*SH_MAG[2] + P[15][0]*SK_MX[3] - P[15][2]*SK_MX[2] - P[15][15]*SK_MX[1] + P[15][16]*SK_MX[5] - P[15][17]*SK_MX[4]); + Kfusion[16] = SK_MX[0]*(P[16][18] + P[16][1]*SH_MAG[0] + P[16][3]*SH_MAG[2] + P[16][0]*SK_MX[3] - P[16][2]*SK_MX[2] - P[16][15]*SK_MX[1] + P[16][16]*SK_MX[5] - P[16][17]*SK_MX[4]); + Kfusion[17] = SK_MX[0]*(P[17][18] + P[17][1]*SH_MAG[0] + P[17][3]*SH_MAG[2] + P[17][0]*SK_MX[3] - P[17][2]*SK_MX[2] - P[17][15]*SK_MX[1] + P[17][16]*SK_MX[5] - P[17][17]*SK_MX[4]); + Kfusion[18] = SK_MX[0]*(P[18][18] + P[18][1]*SH_MAG[0] + P[18][3]*SH_MAG[2] + P[18][0]*SK_MX[3] - P[18][2]*SK_MX[2] - P[18][15]*SK_MX[1] + P[18][16]*SK_MX[5] - P[18][17]*SK_MX[4]); + Kfusion[19] = SK_MX[0]*(P[19][18] + P[19][1]*SH_MAG[0] + P[19][3]*SH_MAG[2] + P[19][0]*SK_MX[3] - P[19][2]*SK_MX[2] - P[19][15]*SK_MX[1] + P[19][16]*SK_MX[5] - P[19][17]*SK_MX[4]); + Kfusion[20] = SK_MX[0]*(P[20][18] + P[20][1]*SH_MAG[0] + P[20][3]*SH_MAG[2] + P[20][0]*SK_MX[3] - P[20][2]*SK_MX[2] - P[20][15]*SK_MX[1] + P[20][16]*SK_MX[5] - P[20][17]*SK_MX[4]); + varInnovMag[0] = 1.0f/SK_MX[0]; + innovMag[0] = MagPred[0] - magData.x; + + // reset the observation index to 0 (we start by fusing the X + // measurement) + obsIndex = 0; + } + else if (obsIndex == 1) // we are now fusing the Y measurement + { + // Calculate observation jacobians + for (uint8_t i=0; i<=20; i++) H_MAG[i] = 0; + H_MAG[0] = SH_MAG[2]; + H_MAG[1] = SH_MAG[1]; + H_MAG[2] = SH_MAG[0]; + H_MAG[3] = 2*magD*q2 - SH_MAG[8] - SH_MAG[7]; + H_MAG[15] = 2*q1*q2 - 2*q0*q3; + H_MAG[16] = SH_MAG[4] - SH_MAG[3] - SH_MAG[5] + SH_MAG[6]; + H_MAG[17] = 2*q0*q1 + 2*q2*q3; + H_MAG[19] = 1; + + // Calculate Kalman gain + SK_MY[0] = 1/(P[19][19] + R_MAG + P[0][19]*SH_MAG[2] + P[1][19]*SH_MAG[1] + P[2][19]*SH_MAG[0] - P[16][19]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - (2*q0*q3 - 2*q1*q2)*(P[19][15] + P[0][15]*SH_MAG[2] + P[1][15]*SH_MAG[1] + P[2][15]*SH_MAG[0] - P[16][15]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][15]*(2*q0*q3 - 2*q1*q2) + P[17][15]*(2*q0*q1 + 2*q2*q3) - P[3][15]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (2*q0*q1 + 2*q2*q3)*(P[19][17] + P[0][17]*SH_MAG[2] + P[1][17]*SH_MAG[1] + P[2][17]*SH_MAG[0] - P[16][17]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][17]*(2*q0*q3 - 2*q1*q2) + P[17][17]*(2*q0*q1 + 2*q2*q3) - P[3][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[19][3] + P[0][3]*SH_MAG[2] + P[1][3]*SH_MAG[1] + P[2][3]*SH_MAG[0] - P[16][3]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][3]*(2*q0*q3 - 2*q1*q2) + P[17][3]*(2*q0*q1 + 2*q2*q3) - P[3][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[15][19]*(2*q0*q3 - 2*q1*q2) + P[17][19]*(2*q0*q1 + 2*q2*q3) + SH_MAG[2]*(P[19][0] + P[0][0]*SH_MAG[2] + P[1][0]*SH_MAG[1] + P[2][0]*SH_MAG[0] - P[16][0]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][0]*(2*q0*q3 - 2*q1*q2) + P[17][0]*(2*q0*q1 + 2*q2*q3) - P[3][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[19][1] + P[0][1]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[2][1]*SH_MAG[0] - P[16][1]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][1]*(2*q0*q3 - 2*q1*q2) + P[17][1]*(2*q0*q1 + 2*q2*q3) - P[3][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[19][2] + P[0][2]*SH_MAG[2] + P[1][2]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[16][2]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][2]*(2*q0*q3 - 2*q1*q2) + P[17][2]*(2*q0*q1 + 2*q2*q3) - P[3][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6])*(P[19][16] + P[0][16]*SH_MAG[2] + P[1][16]*SH_MAG[1] + P[2][16]*SH_MAG[0] - P[16][16]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][16]*(2*q0*q3 - 2*q1*q2) + P[17][16]*(2*q0*q1 + 2*q2*q3) - P[3][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[3][19]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); + SK_MY[1] = SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]; + SK_MY[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; + SK_MY[3] = 2*q0*q3 - 2*q1*q2; + SK_MY[4] = 2*q0*q1 + 2*q2*q3; + Kfusion[0] = SK_MY[0]*(P[0][19] + P[0][0]*SH_MAG[2] + P[0][1]*SH_MAG[1] + P[0][2]*SH_MAG[0] - P[0][3]*SK_MY[2] - P[0][16]*SK_MY[1] - P[0][15]*SK_MY[3] + P[0][17]*SK_MY[4]); + Kfusion[1] = SK_MY[0]*(P[1][19] + P[1][0]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[1][2]*SH_MAG[0] - P[1][3]*SK_MY[2] - P[1][16]*SK_MY[1] - P[1][15]*SK_MY[3] + P[1][17]*SK_MY[4]); + Kfusion[2] = SK_MY[0]*(P[2][19] + P[2][0]*SH_MAG[2] + P[2][1]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[2][3]*SK_MY[2] - P[2][16]*SK_MY[1] - P[2][15]*SK_MY[3] + P[2][17]*SK_MY[4]); + Kfusion[3] = SK_MY[0]*(P[3][19] + P[3][0]*SH_MAG[2] + P[3][1]*SH_MAG[1] + P[3][2]*SH_MAG[0] - P[3][3]*SK_MY[2] - P[3][16]*SK_MY[1] - P[3][15]*SK_MY[3] + P[3][17]*SK_MY[4]); + Kfusion[4] = SK_MY[0]*(P[4][19] + P[4][0]*SH_MAG[2] + P[4][1]*SH_MAG[1] + P[4][2]*SH_MAG[0] - P[4][3]*SK_MY[2] - P[4][16]*SK_MY[1] - P[4][15]*SK_MY[3] + P[4][17]*SK_MY[4]); + Kfusion[5] = SK_MY[0]*(P[5][19] + P[5][0]*SH_MAG[2] + P[5][1]*SH_MAG[1] + P[5][2]*SH_MAG[0] - P[5][3]*SK_MY[2] - P[5][16]*SK_MY[1] - P[5][15]*SK_MY[3] + P[5][17]*SK_MY[4]); + Kfusion[6] = SK_MY[0]*(P[6][19] + P[6][0]*SH_MAG[2] + P[6][1]*SH_MAG[1] + P[6][2]*SH_MAG[0] - P[6][3]*SK_MY[2] - P[6][16]*SK_MY[1] - P[6][15]*SK_MY[3] + P[6][17]*SK_MY[4]); + Kfusion[7] = SK_MY[0]*(P[7][19] + P[7][0]*SH_MAG[2] + P[7][1]*SH_MAG[1] + P[7][2]*SH_MAG[0] - P[7][3]*SK_MY[2] - P[7][16]*SK_MY[1] - P[7][15]*SK_MY[3] + P[7][17]*SK_MY[4]); + Kfusion[8] = SK_MY[0]*(P[8][19] + P[8][0]*SH_MAG[2] + P[8][1]*SH_MAG[1] + P[8][2]*SH_MAG[0] - P[8][3]*SK_MY[2] - P[8][16]*SK_MY[1] - P[8][15]*SK_MY[3] + P[8][17]*SK_MY[4]); + Kfusion[9] = SK_MY[0]*(P[9][19] + P[9][0]*SH_MAG[2] + P[9][1]*SH_MAG[1] + P[9][2]*SH_MAG[0] - P[9][3]*SK_MY[2] - P[9][16]*SK_MY[1] - P[9][15]*SK_MY[3] + P[9][17]*SK_MY[4]); + Kfusion[10] = SK_MY[0]*(P[10][19] + P[10][0]*SH_MAG[2] + P[10][1]*SH_MAG[1] + P[10][2]*SH_MAG[0] - P[10][3]*SK_MY[2] - P[10][16]*SK_MY[1] - P[10][15]*SK_MY[3] + P[10][17]*SK_MY[4]); + Kfusion[11] = SK_MY[0]*(P[11][19] + P[11][0]*SH_MAG[2] + P[11][1]*SH_MAG[1] + P[11][2]*SH_MAG[0] - P[11][3]*SK_MY[2] - P[11][16]*SK_MY[1] - P[11][15]*SK_MY[3] + P[11][17]*SK_MY[4]); + Kfusion[12] = SK_MY[0]*(P[12][19] + P[12][0]*SH_MAG[2] + P[12][1]*SH_MAG[1] + P[12][2]*SH_MAG[0] - P[12][3]*SK_MY[2] - P[12][16]*SK_MY[1] - P[12][15]*SK_MY[3] + P[12][17]*SK_MY[4]); + Kfusion[13] = SK_MY[0]*(P[13][19] + P[13][0]*SH_MAG[2] + P[13][1]*SH_MAG[1] + P[13][2]*SH_MAG[0] - P[13][3]*SK_MY[2] - P[13][16]*SK_MY[1] - P[13][15]*SK_MY[3] + P[13][17]*SK_MY[4]); + Kfusion[14] = SK_MY[0]*(P[14][19] + P[14][0]*SH_MAG[2] + P[14][1]*SH_MAG[1] + P[14][2]*SH_MAG[0] - P[14][3]*SK_MY[2] - P[14][16]*SK_MY[1] - P[14][15]*SK_MY[3] + P[14][17]*SK_MY[4]); + Kfusion[15] = SK_MY[0]*(P[15][19] + P[15][0]*SH_MAG[2] + P[15][1]*SH_MAG[1] + P[15][2]*SH_MAG[0] - P[15][3]*SK_MY[2] - P[15][16]*SK_MY[1] - P[15][15]*SK_MY[3] + P[15][17]*SK_MY[4]); + Kfusion[16] = SK_MY[0]*(P[16][19] + P[16][0]*SH_MAG[2] + P[16][1]*SH_MAG[1] + P[16][2]*SH_MAG[0] - P[16][3]*SK_MY[2] - P[16][16]*SK_MY[1] - P[16][15]*SK_MY[3] + P[16][17]*SK_MY[4]); + Kfusion[17] = SK_MY[0]*(P[17][19] + P[17][0]*SH_MAG[2] + P[17][1]*SH_MAG[1] + P[17][2]*SH_MAG[0] - P[17][3]*SK_MY[2] - P[17][16]*SK_MY[1] - P[17][15]*SK_MY[3] + P[17][17]*SK_MY[4]); + Kfusion[18] = SK_MY[0]*(P[18][19] + P[18][0]*SH_MAG[2] + P[18][1]*SH_MAG[1] + P[18][2]*SH_MAG[0] - P[18][3]*SK_MY[2] - P[18][16]*SK_MY[1] - P[18][15]*SK_MY[3] + P[18][17]*SK_MY[4]); + Kfusion[19] = SK_MY[0]*(P[19][19] + P[19][0]*SH_MAG[2] + P[19][1]*SH_MAG[1] + P[19][2]*SH_MAG[0] - P[19][3]*SK_MY[2] - P[19][16]*SK_MY[1] - P[19][15]*SK_MY[3] + P[19][17]*SK_MY[4]); + Kfusion[20] = SK_MY[0]*(P[20][19] + P[20][0]*SH_MAG[2] + P[20][1]*SH_MAG[1] + P[20][2]*SH_MAG[0] - P[20][3]*SK_MY[2] - P[20][16]*SK_MY[1] - P[20][15]*SK_MY[3] + P[20][17]*SK_MY[4]); + varInnovMag[1] = 1.0f/SK_MY[0]; + innovMag[1] = MagPred[1] - magData.y; + } + else if (obsIndex == 2) // we are now fusing the Z measurement + { + // Calculate observation jacobians + for (uint8_t i=0; i<=20; i++) H_MAG[i] = 0; + H_MAG[0] = SH_MAG[1]; + H_MAG[1] = 2*magN*q3 - 2*magE*q0 - 2*magD*q1; + H_MAG[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; + H_MAG[3] = SH_MAG[0]; + H_MAG[15] = 2*q0*q2 + 2*q1*q3; + H_MAG[16] = 2*q2*q3 - 2*q0*q1; + H_MAG[17] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]; + H_MAG[20] = 1; + + // Calculate Kalman gain + SK_MZ[0] = 1/(P[20][20] + R_MAG + P[0][20]*SH_MAG[1] + P[3][20]*SH_MAG[0] + P[17][20]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) - (2*magD*q1 + 2*magE*q0 - 2*magN*q3)*(P[20][1] + P[0][1]*SH_MAG[1] + P[3][1]*SH_MAG[0] + P[17][1]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][1]*(2*q0*q2 + 2*q1*q3) - P[16][1]*(2*q0*q1 - 2*q2*q3) - P[1][1]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[20][2] + P[0][2]*SH_MAG[1] + P[3][2]*SH_MAG[0] + P[17][2]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][2]*(2*q0*q2 + 2*q1*q3) - P[16][2]*(2*q0*q1 - 2*q2*q3) - P[1][2]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[20][0] + P[0][0]*SH_MAG[1] + P[3][0]*SH_MAG[0] + P[17][0]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][0]*(2*q0*q2 + 2*q1*q3) - P[16][0]*(2*q0*q1 - 2*q2*q3) - P[1][0]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[20][3] + P[0][3]*SH_MAG[1] + P[3][3]*SH_MAG[0] + P[17][3]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][3]*(2*q0*q2 + 2*q1*q3) - P[16][3]*(2*q0*q1 - 2*q2*q3) - P[1][3]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6])*(P[20][17] + P[0][17]*SH_MAG[1] + P[3][17]*SH_MAG[0] + P[17][17]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][17]*(2*q0*q2 + 2*q1*q3) - P[16][17]*(2*q0*q1 - 2*q2*q3) - P[1][17]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[15][20]*(2*q0*q2 + 2*q1*q3) - P[16][20]*(2*q0*q1 - 2*q2*q3) - P[1][20]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + (2*q0*q2 + 2*q1*q3)*(P[20][15] + P[0][15]*SH_MAG[1] + P[3][15]*SH_MAG[0] + P[17][15]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][15]*(2*q0*q2 + 2*q1*q3) - P[16][15]*(2*q0*q1 - 2*q2*q3) - P[1][15]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][15]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q1 - 2*q2*q3)*(P[20][16] + P[0][16]*SH_MAG[1] + P[3][16]*SH_MAG[0] + P[17][16]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][16]*(2*q0*q2 + 2*q1*q3) - P[16][16]*(2*q0*q1 - 2*q2*q3) - P[1][16]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[2][20]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); + SK_MZ[1] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]; + SK_MZ[2] = 2*magD*q1 + 2*magE*q0 - 2*magN*q3; + SK_MZ[3] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; + SK_MZ[4] = 2*q0*q1 - 2*q2*q3; + SK_MZ[5] = 2*q0*q2 + 2*q1*q3; + Kfusion[0] = SK_MZ[0]*(P[0][20] + P[0][0]*SH_MAG[1] + P[0][3]*SH_MAG[0] - P[0][1]*SK_MZ[2] + P[0][2]*SK_MZ[3] + P[0][17]*SK_MZ[1] + P[0][15]*SK_MZ[5] - P[0][16]*SK_MZ[4]); + Kfusion[1] = SK_MZ[0]*(P[1][20] + P[1][0]*SH_MAG[1] + P[1][3]*SH_MAG[0] - P[1][1]*SK_MZ[2] + P[1][2]*SK_MZ[3] + P[1][17]*SK_MZ[1] + P[1][15]*SK_MZ[5] - P[1][16]*SK_MZ[4]); + Kfusion[2] = SK_MZ[0]*(P[2][20] + P[2][0]*SH_MAG[1] + P[2][3]*SH_MAG[0] - P[2][1]*SK_MZ[2] + P[2][2]*SK_MZ[3] + P[2][17]*SK_MZ[1] + P[2][15]*SK_MZ[5] - P[2][16]*SK_MZ[4]); + Kfusion[3] = SK_MZ[0]*(P[3][20] + P[3][0]*SH_MAG[1] + P[3][3]*SH_MAG[0] - P[3][1]*SK_MZ[2] + P[3][2]*SK_MZ[3] + P[3][17]*SK_MZ[1] + P[3][15]*SK_MZ[5] - P[3][16]*SK_MZ[4]); + Kfusion[4] = SK_MZ[0]*(P[4][20] + P[4][0]*SH_MAG[1] + P[4][3]*SH_MAG[0] - P[4][1]*SK_MZ[2] + P[4][2]*SK_MZ[3] + P[4][17]*SK_MZ[1] + P[4][15]*SK_MZ[5] - P[4][16]*SK_MZ[4]); + Kfusion[5] = SK_MZ[0]*(P[5][20] + P[5][0]*SH_MAG[1] + P[5][3]*SH_MAG[0] - P[5][1]*SK_MZ[2] + P[5][2]*SK_MZ[3] + P[5][17]*SK_MZ[1] + P[5][15]*SK_MZ[5] - P[5][16]*SK_MZ[4]); + Kfusion[6] = SK_MZ[0]*(P[6][20] + P[6][0]*SH_MAG[1] + P[6][3]*SH_MAG[0] - P[6][1]*SK_MZ[2] + P[6][2]*SK_MZ[3] + P[6][17]*SK_MZ[1] + P[6][15]*SK_MZ[5] - P[6][16]*SK_MZ[4]); + Kfusion[7] = SK_MZ[0]*(P[7][20] + P[7][0]*SH_MAG[1] + P[7][3]*SH_MAG[0] - P[7][1]*SK_MZ[2] + P[7][2]*SK_MZ[3] + P[7][17]*SK_MZ[1] + P[7][15]*SK_MZ[5] - P[7][16]*SK_MZ[4]); + Kfusion[8] = SK_MZ[0]*(P[8][20] + P[8][0]*SH_MAG[1] + P[8][3]*SH_MAG[0] - P[8][1]*SK_MZ[2] + P[8][2]*SK_MZ[3] + P[8][17]*SK_MZ[1] + P[8][15]*SK_MZ[5] - P[8][16]*SK_MZ[4]); + Kfusion[9] = SK_MZ[0]*(P[9][20] + P[9][0]*SH_MAG[1] + P[9][3]*SH_MAG[0] - P[9][1]*SK_MZ[2] + P[9][2]*SK_MZ[3] + P[9][17]*SK_MZ[1] + P[9][15]*SK_MZ[5] - P[9][16]*SK_MZ[4]); + Kfusion[10] = SK_MZ[0]*(P[10][20] + P[10][0]*SH_MAG[1] + P[10][3]*SH_MAG[0] - P[10][1]*SK_MZ[2] + P[10][2]*SK_MZ[3] + P[10][17]*SK_MZ[1] + P[10][15]*SK_MZ[5] - P[10][16]*SK_MZ[4]); + Kfusion[11] = SK_MZ[0]*(P[11][20] + P[11][0]*SH_MAG[1] + P[11][3]*SH_MAG[0] - P[11][1]*SK_MZ[2] + P[11][2]*SK_MZ[3] + P[11][17]*SK_MZ[1] + P[11][15]*SK_MZ[5] - P[11][16]*SK_MZ[4]); + Kfusion[12] = SK_MZ[0]*(P[12][20] + P[12][0]*SH_MAG[1] + P[12][3]*SH_MAG[0] - P[12][1]*SK_MZ[2] + P[12][2]*SK_MZ[3] + P[12][17]*SK_MZ[1] + P[12][15]*SK_MZ[5] - P[12][16]*SK_MZ[4]); + Kfusion[13] = SK_MZ[0]*(P[13][20] + P[13][0]*SH_MAG[1] + P[13][3]*SH_MAG[0] - P[13][1]*SK_MZ[2] + P[13][2]*SK_MZ[3] + P[13][17]*SK_MZ[1] + P[13][15]*SK_MZ[5] - P[13][16]*SK_MZ[4]); + Kfusion[14] = SK_MZ[0]*(P[14][20] + P[14][0]*SH_MAG[1] + P[14][3]*SH_MAG[0] - P[14][1]*SK_MZ[2] + P[14][2]*SK_MZ[3] + P[14][17]*SK_MZ[1] + P[14][15]*SK_MZ[5] - P[14][16]*SK_MZ[4]); + Kfusion[15] = SK_MZ[0]*(P[15][20] + P[15][0]*SH_MAG[1] + P[15][3]*SH_MAG[0] - P[15][1]*SK_MZ[2] + P[15][2]*SK_MZ[3] + P[15][17]*SK_MZ[1] + P[15][15]*SK_MZ[5] - P[15][16]*SK_MZ[4]); + Kfusion[16] = SK_MZ[0]*(P[16][20] + P[16][0]*SH_MAG[1] + P[16][3]*SH_MAG[0] - P[16][1]*SK_MZ[2] + P[16][2]*SK_MZ[3] + P[16][17]*SK_MZ[1] + P[16][15]*SK_MZ[5] - P[16][16]*SK_MZ[4]); + Kfusion[17] = SK_MZ[0]*(P[17][20] + P[17][0]*SH_MAG[1] + P[17][3]*SH_MAG[0] - P[17][1]*SK_MZ[2] + P[17][2]*SK_MZ[3] + P[17][17]*SK_MZ[1] + P[17][15]*SK_MZ[5] - P[17][16]*SK_MZ[4]); + Kfusion[18] = SK_MZ[0]*(P[18][20] + P[18][0]*SH_MAG[1] + P[18][3]*SH_MAG[0] - P[18][1]*SK_MZ[2] + P[18][2]*SK_MZ[3] + P[18][17]*SK_MZ[1] + P[18][15]*SK_MZ[5] - P[18][16]*SK_MZ[4]); + Kfusion[19] = SK_MZ[0]*(P[19][20] + P[19][0]*SH_MAG[1] + P[19][3]*SH_MAG[0] - P[19][1]*SK_MZ[2] + P[19][2]*SK_MZ[3] + P[19][17]*SK_MZ[1] + P[19][15]*SK_MZ[5] - P[19][16]*SK_MZ[4]); + Kfusion[20] = SK_MZ[0]*(P[20][20] + P[20][0]*SH_MAG[1] + P[20][3]*SH_MAG[0] - P[20][1]*SK_MZ[2] + P[20][2]*SK_MZ[3] + P[20][17]*SK_MZ[1] + P[20][15]*SK_MZ[5] - P[20][16]*SK_MZ[4]); + varInnovMag[2] = 1.0f/SK_MZ[0]; + innovMag[2] = MagPred[2] - magData.z; + + } + + // Check the innovation for consistency and don't fuse if > 5Sigma + if ((innovMag[obsIndex]*innovMag[obsIndex]/varInnovMag[obsIndex]) < 25.0) + { + // correct the state vector + for (uint8_t j= 0; j<=indexLimit; j++) + { + states[j] = states[j] - Kfusion[j] * innovMag[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-12) + { + 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<=indexLimit; i++) + { + for (uint8_t j = 0; j<=3; j++) + { + KH[i][j] = Kfusion[i] * H_MAG[j]; + } + for (uint8_t j = 4; j<=17; j++) KH[i][j] = 0.0f; + if (!onGround) + { + for (uint8_t j = 15; j<=20; j++) + { + KH[i][j] = Kfusion[i] * H_MAG[j]; + } + } + else + { + for (uint8_t j = 15; j<=20; j++) + { + KH[i][j] = 0.0f; + } + } + } + for (uint8_t i = 0; i<=indexLimit; i++) + { + for (uint8_t j = 0; j<=indexLimit; j++) + { + KHP[i][j] = 0.0f; + for (uint8_t k = 0; k<=3; k++) + { + KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; + } + if (!onGround) + { + for (uint8_t k = 15; k<=20; k++) + { + KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; + } + } + } + } + } + for (uint8_t i = 0; i<=indexLimit; i++) + { + for (uint8_t j = 0; j<=indexLimit; j++) + { + P[i][j] = P[i][j] - KHP[i][j]; + } + } + } + obsIndex = obsIndex + 1; +} + +void FuseAirspeed() +{ + float vn; + float ve; + float vd; + float vwn; + float vwe; + const float R_TAS = 2.0f; + float SH_TAS[3]; + float SK_TAS; + float H_TAS[n_states]; + float Kfusion[n_states]; + float VtasPred; + float quatMag; + + // Copy required states to local variable names + vn = statesAtVtasMeasTime[4]; + ve = statesAtVtasMeasTime[5]; + vd = statesAtVtasMeasTime[6]; + vwn = statesAtVtasMeasTime[13]; + vwe = statesAtVtasMeasTime[14]; + + // Need to check that it is flying before fusing airspeed data + // Calculate the predicted airspeed + VtasPred = sqrt((ve - vwe)*(ve - vwe) + (vn - vwn)*(vn - vwn) + vd*vd); + // Perform fusion of True Airspeed measurement + if (useAirspeed && fuseVtasData && (VtasPred > 1.0) && (VtasMeas > 8.0)) + { + // Calculate observation jacobians + SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd))); + SH_TAS[1] = (SH_TAS[0]*(2*ve - 2*vwe))/2; + SH_TAS[2] = (SH_TAS[0]*(2*vn - 2*vwn))/2; + for (uint8_t i=0; i<=20; i++) H_TAS[i] = 0.0f; + H_TAS[4] = SH_TAS[2]; + H_TAS[5] = SH_TAS[1]; + H_TAS[6] = vd*SH_TAS[0]; + H_TAS[13] = -SH_TAS[2]; + H_TAS[14] = -SH_TAS[1]; + + // Calculate Kalman gains + SK_TAS = 1.0f/(R_TAS + SH_TAS[2]*(P[4][4]*SH_TAS[2] + P[5][4]*SH_TAS[1] - P[13][4]*SH_TAS[2] - P[14][4]*SH_TAS[1] + P[6][4]*vd*SH_TAS[0]) + SH_TAS[1]*(P[4][5]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[13][5]*SH_TAS[2] - P[14][5]*SH_TAS[1] + P[6][5]*vd*SH_TAS[0]) - SH_TAS[2]*(P[4][13]*SH_TAS[2] + P[5][13]*SH_TAS[1] - P[13][13]*SH_TAS[2] - P[14][13]*SH_TAS[1] + P[6][13]*vd*SH_TAS[0]) - SH_TAS[1]*(P[4][14]*SH_TAS[2] + P[5][14]*SH_TAS[1] - P[13][14]*SH_TAS[2] - P[14][14]*SH_TAS[1] + P[6][14]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[4][6]*SH_TAS[2] + P[5][6]*SH_TAS[1] - P[13][6]*SH_TAS[2] - P[14][6]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0])); + Kfusion[0] = SK_TAS*(P[0][4]*SH_TAS[2] - P[0][13]*SH_TAS[2] + P[0][5]*SH_TAS[1] - P[0][14]*SH_TAS[1] + P[0][6]*vd*SH_TAS[0]); + Kfusion[1] = SK_TAS*(P[1][4]*SH_TAS[2] - P[1][13]*SH_TAS[2] + P[1][5]*SH_TAS[1] - P[1][14]*SH_TAS[1] + P[1][6]*vd*SH_TAS[0]); + Kfusion[2] = SK_TAS*(P[2][4]*SH_TAS[2] - P[2][13]*SH_TAS[2] + P[2][5]*SH_TAS[1] - P[2][14]*SH_TAS[1] + P[2][6]*vd*SH_TAS[0]); + Kfusion[3] = SK_TAS*(P[3][4]*SH_TAS[2] - P[3][13]*SH_TAS[2] + P[3][5]*SH_TAS[1] - P[3][14]*SH_TAS[1] + P[3][6]*vd*SH_TAS[0]); + Kfusion[4] = SK_TAS*(P[4][4]*SH_TAS[2] - P[4][13]*SH_TAS[2] + P[4][5]*SH_TAS[1] - P[4][14]*SH_TAS[1] + P[4][6]*vd*SH_TAS[0]); + Kfusion[5] = SK_TAS*(P[5][4]*SH_TAS[2] - P[5][13]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[5][14]*SH_TAS[1] + P[5][6]*vd*SH_TAS[0]); + Kfusion[6] = SK_TAS*(P[6][4]*SH_TAS[2] - P[6][13]*SH_TAS[2] + P[6][5]*SH_TAS[1] - P[6][14]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0]); + Kfusion[7] = SK_TAS*(P[7][4]*SH_TAS[2] - P[7][13]*SH_TAS[2] + P[7][5]*SH_TAS[1] - P[7][14]*SH_TAS[1] + P[7][6]*vd*SH_TAS[0]); + Kfusion[8] = SK_TAS*(P[8][4]*SH_TAS[2] - P[8][13]*SH_TAS[2] + P[8][5]*SH_TAS[1] - P[8][14]*SH_TAS[1] + P[8][6]*vd*SH_TAS[0]); + Kfusion[9] = SK_TAS*(P[9][4]*SH_TAS[2] - P[9][13]*SH_TAS[2] + P[9][5]*SH_TAS[1] - P[9][14]*SH_TAS[1] + P[9][6]*vd*SH_TAS[0]); + Kfusion[10] = SK_TAS*(P[10][4]*SH_TAS[2] - P[10][13]*SH_TAS[2] + P[10][5]*SH_TAS[1] - P[10][14]*SH_TAS[1] + P[10][6]*vd*SH_TAS[0]); + Kfusion[11] = SK_TAS*(P[11][4]*SH_TAS[2] - P[11][13]*SH_TAS[2] + P[11][5]*SH_TAS[1] - P[11][14]*SH_TAS[1] + P[11][6]*vd*SH_TAS[0]); + Kfusion[12] = SK_TAS*(P[12][4]*SH_TAS[2] - P[12][13]*SH_TAS[2] + P[12][5]*SH_TAS[1] - P[12][14]*SH_TAS[1] + P[12][6]*vd*SH_TAS[0]); + Kfusion[13] = SK_TAS*(P[13][4]*SH_TAS[2] - P[13][13]*SH_TAS[2] + P[13][5]*SH_TAS[1] - P[13][14]*SH_TAS[1] + P[13][6]*vd*SH_TAS[0]); + Kfusion[14] = SK_TAS*(P[14][4]*SH_TAS[2] - P[14][13]*SH_TAS[2] + P[14][5]*SH_TAS[1] - P[14][14]*SH_TAS[1] + P[14][6]*vd*SH_TAS[0]); + Kfusion[15] = SK_TAS*(P[15][4]*SH_TAS[2] - P[15][13]*SH_TAS[2] + P[15][5]*SH_TAS[1] - P[15][14]*SH_TAS[1] + P[15][6]*vd*SH_TAS[0]); + Kfusion[16] = SK_TAS*(P[16][4]*SH_TAS[2] - P[16][13]*SH_TAS[2] + P[16][5]*SH_TAS[1] - P[16][14]*SH_TAS[1] + P[16][6]*vd*SH_TAS[0]); + Kfusion[17] = SK_TAS*(P[17][4]*SH_TAS[2] - P[17][13]*SH_TAS[2] + P[17][5]*SH_TAS[1] - P[17][14]*SH_TAS[1] + P[17][6]*vd*SH_TAS[0]); + Kfusion[18] = SK_TAS*(P[18][4]*SH_TAS[2] - P[18][13]*SH_TAS[2] + P[18][5]*SH_TAS[1] - P[18][14]*SH_TAS[1] + P[18][6]*vd*SH_TAS[0]); + Kfusion[19] = SK_TAS*(P[19][4]*SH_TAS[2] - P[19][13]*SH_TAS[2] + P[19][5]*SH_TAS[1] - P[19][14]*SH_TAS[1] + P[19][6]*vd*SH_TAS[0]); + Kfusion[20] = SK_TAS*(P[20][4]*SH_TAS[2] - P[20][13]*SH_TAS[2] + P[20][5]*SH_TAS[1] - P[20][14]*SH_TAS[1] + P[20][6]*vd*SH_TAS[0]); + varInnovVtas = 1.0f/SK_TAS; + + // Calculate the measurement innovation + innovVtas = VtasPred - VtasMeas; + // Check the innovation for consistency and don't fuse if > 5Sigma + if ((innovVtas*innovVtas*SK_TAS) < 25.0) + { + // correct the state vector + for (uint8_t j=0; j<=20; j++) + { + states[j] = states[j] - Kfusion[j] * innovVtas; + } + // normalise the quaternion states + 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 H to reduce the + // number of operations + for (uint8_t i = 0; i<=20; i++) + { + for (uint8_t j = 0; j<=3; j++) KH[i][j] = 0.0; + for (uint8_t j = 4; j<=6; j++) + { + KH[i][j] = Kfusion[i] * H_TAS[j]; + } + for (uint8_t j = 7; j<=12; j++) KH[i][j] = 0.0; + for (uint8_t j = 13; j<=14; j++) + { + KH[i][j] = Kfusion[i] * H_TAS[j]; + } + for (uint8_t j = 15; j<=20; j++) KH[i][j] = 0.0; + } + for (uint8_t i = 0; i<=20; i++) + { + for (uint8_t j = 0; j<=20; j++) + { + KHP[i][j] = 0.0; + for (uint8_t k = 4; k<=6; k++) + { + KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; + } + for (uint8_t k = 13; k<=14; k++) + { + KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; + } + } + } + for (uint8_t i = 0; i<=20; i++) + { + for (uint8_t j = 0; j<=20; j++) + { + P[i][j] = P[i][j] - KHP[i][j]; + } + } + } + } +} +void zeroRows(float covMat[n_states][n_states], uint8_t first, uint8_t last) +{ + uint8_t row; + uint8_t col; + for (row=first; row<=last; row++) + { + for (col=0; col +#include + +#pragma once + +#define GRAVITY_MSS 9.80665f +#define deg2rad 0.017453292f +#define rad2deg 57.295780f +#define pi 3.141592657f +#define earthRate 0.000072921f +#define earthRadius 6378145.0f +#define earthRadiusInv 1.5678540e-7f + +class Vector3f +{ +private: +public: + float x; + float y; + float z; + + float length(void) const; + Vector3f zero(void) const; +}; + +class Mat3f +{ +private: +public: + Vector3f x; + Vector3f y; + Vector3f z; + + Mat3f transpose(void) const; +}; + +Vector3f operator*(float sclIn1, Vector3f vecIn1); +Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2); +Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2); +Vector3f operator*( Mat3f matIn, Vector3f vecIn); +Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2); +Vector3f operator*(Vector3f vecIn1, float sclIn1); + +void swap_var(float &d1, float &d2); + +const unsigned int n_states = 21; +const unsigned int data_buffer_size = 50; + +extern uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored +extern Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad) +extern Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s) +extern Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad) +extern Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s) +extern float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2) +extern Vector3f earthRateNED; // earths angular rate vector in NED (rad/s) +extern Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s) +extern Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2) +extern Vector3f dVelIMU; +extern Vector3f dAngIMU; + +extern float P[n_states][n_states]; // covariance matrix +extern float Kfusion[n_states]; // Kalman gains +extern float states[n_states]; // state matrix +extern float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps + +extern Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad) +extern Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s) +extern Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad) +extern Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s) + +extern float dt; // time lapsed since last covariance prediction +extern float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec) + +extern bool onGround; // boolean true when the flight vehicle is on the ground (not flying) +extern bool useAirspeed; // boolean true if airspeed data is being used +extern bool useCompass; // boolean true if magnetometer data is being used +extern uint8_t fusionModeGPS ; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity +extern float innovVelPos[6]; // innovation output +extern float varInnovVelPos[6]; // innovation variance output + +extern bool fuseVelData; // this boolean causes the posNE and velNED obs to be fused +extern bool fusePosData; // this boolean causes the posNE and velNED obs to be fused +extern bool fuseHgtData; // this boolean causes the hgtMea obs to be fused +extern bool fuseMagData; // boolean true when magnetometer data is to be fused + +extern float velNED[3]; // North, East, Down velocity obs (m/s) +extern float posNE[2]; // North, East position obs (m) +extern float hgtMea; // measured height (m) +extern float posNED[3]; // North, East Down position (m) + +extern float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements +extern float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements +extern float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement + +extern float innovMag[3]; // innovation output +extern float varInnovMag[3]; // innovation variance output +extern Vector3f magData; // magnetometer flux radings in X,Y,Z body axes +extern float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time +extern float innovVtas; // innovation output +extern float varInnovVtas; // innovation variance output +extern bool fuseVtasData; // boolean true when airspeed data is to be fused +extern float VtasMeas; // true airspeed measurement (m/s) +extern float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time +extern float latRef; // WGS-84 latitude of reference point (rad) +extern float lonRef; // WGS-84 longitude of reference point (rad) +extern float hgtRef; // WGS-84 height of reference point (m) +extern Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes +extern uint8_t covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction +extern float EAS2TAS; // ratio f true to equivalent airspeed + +// GPS input data variables +extern float gpsCourse; +extern float gpsGndSpd; +extern float gpsVelD; +extern float gpsLat; +extern float gpsLon; +extern float gpsHgt; +extern uint8_t GPSstatus; + +extern bool statesInitialised; + +const float covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions +const float covDelAngMax = 0.05f; // maximum delta angle between covariance predictions + +void UpdateStrapdownEquationsNED(); + +void CovariancePrediction(); + +void FuseVelposNED(); + +void FuseMagnetometer(); + +void FuseAirspeed(); + +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); + +float sq(float valIn); + +void quatNorm(float quatOut[4], float quatIn[4]); + +// store staes along with system time stamp in msces +void StoreStates(); + +// recall stste vector stored at closest time to the one specified by msec +void RecallStates(float statesForFusion[n_states], uint32_t msec); + +void quat2Tnb(Mat3f &Tnb, float quat[4]); + +void quat2Tbn(Mat3f &Tbn, float quat[4]); + +void calcEarthRateNED(Vector3f &omega, float latitude); + +void eul2quat(float quat[4], float eul[3]); + +void quat2eul(float eul[3],float quat[4]); + +void calcvelNED(float velNED[3], float gpsCourse, float gpsGndSpd, float gpsVelD); + +void calcposNED(float posNED[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); + +void calcLLH(float posNED[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); + +void OnGroundCheck(); + +void CovarianceInit(); + +void InitialiseFilter(); + +uint32_t millis(); + diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index dc05540631..7a71e774c8 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -78,7 +78,7 @@ #include #include -#include "../../../InertialNav/code/estimator.h" +#include "estimator.h" diff --git a/src/modules/fw_att_pos_estimator/module.mk b/src/modules/fw_att_pos_estimator/module.mk index c1384be7e9..c992959e0d 100644 --- a/src/modules/fw_att_pos_estimator/module.mk +++ b/src/modules/fw_att_pos_estimator/module.mk @@ -39,4 +39,4 @@ MODULE_COMMAND = fw_att_pos_estimator SRCS = fw_att_pos_estimator_main.cpp \ fw_att_pos_estimator_params.c \ - ../../../../InertialNav/code/estimator.cpp + estimator.cpp From ac77fe9c27d7253b01805ff94d3e0f8e21017709 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Jan 2014 18:40:02 +0100 Subject: [PATCH 029/381] WIP state on getting MAVLink as a class, much of the work done, but does not compile yet --- src/modules/mavlink/mavlink.c | 36 --- src/modules/mavlink/mavlink_main.cpp | 262 ++++++++++--------- src/modules/mavlink/mavlink_main.h | 153 ++++++++++- src/modules/mavlink/mavlink_orb_listener.cpp | 6 +- src/modules/mavlink/mavlink_orb_listener.h | 19 +- src/modules/mavlink/mavlink_receiver.cpp | 90 +++---- src/modules/mavlink/mavlink_receiver.h | 77 +++++- 7 files changed, 400 insertions(+), 243 deletions(-) diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index aba8c98daf..b143e62f0c 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -85,42 +85,6 @@ mavlink_system_t mavlink_system = { 0 }; // System ID, 1-255, Component/Subsystem ID, 1-255 -/* - * Internal function to send the bytes through the right serial port - */ -void -mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length) -{ - int uart = -1; - - switch (channel) { - case MAVLINK_COMM_0: - uart = Mavlink::get_uart_fd(0); - break; - case MAVLINK_COMM_1: - uart = Mavlink::get_uart_fd(1); - break; - case MAVLINK_COMM_2: - uart = Mavlink::get_uart_fd(2); - break; - case MAVLINK_COMM_3: - uart = Mavlink::get_uart_fd(3); - break; - case MAVLINK_COMM_4: - uart = Mavlink::get_uart_fd(4); - break; - case MAVLINK_COMM_5: - uart = Mavlink::get_uart_fd(5); - break; - case MAVLINK_COMM_6: - uart = Mavlink::get_uart_fd(6); - break; - } - - write(uart, ch, (size_t)(sizeof(uint8_t) * length)); - -} - /* * Internal function to give access to the channel status for each channel */ diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 339030a868..1c7986cbb8 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -47,6 +47,7 @@ #include #include #include +#include #include #include #include @@ -70,10 +71,10 @@ #include #include #include +#include #include "mavlink_bridge_header.h" -#include "mavlink_parameters.h" #include #include "math.h" /* isinf / isnan checks */ #include @@ -87,7 +88,9 @@ #include #include #include -#include +#include "mavlink_main.h" +#include "mavlink_orb_listener.h" +#include "mavlink_receiver.h" /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -102,7 +105,49 @@ static const int ERROR = -1; */ extern "C" __EXPORT int mavlink_main(int argc, char *argv[]); +/* + * Internal function to send the bytes through the right serial port + */ +void +mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length) +{ + int uart = -1; + switch (channel) { + case MAVLINK_COMM_0: + uart = Mavlink::get_uart_fd(0); + break; + case MAVLINK_COMM_1: + uart = Mavlink::get_uart_fd(1); + break; + case MAVLINK_COMM_2: + uart = Mavlink::get_uart_fd(2); + break; + case MAVLINK_COMM_3: + uart = Mavlink::get_uart_fd(3); + break; + #ifdef MAVLINK_COMM_4 + case MAVLINK_COMM_4: + uart = Mavlink::get_uart_fd(4); + break; + #endif + #ifdef MAVLINK_COMM_5 + case MAVLINK_COMM_5: + uart = Mavlink::get_uart_fd(5); + break; + #endif + #ifdef MAVLINK_COMM_6 + case MAVLINK_COMM_6: + uart = Mavlink::get_uart_fd(6); + break; + #endif + } + + write(uart, ch, (size_t)(sizeof(uint8_t) * length)); + +} + +static void usage(void); namespace mavlink { @@ -122,6 +167,8 @@ Mavlink::Mavlink(const char *port, unsigned baud_rate) : _mavlink_hil_enabled(false) // _params_sub(-1) { + wpm = &wpm_s; + fops.ioctl = (int (*)(file*, int, long unsigned int))&mavlink_dev_ioctl; // _parameter_handles.min_altitude = param_find("NAV_MIN_ALT"); } @@ -191,6 +238,15 @@ Mavlink* Mavlink::get_instance(unsigned instance) return inst; } +int Mavlink::get_uart_fd(unsigned index) +{ + Mavlink* inst = get_instance(index); + if (inst) + return inst->_mavlink_fd; + + return -1; +} + void Mavlink::parameters_update() { @@ -202,48 +258,12 @@ Mavlink::parameters_update() } -/* XXX not widely used */ -uint8_t chan = MAVLINK_COMM_0; - -/* XXX probably should be in a header... */ -extern pthread_t receive_start(int uart); - -/* Allocate storage space for waypoints */ -static mavlink_wpm_storage wpm_s; -mavlink_wpm_storage *wpm = &wpm_s; - -bool mavlink_hil_enabled = false; - -/* protocol interface */ -static int uart; -static int baudrate; -bool gcs_link = true; - -/* interface mode */ -static enum { - MAVLINK_INTERFACE_MODE_OFFBOARD, - MAVLINK_INTERFACE_MODE_ONBOARD -} mavlink_link_mode = MAVLINK_INTERFACE_MODE_OFFBOARD; - -static struct mavlink_logbuffer lb; - -static void mavlink_update_system(void); -static int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); -static void usage(void); -int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval); - /**************************************************************************** * MAVLink text message logger ****************************************************************************/ -static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); - -static const struct file_operations mavlink_fops = { - .ioctl = mavlink_dev_ioctl -}; - -static int -mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) +int +Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) { static unsigned int total_counter = 0; @@ -252,10 +272,11 @@ mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) case (int)MAVLINK_IOC_SEND_TEXT_CRITICAL: case (int)MAVLINK_IOC_SEND_TEXT_EMERGENCY: { const char *txt = (const char *)arg; - struct mavlink_logmessage msg; - strncpy(msg.text, txt, sizeof(msg.text)); - mavlink_logbuffer_write(&lb, &msg); - total_counter++; + printf("logmsg: %s\n", txt); + //struct mavlink_logmessage msg; + //strncpy(msg.text, txt, sizeof(msg.text)); + //mavlink_logbuffer_write(&lb, &msg); + //total_counter++; return OK; } @@ -264,7 +285,7 @@ mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) } } -void mavlink_update_system(void) +void Mavlink::mavlink_update_system(void) { static bool initialized = false; static param_t param_system_id; @@ -301,7 +322,7 @@ void mavlink_update_system(void) } } -int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb) +int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb) { /* process baud rate */ int speed; @@ -398,7 +419,7 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf } int -set_hil_on_off(bool hil_enabled) +Mavlink::set_hil_on_off(bool hil_enabled) { int ret = OK; @@ -427,13 +448,13 @@ set_hil_on_off(bool hil_enabled) hil_rate_interval = 5; } - orb_set_interval(mavlink_subs.spa_sub, hil_rate_interval); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, hil_rate_interval); + orb_set_interval(subs.spa_sub, hil_rate_interval); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, hil_rate_interval); } if (!hil_enabled && mavlink_hil_enabled) { mavlink_hil_enabled = false; - orb_set_interval(mavlink_subs.spa_sub, 200); + orb_set_interval(subs.spa_sub, 200); } else { ret = ERROR; @@ -443,7 +464,7 @@ set_hil_on_off(bool hil_enabled) } void -get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode) +Mavlink::get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode) { /* reset MAVLink mode bitfield */ *mavlink_base_mode = 0; @@ -518,7 +539,7 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u } -int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval) +int Mavlink::set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval) { int ret = OK; @@ -552,7 +573,7 @@ int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_m orb_set_interval(subs->actuators_sub, min_interval); orb_set_interval(subs->actuators_effective_sub, min_interval); orb_set_interval(subs->spa_sub, min_interval); - orb_set_interval(mavlink_subs.rates_setpoint_sub, min_interval); + orb_set_interval(subs->rates_setpoint_sub, min_interval); break; case MAVLINK_MSG_ID_MANUAL_CONTROL: @@ -575,34 +596,19 @@ int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_m extern mavlink_system_t mavlink_system; -extern int mavlink_missionlib_send_message(mavlink_message_t *msg); -extern int mavlink_missionlib_send_gcs_string(const char *string); - -/** - * If the queue index is not at 0, the queue sending - * logic will send parameters from the current index - * to len - 1, the end of the param list. - */ -static unsigned int mavlink_param_queue_index = 0; - -/** - * Callback for param interface. - */ -void mavlink_pm_callback(void *arg, param_t param); - -void mavlink_pm_callback(void *arg, param_t param) +void Mavlink::mavlink_pm_callback(void *arg, param_t param) { - mavlink_pm_send_param(param); + //mavlink_pm_send_param(param); usleep(*(unsigned int *)arg); } -void mavlink_pm_send_all_params(unsigned int delay) +void Mavlink::mavlink_pm_send_all_params(unsigned int delay) { unsigned int dbuf = delay; param_foreach(&mavlink_pm_callback, &dbuf, false); } -int mavlink_pm_queued_send() +int Mavlink::mavlink_pm_queued_send() { if (mavlink_param_queue_index < param_count()) { mavlink_pm_send_param(param_for_index(mavlink_param_queue_index)); @@ -614,22 +620,22 @@ int mavlink_pm_queued_send() } } -void mavlink_pm_start_queued_send() +void Mavlink::mavlink_pm_start_queued_send() { mavlink_param_queue_index = 0; } -int mavlink_pm_send_param_for_index(uint16_t index) +int Mavlink::mavlink_pm_send_param_for_index(uint16_t index) { return mavlink_pm_send_param(param_for_index(index)); } -int mavlink_pm_send_param_for_name(const char *name) +int Mavlink::mavlink_pm_send_param_for_name(const char *name) { return mavlink_pm_send_param(param_find(name)); } -int mavlink_pm_send_param(param_t param) +int Mavlink::mavlink_pm_send_param(param_t param) { if (param == PARAM_INVALID) return 1; @@ -679,11 +685,11 @@ int mavlink_pm_send_param(param_t param) mavlink_type, param_count(), param_get_index(param)); - ret = mavlink_missionlib_send_message(&tx_msg); - return ret; + mavlink_missionlib_send_message(&tx_msg); + return OK; } -void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg) +void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg) { switch (msg->msgid) { case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { @@ -748,7 +754,7 @@ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_mess } } -void publish_mission() +void Mavlink::publish_mission() { /* Initialize mission publication if necessary */ if (mission_pub < 0) { @@ -759,7 +765,7 @@ void publish_mission() } } -int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item) +int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item) { /* only support global waypoints for now */ switch (mavlink_mission_item->frame) { @@ -797,7 +803,7 @@ int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavli mission_item->yaw = _wrap_pi(mavlink_mission_item->param4*M_DEG_TO_RAD_F); mission_item->loiter_radius = fabsf(mavlink_mission_item->param3); mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */ - mission_item->nav_cmd = mavlink_mission_item->command; + mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command; mission_item->time_inside = mavlink_mission_item->param1; mission_item->autocontinue = mavlink_mission_item->autocontinue; @@ -807,7 +813,7 @@ int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavli return OK; } -int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item) +int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item) { if (mission_item->altitude_is_relative) { mavlink_mission_item->frame = MAV_FRAME_GLOBAL; @@ -838,7 +844,7 @@ int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *missio return OK; } -void mavlink_wpm_init(mavlink_wpm_storage *state) +void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state) { state->size = 0; state->max_size = MAVLINK_WPM_MAX_WP_COUNT; @@ -854,7 +860,7 @@ void mavlink_wpm_init(mavlink_wpm_storage *state) /* * @brief Sends an waypoint ack message */ -void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type) +void Mavlink::mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type) { mavlink_message_t msg; mavlink_mission_ack_t wpa; @@ -878,7 +884,7 @@ void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type) * * @param seq The waypoint sequence number the MAV should fly to. */ -void mavlink_wpm_send_waypoint_current(uint16_t seq) +void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq) { if (seq < wpm->size) { mavlink_message_t msg; @@ -897,7 +903,7 @@ void mavlink_wpm_send_waypoint_current(uint16_t seq) } } -void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count) +void Mavlink::mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count) { mavlink_message_t msg; mavlink_mission_count_t wpc; @@ -912,7 +918,7 @@ void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t cou if (verbose) warnx("Sent waypoint count (%u) to ID %u", wpc.count, wpc.target_system); } -void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq) +void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq) { struct mission_item_s mission_item; @@ -946,7 +952,7 @@ void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq) } } -void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq) +void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq) { if (seq < wpm->max_size) { mavlink_message_t msg; @@ -972,7 +978,7 @@ void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t s * * @param seq The waypoint sequence number the MAV has reached. */ -void mavlink_wpm_send_waypoint_reached(uint16_t seq) +void Mavlink::mavlink_wpm_send_waypoint_reached(uint16_t seq) { mavlink_message_t msg; mavlink_mission_item_reached_t wp_reached; @@ -986,7 +992,7 @@ void mavlink_wpm_send_waypoint_reached(uint16_t seq) } -void mavlink_waypoint_eventloop(uint64_t now) +void Mavlink::mavlink_waypoint_eventloop(uint64_t now) { /* check for timed-out operations */ if (now - wpm->timestamp_lastaction > wpm->timeout && wpm->current_state != MAVLINK_WPM_STATE_IDLE) { @@ -1002,7 +1008,7 @@ void mavlink_waypoint_eventloop(uint64_t now) } -void mavlink_wpm_message_handler(const mavlink_message_t *msg) +void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) { uint64_t now = hrt_absolute_time(); @@ -1377,7 +1383,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg) } void -mavlink_missionlib_send_message(mavlink_message_t *msg) +Mavlink::mavlink_missionlib_send_message(mavlink_message_t *msg) { uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg); @@ -1387,7 +1393,7 @@ mavlink_missionlib_send_message(mavlink_message_t *msg) int -mavlink_missionlib_send_gcs_string(const char *string) +Mavlink::mavlink_missionlib_send_gcs_string(const char *string) { const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN; mavlink_statustext_t statustext; @@ -1419,11 +1425,11 @@ mavlink_missionlib_send_gcs_string(const char *string) void Mavlink::task_main_trampoline(int argc, char *argv[]) { - mavlink::g_mavlink->task_main(); + mavlink::g_mavlink->task_main(argc, argv); } -void -Mavlink::task_main() +int +Mavlink::task_main(int argc, char *argv[]) { /* inform about start */ warnx("Initializing.."); @@ -1529,16 +1535,18 @@ Mavlink::task_main() err(1, "could not open %s", device_name); /* create the device node that's used for sending text log messages, etc. */ - register_driver(MAVLINK_LOG_DEVICE, &mavlink_fops, 0666, NULL); + register_driver(MAVLINK_LOG_DEVICE, &fops, 0666, NULL); /* Initialize system properties */ mavlink_update_system(); /* start the MAVLink receiver */ - receive_thread = receive_start(uart); + MavlinkReceiver rcv(this); + receive_thread = rcv.receive_start(uart); /* start the ORB receiver */ - uorb_receive_thread = uorb_receive_start(); + MavlinkOrbListener listener(this); + uorb_receive_thread = listener.uorb_receive_start(); /* initialize waypoint manager */ mavlink_wpm_init(wpm); @@ -1546,57 +1554,57 @@ Mavlink::task_main() /* all subscriptions are now active, set up initial guess about rate limits */ if (baudrate >= 230400) { /* 200 Hz / 5 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 20); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 20); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_HIGHRES_IMU, 20); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_RAW_IMU, 20); /* 50 Hz / 20 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 30); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_ATTITUDE, 30); /* 20 Hz / 50 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 10); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 10); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50); /* 10 Hz */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 100); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_GPS_RAW_INT, 100); /* 10 Hz */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 100); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 100); } else if (baudrate >= 115200) { /* 20 Hz / 50 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 50); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 50); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 50); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_HIGHRES_IMU, 50); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_RAW_IMU, 50); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_ATTITUDE, 50); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50); /* 5 Hz / 200 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200); /* 5 Hz / 200 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 200); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_GPS_RAW_INT, 200); /* 2 Hz */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); } else if (baudrate >= 57600) { /* 10 Hz / 100 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 300); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 300); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_RAW_IMU, 300); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_HIGHRES_IMU, 300); /* 10 Hz / 100 ms ATTITUDE */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 200); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_ATTITUDE, 200); /* 5 Hz / 200 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200); /* 5 Hz / 200 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 500); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 500); /* 2 Hz */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); /* 2 Hz */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 500); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_GPS_RAW_INT, 500); } else { /* very low baud rate, limit to 1 Hz / 1000 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 1000); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 1000); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 1000); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_RAW_IMU, 1000); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_ATTITUDE, 1000); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_HIGHRES_IMU, 1000); /* 1 Hz / 1000 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 1000); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 1000); /* 0.5 Hz */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 2000); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 2000); /* 0.1 Hz */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 10000); + set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 10000); } int mission_result_sub = orb_subscribe(ORB_ID(mission_result)); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 8d6f0bd6f7..244af04a67 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -73,6 +73,70 @@ #include #include + +#include + + // FIXME XXX - TO BE MOVED TO XML +enum MAVLINK_WPM_STATES { + MAVLINK_WPM_STATE_IDLE = 0, + MAVLINK_WPM_STATE_SENDLIST, + MAVLINK_WPM_STATE_SENDLIST_SENDWPS, + MAVLINK_WPM_STATE_GETLIST, + MAVLINK_WPM_STATE_GETLIST_GETWPS, + MAVLINK_WPM_STATE_GETLIST_GOTALL, + MAVLINK_WPM_STATE_ENUM_END +}; + +enum MAVLINK_WPM_CODES { + MAVLINK_WPM_CODE_OK = 0, + MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED, + MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED, + MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS, + MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED, + MAVLINK_WPM_CODE_ENUM_END +}; + + +/* WAYPOINT MANAGER - MISSION LIB */ + +#define MAVLINK_WPM_MAX_WP_COUNT 15 +#define MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE ///< Enable double buffer and in-flight updates +#ifndef MAVLINK_WPM_TEXT_FEEDBACK +#define MAVLINK_WPM_TEXT_FEEDBACK 0 ///< Report back status information as text +#endif +#define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication timeout in useconds +#define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000 ///< When to send a new setpoint +#define MAVLINK_WPM_PROTOCOL_DELAY_DEFAULT 40000 + + +struct mavlink_wpm_storage { + mavlink_mission_item_t waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Currently active waypoints +#ifdef MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE + mavlink_mission_item_t rcv_waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Receive buffer for next waypoints +#endif + uint16_t size; + uint16_t max_size; + uint16_t rcv_size; + enum MAVLINK_WPM_STATES current_state; + int16_t current_wp_id; ///< Waypoint in current transmission + int16_t current_active_wp_id; ///< Waypoint the system is currently heading towards + uint16_t current_count; + uint8_t current_partner_sysid; + uint8_t current_partner_compid; + uint64_t timestamp_lastaction; + uint64_t timestamp_last_send_setpoint; + uint64_t timestamp_firstinside_orbit; + uint64_t timestamp_lastoutside_orbit; + uint32_t timeout; + uint32_t delay_setpoint; + float accept_range_yaw; + float accept_range_distance; + bool yaw_reached; + bool pos_reached; + bool idle; + int current_dataman_id; +}; + class Mavlink { public: @@ -104,6 +168,8 @@ public: static Mavlink* get_instance(unsigned instance); + static int get_uart_fd(unsigned index); + struct mavlink_subscriptions { int sensor_sub; int att_sub; @@ -149,12 +215,19 @@ public: /** Actuator armed state */ struct actuator_armed_s armed; +protected: + /** + * Pointer to the default cdev file operations table; useful for + * registering clone devices etc. + */ + struct file_operations fops; + int _mavlink_fd; + private: bool _task_should_exit; /**< if true, sensor task should exit */ int _mavlink_task; /**< task handle for sensor task */ - int _mavlink_fd; int _mavlink_incoming_fd; /**< file descriptor on which to receive incoming strings */ perf_counter_t _loop_perf; /**< loop performance counter */ @@ -165,10 +238,45 @@ private: int _params_sub; - orb_advert_t mission_pub = -1; -struct mission_s mission; + orb_advert_t mission_pub; + struct mission_s mission; + uint8_t missionlib_msg_buf[300]; //XXX MAGIC NUMBER + bool thread_running; + bool thread_should_exit; -uint8_t mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER; + uint8_t mavlink_wpm_comp_id; + mavlink_channel_t chan; + +// XXX probably should be in a header... +// extern pthread_t receive_start(int uart); + + pthread_t receive_thread; + pthread_t uorb_receive_thread; + + /* Allocate storage space for waypoints */ + mavlink_wpm_storage wpm_s; + mavlink_wpm_storage *wpm; + + bool verbose; + bool mavlink_hil_enabled; + int uart; + int baudrate; + bool gcs_link; + /** + * If the queue index is not at 0, the queue sending + * logic will send parameters from the current index + * to len - 1, the end of the param list. + */ + unsigned int mavlink_param_queue_index; + + /* interface mode */ + enum { + MAVLINK_INTERFACE_MODE_OFFBOARD, + MAVLINK_INTERFACE_MODE_ONBOARD + } mavlink_link_mode; + + struct mavlink_logbuffer lb; + bool mavlink_link_termination_allowed; /** * Update our local parameter cache. @@ -240,7 +348,38 @@ uint8_t mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER; * mavlink_pm_queued_send(). * @see mavlink_pm_queued_send() */ - void mavlink_pm_start_queued_send(void); + void mavlink_pm_start_queued_send(); + + void mavlink_update_system(); + + void mavlink_wpm_message_handler(const mavlink_message_t *msg); + void mavlink_waypoint_eventloop(uint64_t now); + void mavlink_wpm_send_waypoint_reached(uint16_t seq); + void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq); + void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq); + void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count); + void mavlink_wpm_send_waypoint_current(uint16_t seq); + void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type); + void mavlink_wpm_init(mavlink_wpm_storage *state); + int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item); + int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item); + void publish_mission(); + + void mavlink_missionlib_send_message(mavlink_message_t *msg); + int mavlink_missionlib_send_gcs_string(const char *string); + + int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); + + int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval); + + void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode); + + /** + * Callback for param interface. + */ + static void mavlink_pm_callback(void *arg, param_t param); + + static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); /** * Shim for calling task_main from task_create. @@ -248,8 +387,8 @@ uint8_t mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER; void task_main_trampoline(int argc, char *argv[]); /** - * Main sensor collection task. + * Main mavlink task. */ - void task_main() __attribute__((noreturn)); + int task_main(int argc, char *argv[]) __attribute__((noreturn)); }; diff --git a/src/modules/mavlink/mavlink_orb_listener.cpp b/src/modules/mavlink/mavlink_orb_listener.cpp index 3edad0f16f..e2675dfa71 100644 --- a/src/modules/mavlink/mavlink_orb_listener.cpp +++ b/src/modules/mavlink/mavlink_orb_listener.cpp @@ -661,8 +661,8 @@ MavlinkOrbListener::l_control_mode(const struct listener *l) mavlink_state); } -static void * -uorb_receive_thread(void *arg) +void * +MavlinkOrbListener::uorb_receive_thread(void *arg) { /* Set thread name */ prctl(PR_SET_NAME, "mavlink_orb_rcv", getpid()); @@ -712,7 +712,7 @@ uorb_receive_thread(void *arg) } pthread_t -uorb_receive_start(void) +MavlinkOrbListener::uorb_receive_start(void) { /* --- SENSORS RAW VALUE --- */ mavlink_subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); diff --git a/src/modules/mavlink/mavlink_orb_listener.h b/src/modules/mavlink/mavlink_orb_listener.h index 32a174fcd0..29e081b360 100644 --- a/src/modules/mavlink/mavlink_orb_listener.h +++ b/src/modules/mavlink/mavlink_orb_listener.h @@ -55,18 +55,21 @@ public: */ ~MavlinkOrbListener(); - /** - * Start the mavlink task. - * - * @return OK on success. - */ - int start(); + // * + // * Start the mavlink task. + // * + // * @return OK on success. + + // int start(); /** * Display the mavlink status. */ void status(); + pthread_t uorb_receive_start(void); + void * uorb_receive_thread(void *arg); + private: bool _task_should_exit; /**< if true, sensor task should exit */ @@ -122,8 +125,6 @@ private: struct vehicle_attitude_s att; struct airspeed_s airspeed; - struct mavlink_subscriptions mavlink_subs; - int status_sub; int rc_sub; @@ -138,6 +139,6 @@ private: */ uint64_t last_sensor_timestamp; - hrt_abstime last_sent_vfr = 0; + hrt_abstime last_sent_vfr; }; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 6881a22802..ab4074558a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -76,61 +76,42 @@ __BEGIN_DECLS #include "mavlink_bridge_header.h" -#include "waypoints.h" -#include "orb_topics.h" -#include "mavlink_parameters.h" +// #include "waypoints.h" +#include "mavlink_receiver.h" +#include "mavlink_main.h" #include "util.h" __END_DECLS -/* XXX should be in a header somewhere */ -extern "C" pthread_t receive_start(int uart); +void MavlinkReceiver::MavlinkReceiver() : + pub_hil_global_pos(-1), + pub_hil_local_pos(-1), + pub_hil_attitude(-1), + pub_hil_gps(-1), + pub_hil_sensors(-1), + pub_hil_gyro(-1), + pub_hil_accel(-1), + pub_hil_mag(-1), + pub_hil_baro(-1), + pub_hil_airspeed(-1), + pub_hil_battery(-1), + hil_counter(0), + hil_frames(0), + old_timestamp(0), + cmd_pub(-1), + flow_pub(-1), + offboard_control_sp_pub(-1), + vicon_position_pub(-1), + telemetry_status_pub(-1), + lat0(0), + lon0(0), + alt0(0) +{ -static void handle_message(mavlink_message_t *msg); -static void *receive_thread(void *arg); +} -static mavlink_status_t status; -static struct vehicle_vicon_position_s vicon_position; -static struct vehicle_command_s vcmd; -static struct offboard_control_setpoint_s offboard_control_sp; - -struct vehicle_global_position_s hil_global_pos; -struct vehicle_local_position_s hil_local_pos; -struct vehicle_attitude_s hil_attitude; -struct vehicle_gps_position_s hil_gps; -struct sensor_combined_s hil_sensors; -struct battery_status_s hil_battery_status; -static orb_advert_t pub_hil_global_pos = -1; -static orb_advert_t pub_hil_local_pos = -1; -static orb_advert_t pub_hil_attitude = -1; -static orb_advert_t pub_hil_gps = -1; -static orb_advert_t pub_hil_sensors = -1; -static orb_advert_t pub_hil_gyro = -1; -static orb_advert_t pub_hil_accel = -1; -static orb_advert_t pub_hil_mag = -1; -static orb_advert_t pub_hil_baro = -1; -static orb_advert_t pub_hil_airspeed = -1; -static orb_advert_t pub_hil_battery = -1; - -/* packet counter */ -static int hil_counter = 0; -static int hil_frames = 0; -static uint64_t old_timestamp = 0; - -static orb_advert_t cmd_pub = -1; -static orb_advert_t flow_pub = -1; - -static orb_advert_t offboard_control_sp_pub = -1; -static orb_advert_t vicon_position_pub = -1; -static orb_advert_t telemetry_status_pub = -1; - -// variables for HIL reference position -static int32_t lat0 = 0; -static int32_t lon0 = 0; -static double alt0 = 0; - -static void -handle_message(mavlink_message_t *msg) +void +MavlinkReceiver::handle_message(mavlink_message_t *msg) { if (msg->msgid == MAVLINK_MSG_ID_COMMAND_LONG) { @@ -804,8 +785,8 @@ handle_message(mavlink_message_t *msg) /** * Receive data from UART. */ -static void * -receive_thread(void *arg) +void * +MavlinkReceiver::receive_thread(void *arg) { int uart_fd = *((int *)arg); @@ -851,8 +832,13 @@ receive_thread(void *arg) return NULL; } +void MavlinkReceiver::print_status() +{ + +} + pthread_t -receive_start(int uart) +MavlinkReceiver::receive_start(int uart) { pthread_attr_t receiveloop_attr; pthread_attr_init(&receiveloop_attr); diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 88ae4b1103..556b6f8ad6 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -40,6 +40,36 @@ #pragma once +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + class Mavlink; class MavlinkReceiver @@ -65,7 +95,9 @@ public: /** * Display the mavlink status. */ - void status(); + void print_status(); + + pthread_t receive_start(int uart); private: @@ -75,14 +107,41 @@ private: Mavlink* _mavlink; - /** - * Shim for calling task_main from task_create. - */ - void task_main_trampoline(int argc, char *argv[]); - /** - * Main sensor collection task. - */ - void task_main() __attribute__((noreturn)); + void handle_message(mavlink_message_t *msg); + void *receive_thread(void *arg); + + mavlink_status_t status; + struct vehicle_vicon_position_s vicon_position; + struct vehicle_command_s vcmd; + struct offboard_control_setpoint_s offboard_control_sp; + struct vehicle_global_position_s hil_global_pos; + struct vehicle_local_position_s hil_local_pos; + struct vehicle_attitude_s hil_attitude; + struct vehicle_gps_position_s hil_gps; + struct sensor_combined_s hil_sensors; + struct battery_status_s hil_battery_status; + orb_advert_t pub_hil_global_pos; + orb_advert_t pub_hil_local_pos; + orb_advert_t pub_hil_attitude; + orb_advert_t pub_hil_gps; + orb_advert_t pub_hil_sensors; + orb_advert_t pub_hil_gyro; + orb_advert_t pub_hil_accel; + orb_advert_t pub_hil_mag; + orb_advert_t pub_hil_baro; + orb_advert_t pub_hil_airspeed; + orb_advert_t pub_hil_battery; + int hil_counter; + int hil_frames; + uint64_t old_timestamp; + orb_advert_t cmd_pub; + orb_advert_t flow_pub; + orb_advert_t offboard_control_sp_pub; + orb_advert_t vicon_position_pub; + orb_advert_t telemetry_status_pub; + int32_t lat0; + int32_t lon0; + double alt0; }; From 9c355d280eb379c72df636c1d47e5b14ae3e3e6e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 28 Jan 2014 15:13:14 +0100 Subject: [PATCH 030/381] Merged beta into mavlink rework branch --- Documentation/Doxyfile | 2 +- .../init.d/1000_rc_fw_easystar.hil | 58 +- .../px4fmu_common/init.d/10015_tbs_discovery | 85 +- ROMFS/px4fmu_common/init.d/10016_3dr_iris | 102 +- ROMFS/px4fmu_common/init.d/1001_rc_quad.hil | 105 -- ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil | 42 + .../px4fmu_common/init.d/1002_rc_fw_state.hil | 56 +- ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil | 105 +- .../init.d/1004_rc_fw_Rascal110.hil | 69 +- ROMFS/px4fmu_common/init.d/12001_octo_cox_pwm | 37 + ROMFS/px4fmu_common/init.d/2100_mpx_easystar | 61 +- ROMFS/px4fmu_common/init.d/2101_hk_bixler | 48 +- ROMFS/px4fmu_common/init.d/2102_3dr_skywalker | 48 +- ROMFS/px4fmu_common/init.d/3030_io_camflyer | 84 +- ROMFS/px4fmu_common/init.d/3031_io_phantom | 85 -- ROMFS/px4fmu_common/init.d/3031_phantom | 44 + ROMFS/px4fmu_common/init.d/3032_skywalker_x5 | 89 +- ROMFS/px4fmu_common/init.d/3033_io_wingwing | 84 -- ROMFS/px4fmu_common/init.d/3033_wingwing | 43 + ROMFS/px4fmu_common/init.d/3034_fx79 | 43 + ROMFS/px4fmu_common/init.d/3034_io_fx79 | 84 -- ROMFS/px4fmu_common/init.d/4009_ardrone_flow | 2 +- ROMFS/px4fmu_common/init.d/4010_dji_f330 | 100 +- ROMFS/px4fmu_common/init.d/4011_dji_f450 | 91 +- ROMFS/px4fmu_common/init.d/4012_hk_x550 | 31 + ROMFS/px4fmu_common/init.d/40_io_segway | 51 - ROMFS/px4fmu_common/init.d/5001_quad_+_pwm | 37 + ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm | 37 + ROMFS/px4fmu_common/init.d/666_fmu_q_x550 | 76 -- ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm | 37 + ROMFS/px4fmu_common/init.d/8001_octo_x_pwm | 37 + ROMFS/px4fmu_common/init.d/800_sdlogger | 51 - ROMFS/px4fmu_common/init.d/9001_octo_+_pwm | 37 + ROMFS/px4fmu_common/init.d/cmp_test | 9 - ROMFS/px4fmu_common/init.d/rc.autostart | 195 ++++ .../init.d/rc.custom_dji_f330_mkblctrl | 113 -- ROMFS/px4fmu_common/init.d/rc.custom_io_esc | 120 -- ROMFS/px4fmu_common/init.d/rc.fixedwing | 34 - ROMFS/px4fmu_common/init.d/rc.fw_apps | 19 + ROMFS/px4fmu_common/init.d/rc.hexa | 94 -- ROMFS/px4fmu_common/init.d/rc.interface | 72 ++ ROMFS/px4fmu_common/init.d/rc.io | 36 +- ROMFS/px4fmu_common/init.d/rc.logging | 10 +- ROMFS/px4fmu_common/init.d/rc.mc_apps | 24 + ROMFS/px4fmu_common/init.d/rc.mc_interface | 49 - ROMFS/px4fmu_common/init.d/rc.multirotor | 39 - ROMFS/px4fmu_common/init.d/rc.octo | 94 -- ROMFS/px4fmu_common/init.d/rc.sensors | 23 +- ROMFS/px4fmu_common/init.d/rc.standalone | 13 - ROMFS/px4fmu_common/init.d/rc.usb | 33 - ROMFS/px4fmu_common/init.d/rcS | 785 +++++++------ ROMFS/px4fmu_test/init.d/rcS | 67 +- Tools/fsm_visualisation.py | 201 ++++ Tools/px4params/dokuwikiout.py | 30 +- Tools/px4params/dokuwikiout_listings.py | 27 + Tools/tests-host/Makefile | 10 +- Tools/tests-host/hrt.cpp | 16 + Tools/tests-host/mixer_test.cpp | 2 + Tools/tests-host/queue.h | 133 +++ makefiles/config_px4fmu-v1_backside.mk | 148 --- makefiles/config_px4fmu-v1_default.mk | 9 +- makefiles/config_px4fmu-v2_default.mk | 8 +- makefiles/config_px4fmu-v2_test.mk | 14 + nuttx-configs/px4fmu-v1/nsh/defconfig | 23 +- nuttx-configs/px4fmu-v2/nsh/defconfig | 20 +- src/drivers/airspeed/airspeed.cpp | 2 +- src/drivers/boards/px4fmu-v1/board_config.h | 1 + src/drivers/boards/px4fmu-v2/board_config.h | 8 +- src/drivers/boards/px4fmu-v2/px4fmu2_init.c | 15 +- src/drivers/boards/px4io-v1/board_config.h | 6 +- src/drivers/boards/px4io-v2/board_config.h | 8 +- src/drivers/boards/px4io-v2/px4iov2_init.c | 2 - src/drivers/drv_pwm_output.h | 4 + src/drivers/frsky_telemetry/frsky_data.c | 289 +++++ .../frsky_telemetry/frsky_data.h} | 19 +- src/drivers/frsky_telemetry/frsky_telemetry.c | 266 +++++ .../frsky_telemetry}/module.mk | 10 +- src/drivers/gps/gps.cpp | 220 ++-- src/drivers/hil/hil.cpp | 9 +- src/drivers/hmc5883/hmc5883.cpp | 160 +-- .../hott/hott_sensors/hott_sensors.cpp | 2 +- .../hott/hott_telemetry/hott_telemetry.cpp | 2 +- src/drivers/meas_airspeed/meas_airspeed.cpp | 26 +- src/drivers/ms5611/ms5611.cpp | 6 + src/drivers/px4fmu/fmu.cpp | 75 +- src/drivers/px4fmu/module.mk | 3 +- src/drivers/px4io/px4io.cpp | 223 ++-- src/drivers/px4io/px4io_uploader.cpp | 17 +- src/drivers/px4io/uploader.h | 2 +- src/drivers/rgbled/rgbled.cpp | 12 +- src/examples/fixedwing_control/main.c | 6 +- src/lib/conversion/rotation.cpp | 15 +- src/lib/conversion/rotation.h | 2 +- .../ecl/attitude_fw/ecl_pitch_controller.cpp | 2 +- .../ecl/attitude_fw/ecl_roll_controller.cpp | 2 +- .../ecl/attitude_fw/ecl_yaw_controller.cpp | 2 +- src/lib/ecl/l1/ecl_l1_pos_controller.cpp | 43 +- src/lib/ecl/l1/ecl_l1_pos_controller.h | 12 +- src/lib/external_lgpl/tecs/tecs.cpp | 6 +- src/lib/external_lgpl/tecs/tecs.h | 6 +- src/lib/mathlib/math/Dcm.cpp | 174 --- src/lib/mathlib/math/Dcm.hpp | 108 -- src/lib/mathlib/math/EulerAngles.cpp | 126 -- src/lib/mathlib/math/Matrix.cpp | 193 --- src/lib/mathlib/math/Matrix.hpp | 416 ++++++- src/lib/mathlib/math/Quaternion.cpp | 174 --- src/lib/mathlib/math/Quaternion.hpp | 138 ++- src/lib/mathlib/math/Vector.cpp | 100 -- src/lib/mathlib/math/Vector.hpp | 477 +++++++- src/lib/mathlib/math/Vector2f.cpp | 103 -- src/lib/mathlib/math/Vector3.cpp | 99 -- src/lib/mathlib/math/arm/Matrix.hpp | 292 ----- src/lib/mathlib/math/arm/Vector.hpp | 236 ---- src/lib/mathlib/math/generic/Matrix.hpp | 437 ------- src/lib/mathlib/math/generic/Vector.hpp | 245 ---- src/lib/mathlib/mathlib.h | 8 +- src/lib/mathlib/module.mk | 17 - .../version/version.h} | 8 +- src/mainpage.dox | 9 + .../att_pos_estimator_ekf/KalmanNav.cpp | 124 +- .../att_pos_estimator_ekf/KalmanNav.hpp | 20 +- .../att_pos_estimator_ekf/kalman_main.cpp | 2 +- .../attitude_estimator_ekf_main.cpp | 115 +- .../attitude_estimator_ekf_params.c | 13 + .../attitude_estimator_ekf_params.h | 4 + .../commander/accelerometer_calibration.cpp | 14 +- src/modules/commander/commander.cpp | 415 ++++--- src/modules/commander/commander_helper.cpp | 44 +- src/modules/commander/commander_helper.h | 7 +- src/modules/commander/commander_params.c | 7 +- .../commander/state_machine_helper.cpp | 168 +-- src/modules/commander/state_machine_helper.h | 4 +- src/modules/controllib/uorb/blocks.cpp | 6 +- src/modules/controllib/uorb/blocks.hpp | 8 +- src/modules/fixedwing_backside/fixedwing.cpp | 8 +- src/modules/fixedwing_backside/fixedwing.hpp | 2 +- .../fixedwing_pos_control_main.c | 2 +- .../fw_att_control/fw_att_control_main.cpp | 10 +- .../fw_pos_control_l1_main.cpp | 144 ++- .../fw_pos_control_l1_params.c | 67 +- src/modules/mavlink/mavlink_orb_listener.cpp | 38 +- src/modules/mavlink/mavlink_receiver.cpp | 18 +- src/modules/mavlink/mavlink_receiver.h | 3 +- src/modules/mavlink_onboard/orb_topics.h | 2 +- .../mc_att_control/mc_att_control_main.cpp | 779 +++++++++++++ .../mc_att_control/mc_att_control_params.c} | 23 +- .../module.mk | 11 +- .../mc_pos_control/mc_pos_control_main.cpp | 1031 +++++++++++++++++ .../mc_pos_control/mc_pos_control_params.c} | 63 +- .../module.mk | 11 +- .../multirotor_att_control_main.c | 465 -------- .../multirotor_attitude_control.c | 254 ---- .../multirotor_attitude_control.h | 65 -- .../multirotor_rate_control.c | 196 ---- .../multirotor_rate_control.h | 64 - .../multirotor_pos_control.c | 553 --------- .../multirotor_pos_control_params.c | 112 -- .../multirotor_pos_control_params.h | 101 -- .../multirotor_pos_control/thrust_pid.c | 189 --- .../multirotor_pos_control/thrust_pid.h | 76 -- src/modules/navigator/navigator_main.cpp | 1011 +++++++++------- src/modules/navigator/navigator_params.c | 7 +- .../position_estimator_inav_main.c | 628 ++++++---- .../position_estimator_inav_params.c | 57 +- .../position_estimator_inav_params.h | 34 +- src/modules/px4iofirmware/adc.c | 53 +- src/modules/px4iofirmware/controls.c | 11 + src/modules/px4iofirmware/dsm.c | 6 + src/modules/px4iofirmware/mixer.cpp | 30 +- src/modules/px4iofirmware/px4io.c | 82 +- src/modules/px4iofirmware/px4io.h | 8 +- src/modules/px4iofirmware/registers.c | 26 +- src/modules/px4iofirmware/safety.c | 8 +- src/modules/px4iofirmware/sbus.c | 26 +- src/modules/sdlog2/sdlog2.c | 376 ++++-- src/modules/sdlog2/sdlog2_messages.h | 39 +- src/modules/sensors/module.mk | 2 +- src/modules/sensors/sensor_params.c | 250 +++- src/modules/sensors/sensors.cpp | 106 +- .../systemlib/board_serial.c} | 54 +- .../systemlib/board_serial.h} | 17 +- src/modules/systemlib/bson/tinybson.c | 3 + src/modules/systemlib/module.mk | 6 +- src/modules/systemlib/otp.c | 224 ++++ src/modules/systemlib/otp.h | 151 +++ src/modules/systemlib/param/param.c | 35 +- src/modules/systemlib/pid/pid.c | 106 +- src/modules/systemlib/pid/pid.h | 41 +- src/modules/systemlib/pwm_limit/pwm_limit.c | 110 +- src/modules/systemlib/pwm_limit/pwm_limit.h | 20 +- src/modules/uORB/objects_common.cpp | 4 +- src/modules/uORB/topics/battery_status.h | 9 +- src/modules/uORB/topics/home_position.h | 2 +- ..._triplet.h => position_setpoint_triplet.h} | 41 +- src/modules/uORB/topics/vehicle_attitude.h | 1 + .../uORB/topics/vehicle_attitude_setpoint.h | 2 +- .../uORB/topics/vehicle_control_mode.h | 3 +- .../uORB/topics/vehicle_global_position.h | 17 +- .../uORB/topics/vehicle_local_position.h | 5 + src/modules/uORB/topics/vehicle_status.h | 16 +- src/systemcmds/eeprom/eeprom.c | 265 ----- .../Matrix.cpp => systemcmds/hw_ver/hw_ver.c} | 41 +- src/systemcmds/hw_ver/module.mk | 43 + src/systemcmds/{eeprom => mtd}/24xxxx_mtd.c | 0 src/systemcmds/mtd/module.mk | 6 + src/systemcmds/mtd/mtd.c | 493 ++++++++ src/systemcmds/nshterm/nshterm.c | 4 +- src/systemcmds/param/param.c | 47 +- src/systemcmds/ramtron/module.mk | 6 - src/systemcmds/ramtron/ramtron.c | 279 ----- src/systemcmds/tests/module.mk | 6 +- .../tests/test_conv.cpp} | 66 +- src/systemcmds/tests/test_file.c | 163 ++- src/systemcmds/tests/test_mathlib.cpp | 159 +++ src/systemcmds/tests/test_mixer.cpp | 197 +++- src/systemcmds/tests/test_mount.c | 289 +++++ src/systemcmds/tests/test_mtd.c | 229 ++++ src/systemcmds/tests/test_rc.c | 4 +- src/systemcmds/tests/tests.h | 6 +- src/systemcmds/tests/tests_main.c | 4 + 220 files changed, 10717 insertions(+), 9729 deletions(-) delete mode 100644 ROMFS/px4fmu_common/init.d/1001_rc_quad.hil create mode 100644 ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil create mode 100644 ROMFS/px4fmu_common/init.d/12001_octo_cox_pwm delete mode 100644 ROMFS/px4fmu_common/init.d/3031_io_phantom create mode 100644 ROMFS/px4fmu_common/init.d/3031_phantom delete mode 100644 ROMFS/px4fmu_common/init.d/3033_io_wingwing create mode 100644 ROMFS/px4fmu_common/init.d/3033_wingwing create mode 100644 ROMFS/px4fmu_common/init.d/3034_fx79 delete mode 100644 ROMFS/px4fmu_common/init.d/3034_io_fx79 create mode 100644 ROMFS/px4fmu_common/init.d/4012_hk_x550 delete mode 100644 ROMFS/px4fmu_common/init.d/40_io_segway create mode 100644 ROMFS/px4fmu_common/init.d/5001_quad_+_pwm create mode 100644 ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm delete mode 100644 ROMFS/px4fmu_common/init.d/666_fmu_q_x550 create mode 100644 ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm create mode 100644 ROMFS/px4fmu_common/init.d/8001_octo_x_pwm delete mode 100644 ROMFS/px4fmu_common/init.d/800_sdlogger create mode 100644 ROMFS/px4fmu_common/init.d/9001_octo_+_pwm delete mode 100644 ROMFS/px4fmu_common/init.d/cmp_test create mode 100644 ROMFS/px4fmu_common/init.d/rc.autostart delete mode 100644 ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl delete mode 100644 ROMFS/px4fmu_common/init.d/rc.custom_io_esc delete mode 100644 ROMFS/px4fmu_common/init.d/rc.fixedwing create mode 100644 ROMFS/px4fmu_common/init.d/rc.fw_apps delete mode 100644 ROMFS/px4fmu_common/init.d/rc.hexa create mode 100644 ROMFS/px4fmu_common/init.d/rc.interface create mode 100644 ROMFS/px4fmu_common/init.d/rc.mc_apps delete mode 100644 ROMFS/px4fmu_common/init.d/rc.mc_interface delete mode 100644 ROMFS/px4fmu_common/init.d/rc.multirotor delete mode 100644 ROMFS/px4fmu_common/init.d/rc.octo delete mode 100644 ROMFS/px4fmu_common/init.d/rc.standalone create mode 100755 Tools/fsm_visualisation.py create mode 100644 Tools/px4params/dokuwikiout_listings.py create mode 100644 Tools/tests-host/hrt.cpp create mode 100644 Tools/tests-host/queue.h delete mode 100644 makefiles/config_px4fmu-v1_backside.mk create mode 100644 src/drivers/frsky_telemetry/frsky_data.c rename src/{lib/mathlib/math/generic/Vector.cpp => drivers/frsky_telemetry/frsky_data.h} (79%) create mode 100644 src/drivers/frsky_telemetry/frsky_telemetry.c rename src/{systemcmds/eeprom => drivers/frsky_telemetry}/module.mk (89%) delete mode 100644 src/lib/mathlib/math/Dcm.cpp delete mode 100644 src/lib/mathlib/math/Dcm.hpp delete mode 100644 src/lib/mathlib/math/EulerAngles.cpp delete mode 100644 src/lib/mathlib/math/Matrix.cpp delete mode 100644 src/lib/mathlib/math/Quaternion.cpp delete mode 100644 src/lib/mathlib/math/Vector.cpp delete mode 100644 src/lib/mathlib/math/Vector2f.cpp delete mode 100644 src/lib/mathlib/math/Vector3.cpp delete mode 100644 src/lib/mathlib/math/arm/Matrix.hpp delete mode 100644 src/lib/mathlib/math/arm/Vector.hpp delete mode 100644 src/lib/mathlib/math/generic/Matrix.hpp delete mode 100644 src/lib/mathlib/math/generic/Vector.hpp rename src/{modules/sdlog2/sdlog2_version.h => lib/version/version.h} (95%) create mode 100644 src/mainpage.dox create mode 100644 src/modules/mc_att_control/mc_att_control_main.cpp rename src/{lib/mathlib/math/arm/Vector.cpp => modules/mc_att_control/mc_att_control_params.c} (68%) rename src/modules/{multirotor_pos_control => mc_att_control}/module.mk (87%) create mode 100644 src/modules/mc_pos_control/mc_pos_control_main.cpp rename src/{lib/mathlib/math/Vector2f.hpp => modules/mc_pos_control/mc_pos_control_params.c} (67%) rename src/modules/{multirotor_att_control => mc_pos_control}/module.mk (86%) mode change 100755 => 100644 delete mode 100644 src/modules/multirotor_att_control/multirotor_att_control_main.c delete mode 100644 src/modules/multirotor_att_control/multirotor_attitude_control.c delete mode 100644 src/modules/multirotor_att_control/multirotor_attitude_control.h delete mode 100644 src/modules/multirotor_att_control/multirotor_rate_control.c delete mode 100644 src/modules/multirotor_att_control/multirotor_rate_control.h delete mode 100644 src/modules/multirotor_pos_control/multirotor_pos_control.c delete mode 100644 src/modules/multirotor_pos_control/multirotor_pos_control_params.c delete mode 100644 src/modules/multirotor_pos_control/multirotor_pos_control_params.h delete mode 100644 src/modules/multirotor_pos_control/thrust_pid.c delete mode 100644 src/modules/multirotor_pos_control/thrust_pid.h rename src/{lib/mathlib/math/Vector3.hpp => modules/systemlib/board_serial.c} (65%) rename src/{lib/mathlib/math/arm/Matrix.cpp => modules/systemlib/board_serial.h} (84%) create mode 100644 src/modules/systemlib/otp.c create mode 100644 src/modules/systemlib/otp.h rename src/modules/uORB/topics/{mission_item_triplet.h => position_setpoint_triplet.h} (72%) delete mode 100644 src/systemcmds/eeprom/eeprom.c rename src/{lib/mathlib/math/generic/Matrix.cpp => systemcmds/hw_ver/hw_ver.c} (69%) create mode 100644 src/systemcmds/hw_ver/module.mk rename src/systemcmds/{eeprom => mtd}/24xxxx_mtd.c (100%) create mode 100644 src/systemcmds/mtd/module.mk create mode 100644 src/systemcmds/mtd/mtd.c delete mode 100644 src/systemcmds/ramtron/module.mk delete mode 100644 src/systemcmds/ramtron/ramtron.c rename src/{lib/mathlib/math/EulerAngles.hpp => systemcmds/tests/test_conv.cpp} (66%) create mode 100644 src/systemcmds/tests/test_mathlib.cpp create mode 100644 src/systemcmds/tests/test_mount.c create mode 100644 src/systemcmds/tests/test_mtd.c diff --git a/Documentation/Doxyfile b/Documentation/Doxyfile index 2417028114..b674fbc485 100644 --- a/Documentation/Doxyfile +++ b/Documentation/Doxyfile @@ -599,7 +599,7 @@ RECURSIVE = YES # excluded from the INPUT source files. This way you can easily exclude a # subdirectory from a directory tree whose root is specified with the INPUT tag. -EXCLUDE = ../src/modules/mathlib/CMSIS \ +EXCLUDE = ../src/lib/mathlib/CMSIS \ ../src/modules/attitude_estimator_ekf/codegen # The EXCLUDE_SYMLINKS tag can be used select whether or not files or 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 40a13b5d17..ebe8a1a1ea 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,13 @@ #!nsh # -# USB HIL start +# HILStar / X-Plane +# +# Maintainers: Thomas Gubler # -echo "[HIL] HILStar starting.." +echo "HIL Rascal 110 starting.." -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then # Set all params here, then disable autoconfig @@ -40,48 +39,7 @@ then 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 - -echo "[HIL] setup done, running" +set HIL yes +set VEHICLE_TYPE fw +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 81d4b5d570..56c74a3b52 100644 --- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery @@ -1,74 +1,29 @@ #!nsh - -echo "[init] Team Blacksheep Discovery Quad" - # -# Load default params for this platform +# Team Blacksheep Discovery Quadcopter # -if param compare SYS_AUTOCONFIG 1 +# Maintainers: Simon Wilks +# + +if [ $DO_AUTOCONFIG == yes ] then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 - - param set MC_ATTRATE_D 0.006 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.17 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 + # + # Default parameters for this platform + # param set MC_ATT_P 5.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.15 - param set MC_YAWPOS_P 0.5 - param set MC_YAWRATE_D 0.0 - param set MC_YAWRATE_I 0.0 + param set MC_ATT_I 0.0 + param set MC_YAW_P 0.5 + param set MC_YAW_I 0.15 + param set MC_ATTRATE_P 0.17 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.006 param set MC_YAWRATE_P 0.2 - - param save + param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_D 0.0 fi -# -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor -# -param set MAV_TYPE 2 +set VEHICLE_TYPE mc +set MIXER FMU_quad_w -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - usleep 5000 - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -# -# Load the mixer for a quad with wide arms -# -mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix - -# -# Set PWM output frequency -# -pwm rate -c 1234 -r 400 - -# -# Set disarmed, min and max PWM signals -# -pwm disarmed -c 1234 -p 900 -pwm min -c 1234 -p 1100 -pwm max -c 1234 -p 1900 - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor +set PWM_OUTPUTS 1234 +set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris index b0f4eda79e..a3bcb63eb8 100644 --- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -1,73 +1,49 @@ #!nsh - -echo "[init] 3DR Iris Quad" - # -# Load default params for this platform +# 3DR Iris Quadcopter # -if param compare SYS_AUTOCONFIG 1 +# Maintainers: Anton Babushkin +# + +if [ $DO_AUTOCONFIG == yes ] then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 - - param set MC_ATTRATE_D 0.004 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.13 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 + # + # Default parameters for this platform + # param set MC_ATT_P 9.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.15 - param set MC_YAWPOS_P 0.5 - param set MC_YAWRATE_D 0.0 - param set MC_YAWRATE_I 0.0 + param set MC_ATT_I 0.0 + param set MC_YAW_P 0.5 + param set MC_YAW_I 0.15 + param set MC_ATTRATE_P 0.13 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 param set MC_YAWRATE_P 0.2 + param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_D 0.0 + + param set MPC_TILT_MAX 1.0 + param set MPC_THR_MAX 1.0 + param set MPC_THR_MIN 0.1 + param set MPC_XY_P 1.0 + param set MPC_XY_VEL_D 0.01 + param set MPC_XY_VEL_I 0.02 + param set MPC_XY_VEL_MAX 5 + param set MPC_XY_VEL_P 0.1 + param set MPC_Z_P 1.0 + param set MPC_Z_VEL_D 0.0 + param set MPC_Z_VEL_I 0.02 + param set MPC_Z_VEL_MAX 3 + param set MPC_Z_VEL_P 0.1 - param save + param set BAT_V_SCALING 0.00989 + param set BAT_C_SCALING 0.0124 fi -# -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor -# -param set MAV_TYPE 2 +set VEHICLE_TYPE mc +set MIXER FMU_quad_w -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - usleep 5000 - fmu mode_pwm - param set BAT_V_SCALING 0.0098 - set EXIT_ON_END yes -fi - -# -# Load the mixer for a quad with wide arms -# -mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix - -# -# Set PWM output frequency -# -pwm rate -c 1234 -r 400 - -# -# Set disarmed, min and max PWM signals -# -pwm disarmed -c 1234 -p 900 -pwm min -c 1234 -p 1050 - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor +set PWM_OUTPUTS 1234 +set PWM_RATE 400 +set PWM_DISARMED 900 +set PWM_MIN 1000 +set PWM_MAX 2000 diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil deleted file mode 100644 index 9b664d63ed..0000000000 --- a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil +++ /dev/null @@ -1,105 +0,0 @@ -#!nsh -# -# USB HIL start -# - -echo "[HIL] HILS quadrotor starting.." - -# -# 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 set MC_ATTRATE_D 0.0 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.05 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 - param set MC_ATT_P 3.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_P 2.1 - param set MC_YAWRATE_D 0.0 - param set MC_YAWRATE_I 0.0 - param set MC_YAWRATE_P 0.05 - param set NAV_TAKEOFF_ALT 3.0 - param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.5 - param set MPC_THR_MIN 0.1 - param set MPC_XY_D 0 - param set MPC_XY_P 0.5 - param set MPC_XY_VEL_D 0 - param set MPC_XY_VEL_I 0 - param set MPC_XY_VEL_MAX 3 - param set MPC_XY_VEL_P 0.2 - param set MPC_Z_D 0 - param set MPC_Z_P 1 - param set MPC_Z_VEL_D 0 - param set MPC_Z_VEL_I 0.1 - param set MPC_Z_VEL_MAX 2 - param set MPC_Z_VEL_P 0.20 - - param 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 2 - -# -# 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_quad_x.mix - -# -# Start position estimator -# -position_estimator_inav start - -# -# Start attitude control -# -multirotor_att_control start - -# -# Start position control -# -multirotor_pos_control start - -echo "[HIL] setup done, running" - diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil new file mode 100644 index 0000000000..2d497374a5 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil @@ -0,0 +1,42 @@ +#!nsh +# +# HIL Quadcopter X +# +# Maintainers: Anton Babushkin +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set MC_ATT_P 7.0 + param set MC_ATT_I 0.0 + param set MC_YAW_P 2.0 + param set MC_YAW_I 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + param set MPC_TILT_MAX 1.0 + param set MPC_THR_MAX 1.0 + param set MPC_THR_MIN 0.1 + param set MPC_XY_P 1.0 + param set MPC_XY_VEL_D 0.01 + param set MPC_XY_VEL_I 0.02 + param set MPC_XY_VEL_MAX 5 + param set MPC_XY_VEL_P 0.1 + param set MPC_Z_P 1.0 + param set MPC_Z_VEL_D 0.0 + param set MPC_Z_VEL_I 0.02 + param set MPC_Z_VEL_MAX 3 + param set MPC_Z_VEL_P 0.1 +fi + +set HIL yes + +set VEHICLE_TYPE mc +set MIXER FMU_quad_x diff --git a/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil b/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil index 7b9f41bf6d..46da24d35f 100644 --- a/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil +++ b/ROMFS/px4fmu_common/init.d/1002_rc_fw_state.hil @@ -1,14 +1,13 @@ #!nsh # -# USB HIL start +# HIL Rascal 110 (Flightgear) +# +# Maintainers: Thomas Gubler # -echo "[HIL] HILStar starting in state-HIL mode.." +echo "HIL Rascal 110 starting.." -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then # Set all params here, then disable autoconfig @@ -32,48 +31,15 @@ then 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 - -# -# 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 - -echo "[HIL] setup done, running" +set HIL yes +set VEHICLE_TYPE fw +set MIXER FMU_AERT diff --git a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil index 0cc07ad343..e958448912 100644 --- a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil +++ b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil @@ -1,105 +1,10 @@ #!nsh # -# USB HIL start +# HIL Quadcopter + +# +# Maintainers: Anton Babushkin # -echo "[HIL] HILS quadrotor + starting.." - -# -# 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 set MC_ATTRATE_D 0.0 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.05 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 - param set MC_ATT_P 3.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_P 2.1 - param set MC_YAWRATE_D 0.0 - param set MC_YAWRATE_I 0.0 - param set MC_YAWRATE_P 0.05 - param set NAV_TAKEOFF_ALT 3.0 - param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.5 - param set MPC_THR_MIN 0.1 - param set MPC_XY_D 0 - param set MPC_XY_P 0.5 - param set MPC_XY_VEL_D 0 - param set MPC_XY_VEL_I 0 - param set MPC_XY_VEL_MAX 3 - param set MPC_XY_VEL_P 0.2 - param set MPC_Z_D 0 - param set MPC_Z_P 1 - param set MPC_Z_VEL_D 0 - param set MPC_Z_VEL_I 0.1 - param set MPC_Z_VEL_MAX 2 - param set MPC_Z_VEL_P 0.20 - - param 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 2 - -# -# 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_quad_+.mix - -# -# Start position estimator -# -position_estimator_inav start - -# -# Start attitude control -# -multirotor_att_control start - -# -# Start position control -# -multirotor_pos_control start - -echo "[HIL] setup done, running" +sh /etc/init.d/1001_rc_quad_x.hil +set MIXER FMU_quad_+ 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 344d784224..46da24d35f 100644 --- a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil +++ b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil @@ -1,14 +1,13 @@ #!nsh # -# USB HIL start +# HIL Rascal 110 (Flightgear) +# +# Maintainers: Thomas Gubler # -echo "[HIL] HILStar starting.." +echo "HIL Rascal 110 starting.." -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then # Set all params here, then disable autoconfig @@ -40,59 +39,7 @@ then 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) -# -set MODE autostart -mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix -if [ -f /fs/microsd/etc/mixers/FMU_AERT.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_AERT.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_AERT.mix -else - echo "Using /etc/mixers/FMU_AERT.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix -fi - - -fw_pos_control_l1 start -fw_att_control start - -echo "[HIL] setup done, running" +set HIL yes +set VEHICLE_TYPE fw +set MIXER FMU_AERT diff --git a/ROMFS/px4fmu_common/init.d/12001_octo_cox_pwm b/ROMFS/px4fmu_common/init.d/12001_octo_cox_pwm new file mode 100644 index 0000000000..5f3cec4e0a --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/12001_octo_cox_pwm @@ -0,0 +1,37 @@ +#!nsh +# +# Generic 10” Octo coaxial geometry +# +# Maintainers: Lorenz Meier +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set MC_ATT_P 7.0 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_YAWPOS_P 2.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + # TODO add default MPC parameters +fi + +set VEHICLE_TYPE mc +set MIXER FMU_octo_cox + +set PWM_OUTPUTS 1234 +set PWM_RATE 400 +# DJI ESC range +set PWM_DISARMED 900 +set PWM_MIN 1200 +set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/2100_mpx_easystar b/ROMFS/px4fmu_common/init.d/2100_mpx_easystar index 97c2d7c903..0e5bf60d60 100644 --- a/ROMFS/px4fmu_common/init.d/2100_mpx_easystar +++ b/ROMFS/px4fmu_common/init.d/2100_mpx_easystar @@ -1,13 +1,15 @@ #!nsh - -echo "[init] PX4FMU v1, v2 with or without IO on EasyStar" - # -# Load default params for this platform +# MPX EasyStar Plane # -if param compare SYS_AUTOCONFIG 1 +# Maintainers: ??? +# + +if [ $DO_AUTOCONFIG == yes ] then - # Set all params here, then disable autoconfig + # + # Default parameters for this platform + # param set FW_P_D 0 param set FW_P_I 0 param set FW_P_IMAX 15 @@ -31,50 +33,7 @@ then param set FW_L1_PERIOD 16 param set RC_SCALE_ROLL 1.0 param set RC_SCALE_PITCH 1.0 - - param set SYS_AUTOCONFIG 0 - param save fi -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing -# -param set MAV_TYPE 1 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - - sh /etc/init.d/rc.io - # Limit to 100 Hz updates and (implicit) 50 Hz PWM - px4io limit 100 -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -# -# Load mixer and start controllers (depends on px4io) -# -if [ -f /fs/microsd/etc/mixers/FMU_RET.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_RET.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_RET.mix -else - echo "Using /etc/mixers/FMU_RET.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_RET.mix -fi - -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing +set VEHICLE_TYPE fw +set MIXER FMU_RET diff --git a/ROMFS/px4fmu_common/init.d/2101_hk_bixler b/ROMFS/px4fmu_common/init.d/2101_hk_bixler index 995d3ba074..1ed923b19c 100644 --- a/ROMFS/px4fmu_common/init.d/2101_hk_bixler +++ b/ROMFS/px4fmu_common/init.d/2101_hk_bixler @@ -1,11 +1,11 @@ #!nsh -echo "[init] PX4FMU v1, v2 with or without IO on HK Bixler" +echo "[init] PX4FMU v1, v2 with or without IO on 3DR SkyWalker" # # Load default params for this platform # -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then # Set all params here, then disable autoconfig param set FW_P_D 0 @@ -35,46 +35,6 @@ then param set SYS_AUTOCONFIG 0 param save fi - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing -# -param set MAV_TYPE 1 -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - - sh /etc/init.d/rc.io - # Limit to 100 Hz updates and (implicit) 50 Hz PWM - px4io limit 100 -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -# -# Load mixer and start controllers (depends on px4io) -# -if [ -f /fs/microsd/etc/mixers/FMU_AERT.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_AERT.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_AERT.mix -else - echo "Using /etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix -fi - -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing +set VEHICLE_TYPE fw +set MIXER FMU_AERT \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker b/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker index a6d2ace962..1ed923b19c 100644 --- a/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker +++ b/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker @@ -5,7 +5,7 @@ echo "[init] PX4FMU v1, v2 with or without IO on 3DR SkyWalker" # # Load default params for this platform # -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then # Set all params here, then disable autoconfig param set FW_P_D 0 @@ -35,48 +35,6 @@ then param set SYS_AUTOCONFIG 0 param save fi - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing -# -param set MAV_TYPE 1 -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - - sh /etc/init.d/rc.io - # Limit to 100 Hz updates and (implicit) 50 Hz PWM - px4io limit 100 -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -pwm disarmed -c 3 -p 1056 - -# -# Load mixer and start controllers (depends on px4io) -# -if [ -f /fs/microsd/etc/mixers/FMU_AERT.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_AERT.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_AETR.mix -else - echo "Using /etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_AETR.mix -fi - -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing +set VEHICLE_TYPE fw +set MIXER FMU_AERT \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/3030_io_camflyer b/ROMFS/px4fmu_common/init.d/3030_io_camflyer index 65f01c9743..cbcc6189b6 100644 --- a/ROMFS/px4fmu_common/init.d/3030_io_camflyer +++ b/ROMFS/px4fmu_common/init.d/3030_io_camflyer @@ -2,57 +2,39 @@ echo "[init] PX4FMU v1, v2 with or without IO on Camflyer" -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 +if [ $DO_AUTOCONFIG == yes ] then - # Set all params here, then disable autoconfig - # TODO - - param set SYS_AUTOCONFIG 0 - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing -# -param set MAV_TYPE 1 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - - sh /etc/init.d/rc.io - # Limit to 100 Hz updates and (implicit) 50 Hz PWM - px4io limit 100 -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes + # + # Default parameters for this platform + # + param set FW_AIRSPD_MIN 7 + param set FW_AIRSPD_TRIM 9 + param set FW_AIRSPD_MAX 14 + param set FW_L1_PERIOD 10 + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 20 + param set FW_P_LIM_MAX 30 + param set FW_P_LIM_MIN -20 + param set FW_P_P 30 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 2 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 20 + param set FW_R_P 60 + param set FW_R_RMAX 60 + param set FW_THR_CRUISE 0.65 + param set FW_THR_MAX 0.7 + param set FW_THR_MIN 0 + param set FW_T_SINK_MAX 5 + param set FW_T_SINK_MIN 2 + param set FW_T_TIME_CONST 9 + param set FW_Y_ROLLFF 2.0 + param set RC_SCALE_ROLL 1.0 + param set RC_SCALE_PITCH 1.0 fi -# -# Load mixer and start controllers (depends on px4io) -# -if [ -f /fs/microsd/etc/mixers/FMU_Q.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix -else - echo "Using /etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix -fi - -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing +set VEHICLE_TYPE fw +set MIXER FMU_Q diff --git a/ROMFS/px4fmu_common/init.d/3031_io_phantom b/ROMFS/px4fmu_common/init.d/3031_io_phantom deleted file mode 100644 index 0cf6ee39a5..0000000000 --- a/ROMFS/px4fmu_common/init.d/3031_io_phantom +++ /dev/null @@ -1,85 +0,0 @@ -#!nsh - -echo "[init] PX4FMU v1, v2 with or without IO on Phantom FPV" - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set FW_AIRSPD_MIN 11.4 - param set FW_AIRSPD_TRIM 14 - param set FW_AIRSPD_MAX 22 - param set FW_L1_PERIOD 15 - param set FW_P_D 0 - param set FW_P_I 0 - param set FW_P_IMAX 15 - param set FW_P_LIM_MAX 45 - param set FW_P_LIM_MIN -45 - param set FW_P_P 60 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 - param set FW_P_ROLLFF 2 - param set FW_R_D 0 - param set FW_R_I 5 - param set FW_R_IMAX 15 - param set FW_R_P 80 - param set FW_R_RMAX 60 - param set FW_THR_CRUISE 0.8 - param set FW_THR_LND_MAX 0 - param set FW_THR_MAX 1 - param set FW_THR_MIN 0.5 - param set FW_T_SINK_MAX 5.0 - param set FW_T_SINK_MIN 2.0 - param set FW_Y_ROLLFF 1.0 - param set RC_SCALE_ROLL 0.6 - param set RC_SCALE_PITCH 0.6 - param set TRIM_PITCH 0.1 - - param set SYS_AUTOCONFIG 0 - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing -# -param set MAV_TYPE 1 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - - sh /etc/init.d/rc.io - # Limit to 100 Hz updates and (implicit) 50 Hz PWM - px4io limit 100 -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -# -# Load mixer and start controllers (depends on px4io) -# -if [ -f /fs/microsd/etc/mixers/FMU_Q.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix -else - echo "Using /etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix -fi - -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing diff --git a/ROMFS/px4fmu_common/init.d/3031_phantom b/ROMFS/px4fmu_common/init.d/3031_phantom new file mode 100644 index 0000000000..4ebbe9c61e --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/3031_phantom @@ -0,0 +1,44 @@ +#!nsh +# +# Phantom FPV Flying Wing +# +# Maintainers: Simon Wilks +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set FW_AIRSPD_MIN 11.4 + param set FW_AIRSPD_TRIM 14 + param set FW_AIRSPD_MAX 22 + param set FW_L1_PERIOD 15 + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 15 + param set FW_P_LIM_MAX 45 + param set FW_P_LIM_MIN -45 + param set FW_P_P 60 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 2 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 15 + param set FW_R_P 80 + param set FW_R_RMAX 60 + param set FW_THR_CRUISE 0.8 + param set FW_THR_LND_MAX 0 + param set FW_THR_MAX 1 + param set FW_THR_MIN 0.5 + param set FW_T_SINK_MAX 5.0 + param set FW_T_SINK_MIN 2.0 + param set FW_Y_ROLLFF 1.0 + param set RC_SCALE_ROLL 0.6 + param set RC_SCALE_PITCH 0.6 + param set TRIM_PITCH 0.1 +fi + +set VEHICLE_TYPE fw +set MIXER FMU_Q diff --git a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 index 41e0416548..143310af95 100644 --- a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 +++ b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 @@ -1,58 +1,43 @@ #!nsh - -echo "[init] PX4FMU v1, v2 with or without IO on Skywalker X5" - # -# Load default params for this platform +# Skywalker X5 Flying Wing # -if param compare SYS_AUTOCONFIG 1 +# Maintainers: Thomas Gubler , Julian Oes +# + +if [ $DO_AUTOCONFIG == yes ] then - # Set all params here, then disable autoconfig - # TODO - - param set SYS_AUTOCONFIG 0 - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing -# -param set MAV_TYPE 1 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - - sh /etc/init.d/rc.io - # Limit to 100 Hz updates and (implicit) 50 Hz PWM - px4io limit 100 -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes + # + # Default parameters for this platform + # + param set FW_AIRSPD_MIN 7 + param set FW_AIRSPD_TRIM 9 + param set FW_AIRSPD_MAX 14 + param set FW_L1_PERIOD 10 + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 20 + param set FW_P_LIM_MAX 30 + param set FW_P_LIM_MIN -20 + param set FW_P_P 30 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 2 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 20 + param set FW_R_P 60 + param set FW_R_RMAX 60 + param set FW_THR_CRUISE 0.65 + param set FW_THR_MAX 0.7 + param set FW_THR_MIN 0 + param set FW_T_SINK_MAX 5 + param set FW_T_SINK_MIN 2 + param set FW_T_TIME_CONST 9 + param set FW_Y_ROLLFF 2.0 + param set RC_SCALE_ROLL 1.0 + param set RC_SCALE_PITCH 1.0 fi -# -# Load mixer and start controllers (depends on px4io) -# -if [ -f /fs/microsd/etc/mixers/FMU_Q.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix -else - echo "Using /etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix -fi - -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing +set VEHICLE_TYPE fw +set MIXER FMU_X5 diff --git a/ROMFS/px4fmu_common/init.d/3033_io_wingwing b/ROMFS/px4fmu_common/init.d/3033_io_wingwing deleted file mode 100644 index 82ff425e6d..0000000000 --- a/ROMFS/px4fmu_common/init.d/3033_io_wingwing +++ /dev/null @@ -1,84 +0,0 @@ -#!nsh - -echo "[init] PX4FMU v1, v2 with or without IO on the Wing Wing (aka Z-84)" - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set FW_AIRSPD_MIN 7 - param set FW_AIRSPD_TRIM 9 - param set FW_AIRSPD_MAX 14 - param set FW_L1_PERIOD 10 - param set FW_P_D 0 - param set FW_P_I 0 - param set FW_P_IMAX 20 - param set FW_P_LIM_MAX 30 - param set FW_P_LIM_MIN -20 - param set FW_P_P 30 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 - param set FW_P_ROLLFF 2 - param set FW_R_D 0 - param set FW_R_I 5 - param set FW_R_IMAX 20 - param set FW_R_P 60 - param set FW_R_RMAX 60 - param set FW_THR_CRUISE 0.65 - param set FW_THR_MAX 0.7 - param set FW_THR_MIN 0 - param set FW_T_SINK_MAX 5 - param set FW_T_SINK_MIN 2 - param set FW_T_TIME_CONST 9 - param set FW_Y_ROLLFF 2.0 - param set RC_SCALE_ROLL 1.0 - param set RC_SCALE_PITCH 1.0 - - param set SYS_AUTOCONFIG 0 - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing -# -param set MAV_TYPE 1 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - - sh /etc/init.d/rc.io - # Limit to 100 Hz updates and (implicit) 50 Hz PWM - px4io limit 100 -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -# -# Load mixer and start controllers (depends on px4io) -# -if [ -f /fs/microsd/etc/mixers/FMU_Q.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix -else - echo "Using /etc/mixers/FMU_Q.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix -fi - -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing diff --git a/ROMFS/px4fmu_common/init.d/3033_wingwing b/ROMFS/px4fmu_common/init.d/3033_wingwing new file mode 100644 index 0000000000..e537632784 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/3033_wingwing @@ -0,0 +1,43 @@ +#!nsh +# +# Wing Wing (aka Z-84) Flying Wing +# +# Maintainers: Simon Wilks +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set FW_AIRSPD_MIN 7 + param set FW_AIRSPD_TRIM 9 + param set FW_AIRSPD_MAX 14 + param set FW_L1_PERIOD 10 + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 20 + param set FW_P_LIM_MAX 30 + param set FW_P_LIM_MIN -20 + param set FW_P_P 30 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 2 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 20 + param set FW_R_P 60 + param set FW_R_RMAX 60 + param set FW_THR_CRUISE 0.65 + param set FW_THR_MAX 0.7 + param set FW_THR_MIN 0 + param set FW_T_SINK_MAX 5 + param set FW_T_SINK_MIN 2 + param set FW_T_TIME_CONST 9 + param set FW_Y_ROLLFF 2.0 + param set RC_SCALE_ROLL 1.0 + param set RC_SCALE_PITCH 1.0 +fi + +set VEHICLE_TYPE fw +set MIXER FMU_Q diff --git a/ROMFS/px4fmu_common/init.d/3034_fx79 b/ROMFS/px4fmu_common/init.d/3034_fx79 new file mode 100644 index 0000000000..8d179d1fd4 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/3034_fx79 @@ -0,0 +1,43 @@ +#!nsh +# +# FX-79 Buffalo Flying Wing +# +# Maintainers: Simon Wilks +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set FW_AIRSPD_MAX 20 + param set FW_AIRSPD_TRIM 12 + param set FW_AIRSPD_MIN 15 + param set FW_L1_PERIOD 12 + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 15 + param set FW_P_LIM_MAX 50 + param set FW_P_LIM_MIN -50 + param set FW_P_P 60 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 1.1 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 20 + param set FW_R_P 80 + param set FW_R_RMAX 100 + param set FW_THR_CRUISE 0.75 + param set FW_THR_MAX 1 + param set FW_THR_MIN 0 + param set FW_T_SINK_MAX 5.0 + param set FW_T_SINK_MIN 4.0 + param set FW_T_TIME_CONST 9 + param set FW_Y_ROLLFF 1.1 + param set RC_SCALE_ROLL 1.0 + param set RC_SCALE_PITCH 1.0 +fi + +set VEHICLE_TYPE fw +set MIXER FMU_FX79 diff --git a/ROMFS/px4fmu_common/init.d/3034_io_fx79 b/ROMFS/px4fmu_common/init.d/3034_io_fx79 deleted file mode 100644 index 759c17bb44..0000000000 --- a/ROMFS/px4fmu_common/init.d/3034_io_fx79 +++ /dev/null @@ -1,84 +0,0 @@ -#!nsh - -echo "[init] PX4FMU v1, v2 with or without IO on FX-79 Buffalo" - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set FW_AIRSPD_MAX 20 - param set FW_AIRSPD_TRIM 12 - param set FW_AIRSPD_MIN 15 - param set FW_L1_PERIOD 12 - param set FW_P_D 0 - param set FW_P_I 0 - param set FW_P_IMAX 15 - param set FW_P_LIM_MAX 50 - param set FW_P_LIM_MIN -50 - param set FW_P_P 60 - param set FW_P_RMAX_NEG 0 - param set FW_P_RMAX_POS 0 - param set FW_P_ROLLFF 1.1 - param set FW_R_D 0 - param set FW_R_I 5 - param set FW_R_IMAX 20 - param set FW_R_P 80 - param set FW_R_RMAX 100 - param set FW_THR_CRUISE 0.75 - param set FW_THR_MAX 1 - param set FW_THR_MIN 0 - param set FW_T_SINK_MAX 5.0 - param set FW_T_SINK_MIN 4.0 - param set FW_T_TIME_CONST 9 - param set FW_Y_ROLLFF 1.1 - param set RC_SCALE_ROLL 1.0 - param set RC_SCALE_PITCH 1.0 - - param set SYS_AUTOCONFIG 0 - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 1 = fixed wing -# -param set MAV_TYPE 1 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - - sh /etc/init.d/rc.io - # Limit to 100 Hz updates and (implicit) 50 Hz PWM - px4io limit 100 -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -# -# Load mixer and start controllers (depends on px4io) -# -if [ -f /fs/microsd/etc/mixers/FMU_FX79.mix ] -then - echo "Using /fs/microsd/etc/mixers/FMU_FX79.mix" - mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_FX79.mix -else - echo "Using /etc/mixers/FMU_FX79.mix" - mixer load /dev/pwm_output /etc/mixers/FMU_FX79.mix -fi - -# -# Start common fixedwing apps -# -sh /etc/init.d/rc.fixedwing diff --git a/ROMFS/px4fmu_common/init.d/4009_ardrone_flow b/ROMFS/px4fmu_common/init.d/4009_ardrone_flow index 2886bcb752..e2cb8833dc 100644 --- a/ROMFS/px4fmu_common/init.d/4009_ardrone_flow +++ b/ROMFS/px4fmu_common/init.d/4009_ardrone_flow @@ -67,7 +67,7 @@ flow_position_estimator start # # Fire up the multi rotor attitude controller # -multirotor_att_control start +mc_att_control_vector start # # Fire up the flow position controller diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330 index 7054210e24..e0cf92d97f 100644 --- a/ROMFS/px4fmu_common/init.d/4010_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330 @@ -1,69 +1,47 @@ #!nsh - -echo "[init] PX4FMU v1, v2 with or without IO on DJI F330" - # -# Load default params for this platform +# DJI Flame Wheel F330 Quadcopter # -if param compare SYS_AUTOCONFIG 1 +# Maintainers: Anton Babushkin +# + +if [ $DO_AUTOCONFIG == yes ] then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 - - param set MC_ATTRATE_D 0.004 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.12 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 + # + # Default parameters for this platform + # param set MC_ATT_P 7.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_P 2.0 - param set MC_YAWRATE_D 0.005 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_P 0.3 - param set NAV_TAKEOFF_ALT 3.0 - param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.7 - param set MPC_THR_MIN 0.3 - param set MPC_XY_D 0 - param set MPC_XY_P 0.5 - param set MPC_XY_VEL_D 0 - param set MPC_XY_VEL_I 0 - param set MPC_XY_VEL_MAX 3 - param set MPC_XY_VEL_P 0.2 - param set MPC_Z_D 0 - param set MPC_Z_P 1 - param set MPC_Z_VEL_D 0 - param set MPC_Z_VEL_I 0.1 - param set MPC_Z_VEL_MAX 2 - param set MPC_Z_VEL_P 0.20 - - param save + param set MC_ATT_I 0.0 + param set MC_YAW_P 2.8 + param set MC_YAW_I 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_YAWRATE_P 0.2 + param set MC_YAWRATE_I 0.05 + param set MC_YAWRATE_D 0.0 + + param set MPC_TILT_MAX 1.0 + param set MPC_THR_MAX 1.0 + param set MPC_THR_MIN 0.1 + param set MPC_XY_P 1.0 + param set MPC_XY_VEL_D 0.01 + param set MPC_XY_VEL_I 0.02 + param set MPC_XY_VEL_MAX 5 + param set MPC_XY_VEL_P 0.1 + param set MPC_Z_P 1.0 + param set MPC_Z_VEL_D 0.0 + param set MPC_Z_VEL_I 0.02 + param set MPC_Z_VEL_MAX 3 + param set MPC_Z_VEL_P 0.1 fi -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 +set VEHICLE_TYPE mc +set MIXER FMU_quad_x - sh /etc/init.d/rc.io -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - usleep 5000 - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -sh /etc/init.d/rc.mc_interface - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor +set PWM_OUTPUTS 1234 +set PWM_RATE 400 +# DJI ESC range +set PWM_DISARMED 900 +set PWM_MIN 1200 +set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 index a1d253191d..ced69783db 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -1,74 +1,35 @@ #!nsh - -echo "[init] PX4FMU v1, v2 with or without IO on DJI F450" - # -# Load default params for this platform +# DJI Flame Wheel F450 Quadcopter # -if param compare SYS_AUTOCONFIG 1 +# Maintainers: Lorenz Meier +# + +if [ $DO_AUTOCONFIG == yes ] then - # Set all params here, then disable autoconfig - param set SYS_AUTOCONFIG 0 - - param set MC_ATTRATE_D 0.004 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.12 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 + # + # Default parameters for this platform + # param set MC_ATT_P 7.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_P 2.0 - param set MC_YAWRATE_D 0.005 - param set MC_YAWRATE_I 0.2 + param set MC_ATT_I 0.0 + param set MC_YAW_P 2.0 + param set MC_YAW_I 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 param set MC_YAWRATE_P 0.3 - - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor -# -param set MAV_TYPE 2 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - usleep 5000 - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + # TODO add default MPC parameters fi -# -# Load mixer -# -mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix +set VEHICLE_TYPE mc +set MIXER FMU_quad_x -# -# Set PWM output frequency -# -pwm rate -c 1234 -r 400 - -# -# Set disarmed, min and max PWM signals (for DJI ESCs) -# -pwm disarmed -c 1234 -p 900 -pwm min -c 1234 -p 1200 -pwm max -c 1234 -p 1800 - -# -# Start common multirotor apps -# -sh /etc/init.d/rc.multirotor +set PWM_OUTPUTS 1234 +set PWM_RATE 400 +# DJI ESC range +set PWM_DISARMED 900 +set PWM_MIN 1200 +set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/4012_hk_x550 b/ROMFS/px4fmu_common/init.d/4012_hk_x550 new file mode 100644 index 0000000000..e1423e0082 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/4012_hk_x550 @@ -0,0 +1,31 @@ +#!nsh +# +# HobbyKing X550 Quadcopter +# +# Maintainers: Todd Stellanova +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set MC_ATT_P 5.5 + param set MC_ATT_I 0 + param set MC_YAW_P 0.6 + param set MC_YAW_I 0 + param set MC_ATTRATE_P 0.14 + param set MC_ATTRATE_I 0 + param set MC_ATTRATE_D 0.006 + param set MC_YAWRATE_P 0.08 + param set MC_YAWRATE_I 0 + param set MC_YAWRATE_D 0 + + # TODO add default MPC parameters +fi + +set VEHICLE_TYPE mc +set MIXER FMU_quad_x + +set PWM_OUTPUTS 1234 +set PWM_RATE 400 diff --git a/ROMFS/px4fmu_common/init.d/40_io_segway b/ROMFS/px4fmu_common/init.d/40_io_segway deleted file mode 100644 index ad455b4403..0000000000 --- a/ROMFS/px4fmu_common/init.d/40_io_segway +++ /dev/null @@ -1,51 +0,0 @@ -#!nsh - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - # TODO - - param set SYS_AUTOCONFIG 0 - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 10 = ground rover -# -param set MAV_TYPE 10 - -# -# Start MAVLink (depends on orb) -# -mavlink start -d /dev/ttyS1 -b 57600 -usleep 5000 - -# -# Start and configure PX4IO interface -# -sh /etc/init.d/rc.io - -# -# Start the sensors (depends on orb, px4io) -# -sh /etc/init.d/rc.sensors - -# -# Start GPS interface (depends on orb) -# -gps start - -# -# Start the attitude estimator (depends on orb) -# -attitude_estimator_ekf start - -# -# Load mixer and start controllers (depends on px4io) -# -roboclaw start /dev/ttyS2 128 1200 -segway start diff --git a/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm b/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm new file mode 100644 index 0000000000..2e5f6ca4f5 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/5001_quad_+_pwm @@ -0,0 +1,37 @@ +#!nsh +# +# Generic 10” Quad + geometry +# +# Maintainers: Lorenz Meier +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set MC_ATT_P 7.0 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_YAWPOS_P 2.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + # TODO add default MPC parameters +fi + +set VEHICLE_TYPE mc +set MIXER FMU_quad_+ + +set PWM_OUTPUTS 1234 +set PWM_RATE 400 +# DJI ESC range +set PWM_DISARMED 900 +set PWM_MIN 1200 +set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm b/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm new file mode 100644 index 0000000000..ddec8f36ef --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/6001_hexa_x_pwm @@ -0,0 +1,37 @@ +#!nsh +# +# Generic 10” Hexa X geometry +# +# Maintainers: Lorenz Meier +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set MC_ATT_P 7.0 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_YAWPOS_P 2.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + # TODO add default MPC parameters +fi + +set VEHICLE_TYPE mc +set MIXER FMU_hexa_x + +set PWM_OUTPUTS 1234 +set PWM_RATE 400 +# DJI ESC range +set PWM_DISARMED 900 +set PWM_MIN 1200 +set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 b/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 deleted file mode 100644 index acd8027fb5..0000000000 --- a/ROMFS/px4fmu_common/init.d/666_fmu_q_x550 +++ /dev/null @@ -1,76 +0,0 @@ -#!nsh - -echo "[init] 666_fmu_q_x550: PX4FMU Quad X550 with or without IO" - -# -# Load default params for this platform -# -if param compare SYS_AUTOCONFIG 1 -then - # Set all params here, then disable autoconfig - param set MC_ATTRATE_P 0.14 - param set MC_ATTRATE_I 0 - param set MC_ATTRATE_D 0.006 - param set MC_ATT_P 5.5 - param set MC_ATT_I 0 - param set MC_ATT_D 0 - param set MC_YAWPOS_D 0 - param set MC_YAWPOS_I 0 - param set MC_YAWPOS_P 0.6 - param set MC_YAWRATE_D 0 - param set MC_YAWRATE_I 0 - param set MC_YAWRATE_P 0.08 - param set RC_SCALE_PITCH 1 - param set RC_SCALE_ROLL 1 - param set RC_SCALE_YAW 3 - - param set SYS_AUTOCONFIG 0 - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor -# -param set MAV_TYPE 2 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - usleep 5000 - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -# -# Load mixer -# -mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix - -# -# Set PWM output frequency -# -pwm rate -c 1234 -r 400 - -# -# Set disarmed, min and max PWM signals -# -pwm disarmed -c 1234 -p 900 -pwm min -c 1234 -p 1100 -pwm max -c 1234 -p 1900 - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor diff --git a/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm b/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm new file mode 100644 index 0000000000..106e0fb54c --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/7001_hexa_+_pwm @@ -0,0 +1,37 @@ +#!nsh +# +# Generic 10” Hexa + geometry +# +# Maintainers: Lorenz Meier +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set MC_ATT_P 7.0 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_YAWPOS_P 2.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + # TODO add default MPC parameters +fi + +set VEHICLE_TYPE mc +set MIXER FMU_hexa_+ + +set PWM_OUTPUTS 1234 +set PWM_RATE 400 +# DJI ESC range +set PWM_DISARMED 900 +set PWM_MIN 1200 +set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm b/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm new file mode 100644 index 0000000000..f0eea339b8 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/8001_octo_x_pwm @@ -0,0 +1,37 @@ +#!nsh +# +# Generic 10” Octo X geometry +# +# Maintainers: Lorenz Meier +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set MC_ATT_P 7.0 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_YAWPOS_P 2.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + # TODO add default MPC parameters +fi + +set VEHICLE_TYPE mc +set MIXER FMU_octo_x + +set PWM_OUTPUTS 1234 +set PWM_RATE 400 +# DJI ESC range +set PWM_DISARMED 900 +set PWM_MIN 1200 +set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/800_sdlogger b/ROMFS/px4fmu_common/init.d/800_sdlogger deleted file mode 100644 index 9b90cbdd04..0000000000 --- a/ROMFS/px4fmu_common/init.d/800_sdlogger +++ /dev/null @@ -1,51 +0,0 @@ -#!nsh - -echo "[init] PX4FMU v1, v2 init to log only - -# -# 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 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io - # Set PWM values for DJI ESCs -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - usleep 5000 - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -sh /etc/init.d/rc.sensors - -gps start - -attitude_estimator_ekf start - -position_estimator_inav start - -if [ -d /fs/microsd ] -then - if [ $BOARD == fmuv1 ] - then - sdlog2 start -r 50 -e -b 16 - else - sdlog2 start -r 200 -e -b 16 - fi -fi diff --git a/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm b/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm new file mode 100644 index 0000000000..992a7aeba6 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/9001_octo_+_pwm @@ -0,0 +1,37 @@ +#!nsh +# +# Generic 10” Octo + geometry +# +# Maintainers: Lorenz Meier +# + +if [ $DO_AUTOCONFIG == yes ] +then + # + # Default parameters for this platform + # + param set MC_ATT_P 7.0 + param set MC_ATT_I 0.0 + param set MC_ATT_D 0.0 + param set MC_ATTRATE_P 0.12 + param set MC_ATTRATE_I 0.0 + param set MC_ATTRATE_D 0.004 + param set MC_YAWPOS_P 2.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_D 0.0 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_D 0.005 + + # TODO add default MPC parameters +fi + +set VEHICLE_TYPE mc +set MIXER FMU_octo_+ + +set PWM_OUTPUTS 1234 +set PWM_RATE 400 +# DJI ESC range +set PWM_DISARMED 900 +set PWM_MIN 1200 +set PWM_MAX 1900 diff --git a/ROMFS/px4fmu_common/init.d/cmp_test b/ROMFS/px4fmu_common/init.d/cmp_test deleted file mode 100644 index f86f4f85bb..0000000000 --- a/ROMFS/px4fmu_common/init.d/cmp_test +++ /dev/null @@ -1,9 +0,0 @@ -#!nsh - -cp /etc/extras/px4io-v2_default.bin /fs/microsd/px4io.loaded -if cmp /etc/extras/px4io-v2_default.bin /fs/microsd/px4io.loaded -then - echo "CMP returned true" -else - echo "CMP returned false" -fi \ No newline at end of file diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart new file mode 100644 index 0000000000..34da2dfef2 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -0,0 +1,195 @@ +# +# Check if auto-setup from one of the standard scripts is wanted +# SYS_AUTOSTART = 0 means no autostart (default) +# +# AUTOSTART PARTITION: +# 0 .. 999 Reserved (historical) +# 1000 .. 1999 Simulation setups +# 2000 .. 2999 Standard planes +# 3000 .. 3999 Flying wing +# 4000 .. 4999 Quad X +# 5000 .. 5999 Quad + +# 6000 .. 6999 Hexa X +# 7000 .. 7999 Hexa + +# 8000 .. 8999 Octo X +# 9000 .. 9999 Octo + +# 10000 .. 10999 Wide arm / H frame +# 11000 .. 11999 Hexa Cox +# 12000 .. 12999 Octo Cox + +# +# Simulation setups +# + +if param compare SYS_AUTOSTART 1000 +then + #sh /etc/init.d/1000_rc_fw_easystar.hil +fi + +if param compare SYS_AUTOSTART 1001 +then + sh /etc/init.d/1001_rc_quad_x.hil +fi + +if param compare SYS_AUTOSTART 1002 +then + sh /etc/init.d/1002_rc_fw_state.hil +fi + +if param compare SYS_AUTOSTART 1003 +then + sh /etc/init.d/1003_rc_quad_+.hil +fi + +if param compare SYS_AUTOSTART 1004 +then + sh /etc/init.d/1004_rc_fw_Rascal110.hil +fi + +# +# Standard plane +# + +if param compare SYS_AUTOSTART 2100 100 +then + sh /etc/init.d/2100_mpx_easystar + set MODE custom +fi + +if param compare SYS_AUTOSTART 2101 101 +then + sh /etc/init.d/2101_hk_bixler + set MODE custom +fi + +if param compare SYS_AUTOSTART 2102 102 +then + sh /etc/init.d/2102_3dr_skywalker + set MODE custom +fi + +# +# Flying wing +# + +if param compare SYS_AUTOSTART 3030 30 +then + sh /etc/init.d/3030_io_camflyer +fi + +if param compare SYS_AUTOSTART 3031 31 +then + sh /etc/init.d/3031_phantom +fi + +if param compare SYS_AUTOSTART 3032 32 +then + sh /etc/init.d/3032_skywalker_x5 +fi + +if param compare SYS_AUTOSTART 3033 33 +then + sh /etc/init.d/3033_wingwing +fi + +if param compare SYS_AUTOSTART 3034 34 +then + sh /etc/init.d/3034_fx79 +fi + +# +# Quad X +# + +if param compare SYS_AUTOSTART 4008 8 +then + #sh /etc/init.d/4008_ardrone +fi + +if param compare SYS_AUTOSTART 4009 9 +then + #sh /etc/init.d/4009_ardrone_flow +fi + +if param compare SYS_AUTOSTART 4010 10 +then + sh /etc/init.d/4010_dji_f330 +fi + +if param compare SYS_AUTOSTART 4011 11 +then + sh /etc/init.d/4011_dji_f450 +fi + +if param compare SYS_AUTOSTART 4012 12 +then + sh /etc/init.d/4012_hk_x550 +fi + +# +# Quad + +# + +if param compare SYS_AUTOSTART 5001 +then + sh /etc/init.d/5001_quad_+_pwm +fi + +# +# Hexa X +# + +if param compare SYS_AUTOSTART 6001 +then + sh /etc/init.d/6001_hexa_x_pwm +fi + +# +# Hexa + +# + +if param compare SYS_AUTOSTART 7001 +then + sh /etc/init.d/7001_hexa_+_pwm +fi + +# +# Octo X +# + +if param compare SYS_AUTOSTART 8001 +then + sh /etc/init.d/8001_octo_x_pwm +fi + +# +# Octo + +# + +if param compare SYS_AUTOSTART 9001 +then + sh /etc/init.d/9001_octo_+_pwm +fi + +# +# Wide arm / H frame +# + +if param compare SYS_AUTOSTART 10015 15 +then + sh /etc/init.d/10015_tbs_discovery +fi + +if param compare SYS_AUTOSTART 10016 16 +then + sh /etc/init.d/10016_3dr_iris +fi + +# +# Octo Coaxial +# + +if param compare SYS_AUTOSTART 12001 +then + sh /etc/init.d/12001_octo_cox_pwm +fi diff --git a/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl b/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl deleted file mode 100644 index 40b2ee68bd..0000000000 --- a/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl +++ /dev/null @@ -1,113 +0,0 @@ -#!nsh - -# -# 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 set MC_ATTRATE_D 0.002 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.09 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 - param set MC_ATT_P 6.8 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_P 2.0 - param set MC_YAWRATE_D 0.005 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_P 0.3 - param set NAV_TAKEOFF_ALT 3.0 - param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.7 - param set MPC_THR_MIN 0.3 - param set MPC_XY_D 0 - param set MPC_XY_P 0.5 - param set MPC_XY_VEL_D 0 - param set MPC_XY_VEL_I 0 - param set MPC_XY_VEL_MAX 3 - param set MPC_XY_VEL_P 0.2 - param set MPC_Z_D 0 - param set MPC_Z_P 1 - param set MPC_Z_VEL_D 0 - param set MPC_Z_VEL_I 0.1 - param set MPC_Z_VEL_MAX 2 - param set MPC_Z_VEL_P 0.20 - - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor -# -param set MAV_TYPE 2 - -set EXIT_ON_END no - -# -# Start the Mikrokopter ESC driver -# -if [ $MKBLCTRL_MODE == yes ] -then - if [ $MKBLCTRL_FRAME == x ] - then - echo "[init] PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame Mikrokopter-Addressing" - mkblctrl -mkmode x - else - echo "[init] PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame Mikrokopter-Addressing" - mkblctrl -mkmode + - fi -else - if [ $MKBLCTRL_FRAME == x ] - then - echo "[init] PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame" - else - echo "[init] PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame" - fi - mkblctrl -fi - -usleep 10000 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io -else - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS0 - usleep 5000 - fmu mode_pwm - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -# -# Load mixer -# -if [ $MKBLCTRL_FRAME == x ] -then - mixer load /dev/mkblctrl /etc/mixers/FMU_quad_x.mix -else - mixer load /dev/mkblctrl /etc/mixers/FMU_quad_+.mix -fi - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor - -if [ $EXIT_ON_END == yes ] -then - exit -fi diff --git a/ROMFS/px4fmu_common/init.d/rc.custom_io_esc b/ROMFS/px4fmu_common/init.d/rc.custom_io_esc deleted file mode 100644 index 045e41e529..0000000000 --- a/ROMFS/px4fmu_common/init.d/rc.custom_io_esc +++ /dev/null @@ -1,120 +0,0 @@ -#!nsh - -# -# 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 set MC_ATTRATE_D 0.002 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.09 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 - param set MC_ATT_P 6.8 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_P 2.0 - param set MC_YAWRATE_D 0.005 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_P 0.3 - param set NAV_TAKEOFF_ALT 3.0 - param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.7 - param set MPC_THR_MIN 0.3 - param set MPC_XY_D 0 - param set MPC_XY_P 0.5 - param set MPC_XY_VEL_D 0 - param set MPC_XY_VEL_I 0 - param set MPC_XY_VEL_MAX 3 - param set MPC_XY_VEL_P 0.2 - param set MPC_Z_D 0 - param set MPC_Z_P 1 - param set MPC_Z_VEL_D 0 - param set MPC_Z_VEL_I 0.1 - param set MPC_Z_VEL_MAX 2 - param set MPC_Z_VEL_P 0.20 - - param save -fi - -echo "RC script for PX4FMU + PX4IO + PPM-ESCs running" - -# -# Force some key parameters to sane values -# MAV_TYPE 2 = quadrotor -# -param set MAV_TYPE 2 - -set EXIT_ON_END no - -usleep 10000 - -# -# Start and configure PX4IO or FMU interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start -d /dev/ttyS1 -b 57600 - usleep 5000 - - sh /etc/init.d/rc.io -else - fmu mode_pwm - # Start MAVLink (on UART1 / ttyS0) - mavlink start -d /dev/ttyS1 -b 57600 - usleep 5000 - param set BAT_V_SCALING 0.004593 - set EXIT_ON_END yes -fi - -if [ $ESC_MAKER = afro ] -then - # Set PWM values for Afro ESCs - pwm disarmed -c 1234 -p 1050 - pwm min -c 1234 -p 1080 - pwm max -c 1234 -p 1860 -else - # Set PWM values for typical ESCs - pwm disarmed -c 1234 -p 900 - pwm min -c 1234 -p 980 - pwm max -c 1234 -p 1800 -fi - -# -# Load mixer -# -if [ $FRAME_GEOMETRY == x ] -then - echo "Frame geometry X" - mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix -else - if [ $FRAME_GEOMETRY == w ] - then - echo "Frame geometry W" - mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix - else - echo "Frame geometry +" - mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix - fi -fi - -# -# Set PWM output frequency -# -pwm rate -r 400 -c 1234 - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor - -if [ $EXIT_ON_END == yes ] -then - exit -fi - -echo "Script end" diff --git a/ROMFS/px4fmu_common/init.d/rc.fixedwing b/ROMFS/px4fmu_common/init.d/rc.fixedwing deleted file mode 100644 index f028510064..0000000000 --- a/ROMFS/px4fmu_common/init.d/rc.fixedwing +++ /dev/null @@ -1,34 +0,0 @@ -#!nsh -# -# Standard everything needed for fixedwing except mixer, actuator output and mavlink -# - -# -# Start the sensors and test them. -# -sh /etc/init.d/rc.sensors - -# -# Start logging (depends on sensors) -# -sh /etc/init.d/rc.logging - -# -# Start GPS interface (depends on orb) -# -gps start - -# -# Start the attitude and position estimator -# -att_pos_estimator_ekf start - -# -# Start attitude controller -# -fw_att_control start - -# -# Start the position controller -# -fw_pos_control_l1 start diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps new file mode 100644 index 0000000000..d354fb06fa --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps @@ -0,0 +1,19 @@ +#!nsh +# +# Standard apps for fixed wing +# + +# +# Start the attitude and position estimator +# +att_pos_estimator_ekf start + +# +# Start attitude controller +# +fw_att_control start + +# +# Start the position controller +# +fw_pos_control_l1 start diff --git a/ROMFS/px4fmu_common/init.d/rc.hexa b/ROMFS/px4fmu_common/init.d/rc.hexa deleted file mode 100644 index 097db28e48..0000000000 --- a/ROMFS/px4fmu_common/init.d/rc.hexa +++ /dev/null @@ -1,94 +0,0 @@ -#!nsh - -echo "[init] PX4FMU v1, v2 with or without IO on Hex" - -# -# 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 set MC_ATTRATE_D 0.004 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.12 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 - param set MC_ATT_P 7.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_P 2.0 - param set MC_YAWRATE_D 0.005 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_P 0.3 - param set NAV_TAKEOFF_ALT 3.0 - param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.7 - param set MPC_THR_MIN 0.3 - param set MPC_XY_D 0 - param set MPC_XY_P 0.5 - param set MPC_XY_VEL_D 0 - param set MPC_XY_VEL_I 0 - param set MPC_XY_VEL_MAX 3 - param set MPC_XY_VEL_P 0.2 - param set MPC_Z_D 0 - param set MPC_Z_P 1 - param set MPC_Z_VEL_D 0 - param set MPC_Z_VEL_I 0.1 - param set MPC_Z_VEL_MAX 2 - param set MPC_Z_VEL_P 0.20 - - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE list: https://pixhawk.ethz.ch/mavlink/ -# 13 = hexarotor -# -param set MAV_TYPE 13 - -set EXIT_ON_END no - -# -# Start and configure PX4IO interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io -else - # This is not possible on a hexa - tone_alarm error -fi - -# -# Load mixer -# -mixer load /dev/pwm_output $MIXER - -# -# Set PWM output frequency to 400 Hz -# -pwm rate -a -r 400 - -# -# Set disarmed, min and max PWM signals -# -pwm disarmed -c 123456 -p 900 -pwm min -c 123456 -p 1100 -pwm max -c 123456 -p 1900 - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor - -if [ $EXIT_ON_END == yes ] -then - exit -fi diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface new file mode 100644 index 0000000000..d25f01dde0 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -0,0 +1,72 @@ +#!nsh +# +# Script to configure control interface +# + +if [ $MIXER != none ] +then + # + # Load mixer + # + set MIXERSD /fs/microsd/etc/mixers/$MIXER.mix + + #Use the mixer file from the SD-card if it exists + if [ -f $MIXERSD ] + then + set MIXER_FILE $MIXERSD + else + set MIXER_FILE /etc/mixers/$MIXER.mix + fi + + if [ $OUTPUT_MODE == mkblctrl ] + then + set OUTPUT_DEV /dev/mkblctrl + else + set OUTPUT_DEV /dev/pwm_output + fi + + if mixer load $OUTPUT_DEV $MIXER_FILE + then + echo "[init] Mixer loaded: $MIXER_FILE" + else + echo "[init] Error loading mixer: $MIXER_FILE" + tone_alarm $TUNE_OUT_ERROR + fi +else + echo "[init] Mixer not defined" + tone_alarm $TUNE_OUT_ERROR +fi + +if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == io ] +then + if [ $PWM_OUTPUTS != none ] + then + # + # Set PWM output frequency + # + if [ $PWM_RATE != none ] + then + echo "[init] Set PWM rate: $PWM_RATE" + pwm rate -c $PWM_OUTPUTS -r $PWM_RATE + fi + + # + # Set disarmed, min and max PWM values + # + 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 +fi diff --git a/ROMFS/px4fmu_common/init.d/rc.io b/ROMFS/px4fmu_common/init.d/rc.io index aaf91b3166..c9d964f8e2 100644 --- a/ROMFS/px4fmu_common/init.d/rc.io +++ b/ROMFS/px4fmu_common/init.d/rc.io @@ -1,23 +1,21 @@ # -# Start PX4IO interface (depends on orb, commander) +# Init PX4IO interface # -if px4io start -then - # - # Allow PX4IO to recover from midair restarts. - # this is very unlikely, but quite safe and robust. - px4io recovery - # - # Disable px4io topic limiting - # - if [ $BOARD == fmuv1 ] - then - px4io limit 200 - else - px4io limit 400 - fi -else - # SOS - tone_alarm error +# +# Allow PX4IO to recover from midair restarts. +# this is very unlikely, but quite safe and robust. +# +px4io recovery + +# +# Adjust PX4IO update rate limit +# +set PX4IO_LIMIT 400 +if hw_ver compare PX4FMU_V1 +then + set PX4IO_LIMIT 200 fi + +echo "[init] Set PX4IO update rate limit: $PX4IO_LIMIT Hz" +px4io limit $PX4IO_LIMIT diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging index dc4be8055a..dcf5bbced8 100644 --- a/ROMFS/px4fmu_common/init.d/rc.logging +++ b/ROMFS/px4fmu_common/init.d/rc.logging @@ -1,14 +1,16 @@ #!nsh # -# Initialise logging services. +# Initialize logging services. # if [ -d /fs/microsd ] then - if [ $BOARD == fmuv1 ] + if hw_ver compare PX4FMU_V1 then - sdlog2 start -r 50 -a -b 16 + echo "Start sdlog2 at 50Hz" + sdlog2 start -r 50 -a -b 16 -t else - sdlog2 start -r 200 -a -b 16 + echo "Start sdlog2 at 200Hz" + sdlog2 start -r 200 -a -b 16 -t fi fi diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps new file mode 100644 index 0000000000..96fe32c8a7 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -0,0 +1,24 @@ +#!nsh +# +# Standard apps for multirotors +# + +# +# Start the attitude estimator +# +attitude_estimator_ekf start + +# +# Start position estimator +# +position_estimator_inav start + +# +# Start attitude control +# +mc_att_control start + +# +# Start position control +# +mc_pos_control start diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_interface b/ROMFS/px4fmu_common/init.d/rc.mc_interface deleted file mode 100644 index 6bb2e84ecf..0000000000 --- a/ROMFS/px4fmu_common/init.d/rc.mc_interface +++ /dev/null @@ -1,49 +0,0 @@ -#!nsh -# -# Script to set PWM min / max limits and mixer -# - -# -# Load mixer -# -if [ $FRAME_GEOMETRY == x ] -then - echo "Frame geometry X" - mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix -else - if [ $FRAME_GEOMETRY == w ] - then - echo "Frame geometry W" - mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix - else - echo "Frame geometry +" - mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix - fi -fi - -if [ $FRAME_COUNT == 4 ] -then - set OUTPUTS 1234 - param set MAV_TYPE 2 -else - if [ $FRAME_COUNT == 6 ] - then - set OUTPUTS 123456 - param set MAV_TYPE 13 - else - set OUTPUTS 12345678 - fi -fi - - -# -# Set PWM output frequency -# -pwm rate -c $OUTPUTS -r $PWM_RATE - -# -# Set disarmed, min and max PWM signals (for DJI ESCs) -# -pwm disarmed -c $OUTPUTS -p $PWM_DISARMED -pwm min -c $OUTPUTS -p $PWM_MIN -pwm max -c $OUTPUTS -p $PWM_MAX diff --git a/ROMFS/px4fmu_common/init.d/rc.multirotor b/ROMFS/px4fmu_common/init.d/rc.multirotor deleted file mode 100644 index bc550ac5a1..0000000000 --- a/ROMFS/px4fmu_common/init.d/rc.multirotor +++ /dev/null @@ -1,39 +0,0 @@ -#!nsh -# -# Standard everything needed for multirotors except mixer, actuator output and mavlink -# - -# -# Start the sensors and test them. -# -sh /etc/init.d/rc.sensors - -# -# Start logging (depends on sensors) -# -sh /etc/init.d/rc.logging - -# -# Start GPS interface (depends on orb) -# -gps start - -# -# Start the attitude estimator -# -attitude_estimator_ekf start - -# -# Start position estimator -# -position_estimator_inav start - -# -# Start attitude control -# -multirotor_att_control start - -# -# Start position control -# -multirotor_pos_control start diff --git a/ROMFS/px4fmu_common/init.d/rc.octo b/ROMFS/px4fmu_common/init.d/rc.octo deleted file mode 100644 index ecb12e96e5..0000000000 --- a/ROMFS/px4fmu_common/init.d/rc.octo +++ /dev/null @@ -1,94 +0,0 @@ -#!nsh - -echo "[init] Octorotor startup" - -# -# 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 set MC_ATTRATE_D 0.004 - param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.12 - param set MC_ATT_D 0.0 - param set MC_ATT_I 0.0 - param set MC_ATT_P 7.0 - param set MC_YAWPOS_D 0.0 - param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_P 2.0 - param set MC_YAWRATE_D 0.005 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_P 0.3 - param set NAV_TAKEOFF_ALT 3.0 - param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.7 - param set MPC_THR_MIN 0.3 - param set MPC_XY_D 0 - param set MPC_XY_P 0.5 - param set MPC_XY_VEL_D 0 - param set MPC_XY_VEL_I 0 - param set MPC_XY_VEL_MAX 3 - param set MPC_XY_VEL_P 0.2 - param set MPC_Z_D 0 - param set MPC_Z_P 1 - param set MPC_Z_VEL_D 0 - param set MPC_Z_VEL_I 0.1 - param set MPC_Z_VEL_MAX 2 - param set MPC_Z_VEL_P 0.20 - - param save -fi - -# -# Force some key parameters to sane values -# MAV_TYPE list: https://pixhawk.ethz.ch/mavlink/ -# 14 = octorotor -# -param set MAV_TYPE 14 - -set EXIT_ON_END no - -# -# Start and configure PX4IO interface -# -if px4io detect -then - # Start MAVLink (depends on orb) - mavlink start - usleep 5000 - - sh /etc/init.d/rc.io -else - # This is not possible on an octo - tone_alarm error -fi - -# -# Load mixer -# -mixer load /dev/pwm_output $MIXER - -# -# Set PWM output frequency to 400 Hz -# -pwm rate -a -r 400 - -# -# Set disarmed, min and max PWM signals -# -pwm disarmed -c 12345678 -p 900 -pwm min -c 12345678 -p 1100 -pwm max -c 12345678 -p 1900 - -# -# Start common for all multirotors apps -# -sh /etc/init.d/rc.multirotor - -if [ $EXIT_ON_END == yes ] -then - exit -fi diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 070a4e7e35..badbf92c3c 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -10,41 +10,42 @@ ms5611 start adc start -# mag might be external +# Mag might be external if hmc5883 start then - echo "using HMC5883" + echo "[init] Using HMC5883" fi if mpu6000 start then - echo "using MPU6000" + echo "[init] Using MPU6000" fi if l3gd20 start then - echo "using L3GD20(H)" + echo "[init] Using L3GD20(H)" fi -if lsm303d start +if hw_ver compare PX4FMU_V2 then - set BOARD fmuv2 -else - set BOARD fmuv1 + if lsm303d start + then + echo "[init] Using LSM303D" + fi fi # Start airspeed sensors if meas_airspeed start then - echo "using MEAS airspeed sensor" + echo "[init] Using MEAS airspeed sensor" else if ets_airspeed start then - echo "using ETS airspeed sensor (bus 3)" + echo "[init] Using ETS airspeed sensor (bus 3)" else if ets_airspeed start -b 1 then - echo "Using ETS airspeed sensor (bus 1)" + echo "[init] Using ETS airspeed sensor (bus 1)" fi fi fi diff --git a/ROMFS/px4fmu_common/init.d/rc.standalone b/ROMFS/px4fmu_common/init.d/rc.standalone deleted file mode 100644 index 67e95215b9..0000000000 --- a/ROMFS/px4fmu_common/init.d/rc.standalone +++ /dev/null @@ -1,13 +0,0 @@ -#!nsh -# -# Flight startup script for PX4FMU standalone configuration. -# - -echo "[init] doing standalone PX4FMU startup..." - -# -# Start the ORB -# -uorb start - -echo "[init] startup done" diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index ccf2cd47ea..0cd8a0e042 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -36,39 +36,6 @@ then echo "Commander started" fi -# Start px4io if present -if px4io start -then - echo "PX4IO driver started" -else - if fmu mode_serial - then - echo "FMU driver started" - fi -fi - -# Start sensors -sh /etc/init.d/rc.sensors - -# Start one of the estimators -if attitude_estimator_ekf status -then - echo "multicopter att filter running" -else - if att_pos_estimator_ekf status - then - echo "fixedwing att filter running" - else - attitude_estimator_ekf start - fi -fi - -# Start GPS -if gps start -then - echo "GPS started" -fi - echo "MAVLink started, exiting shell.." # Exit shell to make it available to MAVLink diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 90f2e2b178..6f4e1f3b56 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -8,39 +8,40 @@ # set MODE autostart -set logfile /fs/microsd/bootlog.txt +set RC_FILE /fs/microsd/etc/rc.txt +set CONFIG_FILE /fs/microsd/etc/config.txt +set EXTRAS_FILE /fs/microsd/etc/extras.txt + +set TUNE_OUT_ERROR ML<> $LOG_FILE + + set IO_PRESENT yes + else + echo "[init] Trying to update" + echo "PX4IO Trying to update" >> $LOG_FILE + + tone_alarm MLL32CP8MB + + if px4io forceupdate 14662 $IO_FILE + 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 + fi + + if [ $IO_PRESENT == no ] + then + echo "[init] ERROR: PX4IO not found" + tone_alarm $TUNE_OUT_ERROR + fi + fi + + # + # Set default output if not set + # + if [ $OUTPUT_MODE == none ] + then + if [ $USE_IO == yes ] + then + set OUTPUT_MODE io + else + set OUTPUT_MODE fmu + fi + fi + + if [ $OUTPUT_MODE == io -a $IO_PRESENT != yes ] + then + # Need IO for output but it not present, disable output + set OUTPUT_MODE none + echo "[init] ERROR: PX4IO not found, disabling output" + + # Avoid using ttyS0 for MAVLink on FMUv1 + if hw_ver compare PX4FMU_V1 + then + set FMU_MODE serial + fi + fi + + if [ $HIL == yes ] + then + set OUTPUT_MODE hil + if hw_ver compare PX4FMU_V1 + then + set FMU_MODE serial + fi + else + # Try to get an USB console if not in HIL mode + nshterm /dev/ttyACM0 & + fi + # # Start the Commander (needs to be this early for in-air-restarts) # commander start - - # - # Start the datamanager - # - dataman start - - # - # Start the Navigator - # - navigator start - - if param compare SYS_AUTOSTART 1000 - then - sh /etc/init.d/1000_rc_fw_easystar.hil - set MODE custom - fi - if param compare SYS_AUTOSTART 1001 - then - sh /etc/init.d/1001_rc_quad.hil - set MODE custom - fi - - if param compare SYS_AUTOSTART 1002 - then - sh /etc/init.d/1002_rc_fw_state.hil - set MODE custom - fi - - if param compare SYS_AUTOSTART 1003 - then - sh /etc/init.d/1003_rc_quad_+.hil - set MODE custom - fi - - if param compare SYS_AUTOSTART 1004 - then - sh /etc/init.d/1004_rc_fw_Rascal110.hil - set MODE custom - fi - - if [ $MODE != custom ] - then - # Try to get an USB console - nshterm /dev/ttyACM0 & - fi - # - # Upgrade PX4IO firmware + # Start primary output # - - if [ -f /etc/extras/px4io-v2_default.bin ] - then - set io_file /etc/extras/px4io-v2_default.bin - else - set io_file /etc/extras/px4io-v1_default.bin - fi - - if px4io start - then - echo "PX4IO OK" - echo "PX4IO OK" >> $logfile - fi + set TTYS1_BUSY no - if px4io checkcrc $io_file + # If OUTPUT_MODE == none then something is wrong with setup and we shouldn't try to enable output + if [ $OUTPUT_MODE != none ] then - echo "PX4IO CRC OK" - echo "PX4IO CRC OK" >> $logfile - else - echo "PX4IO CRC failure" - echo "PX4IO CRC failure" >> $logfile - tone_alarm MBABGP - if px4io forceupdate 14662 $io_file + if [ $OUTPUT_MODE == io ] then - usleep 500000 + echo "[init] Use PX4IO PWM as primary output" if px4io start then - echo "PX4IO restart OK" - echo "PX4IO restart OK" >> $logfile - tone_alarm MSPAA + echo "[init] PX4IO started" + sh /etc/init.d/rc.io else - echo "PX4IO restart failed" - echo "PX4IO restart failed" >> $logfile - tone_alarm MNGGG - sleep 10 - reboot + echo "[init] ERROR: PX4IO start failed" + tone_alarm $TUNE_OUT_ERROR + fi + fi + if [ $OUTPUT_MODE == fmu ] + then + echo "[init] Use FMU PWM as primary output" + if fmu mode_$FMU_MODE + then + echo "[init] FMU mode_$FMU_MODE started" + else + echo "[init] ERROR: FMU mode_$FMU_MODE start failed" + tone_alarm $TUNE_OUT_ERROR + fi + + if hw_ver compare PX4FMU_V1 + then + if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ] + then + set TTYS1_BUSY yes + fi + if [ $FMU_MODE == pwm_gpio ] + then + set TTYS1_BUSY yes + fi + fi + fi + if [ $OUTPUT_MODE == mkblctrl ] + then + echo "[init] Use MKBLCTRL as primary output" + set MKBLCTRL_ARG "" + if [ $MKBLCTRL_MODE == x ] + then + set MKBLCTRL_ARG "-mkmode x" + fi + if [ $MKBLCTRL_MODE == + ] + then + set MKBLCTRL_ARG "-mkmode +" + fi + + if mkblctrl $MKBLCTRL_ARG + then + echo "[init] MKBLCTRL started" + else + echo "[init] ERROR: MKBLCTRL start failed" + tone_alarm $TUNE_OUT_ERROR + fi + + fi + if [ $OUTPUT_MODE == hil ] + then + echo "[init] Use HIL as primary output" + if hil mode_pwm + then + echo "[init] HIL output started" + else + echo "[init] ERROR: HIL output start failed" + tone_alarm $TUNE_OUT_ERROR + fi + fi + + # + # Start IO or FMU for RC PPM input if needed + # + if [ $IO_PRESENT == yes ] + then + if [ $OUTPUT_MODE != io ] + then + if px4io start + then + echo "[init] PX4IO started" + sh /etc/init.d/rc.io + else + echo "[init] ERROR: PX4IO start failed" + tone_alarm $TUNE_OUT_ERROR + fi fi else - echo "PX4IO update failed" - echo "PX4IO update failed" >> $logfile - tone_alarm MNGGG + if [ $OUTPUT_MODE != fmu ] + then + if fmu mode_$FMU_MODE + then + echo "[init] FMU mode_$FMU_MODE started" + else + echo "[init] ERROR: FMU mode_$FMU_MODE start failed" + tone_alarm $TUNE_OUT_ERROR + fi + + if hw_ver compare PX4FMU_V1 + then + if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ] + then + set TTYS1_BUSY yes + fi + if [ $FMU_MODE == pwm_gpio ] + then + set TTYS1_BUSY yes + fi + fi + fi fi fi - + + # + # MAVLink + # set EXIT_ON_END no - # - # Check if auto-setup from one of the standard scripts is wanted - # SYS_AUTOSTART = 0 means no autostart (default) - # - # AUTOSTART PARTITION: - # 0 .. 999 Reserved (historical) - # 1000 .. 1999 Simulation setups - # 2000 .. 2999 Standard planes - # 3000 .. 3999 Flying wing - # 4000 .. 4999 Quad X - # 5000 .. 5999 Quad + - # 6000 .. 6999 Hexa X - # 7000 .. 7999 Hexa + - # 8000 .. 8999 Octo X - # 9000 .. 9999 Octo + - # 10000 .. 10999 Wide arm / H frame - # 11000 .. 11999 Hexa Cox - # 12000 .. 12999 Octo Cox - - if param compare SYS_AUTOSTART 4008 8 + if [ $HIL == yes ] then - sh /etc/init.d/4008_ardrone - set MODE custom - fi - - if param compare SYS_AUTOSTART 4009 9 - then - sh /etc/init.d/4009_ardrone_flow - set MODE custom - fi - - if param compare SYS_AUTOSTART 4010 10 - then - set FRAME_GEOMETRY x - set FRAME_COUNT 4 - set PWM_MIN 1200 - set PWM_MAX 1900 - set PWM_DISARMED 900 - sh /etc/init.d/4010_dji_f330 - set MODE custom - fi - - if param compare SYS_AUTOSTART 4011 11 - then - sh /etc/init.d/4011_dji_f450 - set MODE custom - fi - - if param compare SYS_AUTOSTART 4012 - then - sh /etc/init.d/666_fmu_q_x550 - set MODE custom - fi - - if param compare SYS_AUTOSTART 6012 12 - then - set MIXER /etc/mixers/FMU_hex_x.mix - sh /etc/init.d/rc.hexa - set MODE custom - fi - - if param compare SYS_AUTOSTART 7013 13 - then - set MIXER /etc/mixers/FMU_hex_+.mix - sh /etc/init.d/rc.hexa - set MODE custom - fi - - if param compare SYS_AUTOSTART 8001 - then - set MIXER /etc/mixers/FMU_octo_x.mix - sh /etc/init.d/rc.octo - set MODE custom - fi - - if param compare SYS_AUTOSTART 9001 - then - set MIXER /etc/mixers/FMU_octo_+.mix - sh /etc/init.d/rc.octo - set MODE custom - fi - - if param compare SYS_AUTOSTART 12001 - then - set MIXER /etc/mixers/FMU_octo_cox.mix - sh /etc/init.d/rc.octo - set MODE custom - fi - - if param compare SYS_AUTOSTART 10015 15 - then - sh /etc/init.d/10015_tbs_discovery - set MODE custom - fi - - if param compare SYS_AUTOSTART 10016 16 - then - sh /etc/init.d/10016_3dr_iris - set MODE custom - fi - - # PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame - if param compare SYS_AUTOSTART 4017 17 - then - set MKBLCTRL_MODE no - set MKBLCTRL_FRAME x - sh /etc/init.d/rc.custom_dji_f330_mkblctrl - set MODE custom - fi - - # PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame - if param compare SYS_AUTOSTART 5018 18 - then - set MKBLCTRL_MODE no - set MKBLCTRL_FRAME + - sh /etc/init.d/rc.custom_dji_f330_mkblctrl - set MODE custom - fi - - # PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame Mikrokopter-Addressing - if param compare SYS_AUTOSTART 4019 19 - then - set MKBLCTRL_MODE yes - set MKBLCTRL_FRAME x - sh /etc/init.d/rc.custom_dji_f330_mkblctrl - set MODE custom - fi - - # PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame Mikrokopter-Addressing - if param compare SYS_AUTOSTART 5020 20 - then - set MKBLCTRL_MODE yes - set MKBLCTRL_FRAME + - sh /etc/init.d/rc.custom_dji_f330_mkblctrl - set MODE custom - fi - - # PX4FMU v1 with IO + PPM-based ESCs on Quad X-shape frame - if param compare SYS_AUTOSTART 4021 21 - then - set FRAME_GEOMETRY x - set ESC_MAKER afro - sh /etc/init.d/rc.custom_io_esc - set MODE custom - fi - - # PX4FMU v1 with IO + PPM-based ESCs on Quad X-shape frame - if param compare SYS_AUTOSTART 10022 22 - then - set FRAME_GEOMETRY w - sh /etc/init.d/rc.custom_io_esc - set MODE custom - fi - - if param compare SYS_AUTOSTART 3030 30 - then - sh /etc/init.d/3030_io_camflyer - set MODE custom - fi - - if param compare SYS_AUTOSTART 3031 31 - then - sh /etc/init.d/3031_io_phantom - set MODE custom - fi - - if param compare SYS_AUTOSTART 3032 32 - then - sh /etc/init.d/3032_skywalker_x5 - set MODE custom - fi - - if param compare SYS_AUTOSTART 3033 33 - then - sh /etc/init.d/3033_io_wingwing - set MODE custom - fi - - if param compare SYS_AUTOSTART 3034 34 - then - sh /etc/init.d/3034_io_fx79 - set MODE custom - fi - - if param compare SYS_AUTOSTART 40 - then - sh /etc/init.d/40_io_segway - set MODE custom - fi - - if param compare SYS_AUTOSTART 2100 100 - then - sh /etc/init.d/2100_mpx_easystar - set MODE custom - fi - - if param compare SYS_AUTOSTART 2101 101 - then - sh /etc/init.d/2101_hk_bixler - set MODE custom - fi - - if param compare SYS_AUTOSTART 2102 102 - then - sh /etc/init.d/2102_3dr_skywalker - set MODE custom - fi - - if param compare SYS_AUTOSTART 800 - then - sh /etc/init.d/800_sdlogger - set MODE custom - fi - - # Start any custom extensions that might be missing - if [ -f /fs/microsd/etc/rc.local ] - then - sh /fs/microsd/etc/rc.local - fi - - # If none of the autostart scripts triggered, get a minimal setup - if [ $MODE == autostart ] - then - # Telemetry port is on both FMU boards ttyS1 - # but the AR.Drone motors can be get 'flashed' - # if starting MAVLink on them - so do not - # start it as default (default link: USB) - - # Start commander - commander start - - # Start px4io if present - if px4io detect + sleep 1 + mavlink start -b 230400 -d /dev/ttyACM0 + usleep 5000 + else + if [ $TTYS1_BUSY == yes ] then - px4io start + # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else + mavlink start -d /dev/ttyS0 + usleep 5000 + + # Exit from nsh to free port for mavlink + set EXIT_ON_END yes else - if fmu mode_serial + # Start MAVLink on default port: ttyS1 + mavlink start + usleep 5000 + fi + fi + + # + # Sensors, Logging, GPS + # + echo "[init] Start sensors" + sh /etc/init.d/rc.sensors + + if [ $HIL == no ] + then + echo "[init] Start logging" + sh /etc/init.d/rc.logging + + echo "[init] Start GPS" + gps start + fi + + # + # Fixed wing setup + # + if [ $VEHICLE_TYPE == fw ] + then + echo "[init] Vehicle type: FIXED WING" + + if [ $MIXER == none ] + then + # Set default mixer for fixed wing if not defined + set MIXER FMU_AERT + fi + + if [ $MAV_TYPE == none ] + then + # Use MAV_TYPE = 1 (fixed wing) if not defined + set MAV_TYPE 1 + fi + + param set MAV_TYPE $MAV_TYPE + + # Load mixer and configure outputs + sh /etc/init.d/rc.interface + + # Start standard fixedwing apps + sh /etc/init.d/rc.fw_apps + fi + + # + # Multicopters setup + # + if [ $VEHICLE_TYPE == mc ] + then + echo "[init] Vehicle type: MULTICOPTER" + + if [ $MIXER == none ] + then + # Set default mixer for multicopter if not defined + set MIXER quad_x + fi + + if [ $MAV_TYPE == none ] + then + # Use MAV_TYPE = 2 (quadcopter) if not defined + set MAV_TYPE 2 + + # Use mixer to detect vehicle type + if [ $MIXER == FMU_hex_x -o $MIXER == FMU_hex_+ ] then - echo "FMU driver (no PWM) started" + param set MAV_TYPE 13 + fi + if [ $MIXER == FMU_octo_x -o $MIXER == FMU_octo_+ ] + then + param set MAV_TYPE 14 + fi + if [ $MIXER == FMU_octo_cox ] + then + param set MAV_TYPE 14 fi fi - - # Start sensors - sh /etc/init.d/rc.sensors - - # Start one of the estimators - attitude_estimator_ekf start - - # Start GPS - gps start - + + param set MAV_TYPE $MAV_TYPE + + # Load mixer and configure outputs + sh /etc/init.d/rc.interface + + # Start standard multicopter apps + sh /etc/init.d/rc.mc_apps + fi + + # + # Generic setup (autostart ID not found) + # + if [ $VEHICLE_TYPE == none ] + then + echo "[init] Vehicle type: GENERIC" + + # Load mixer and configure outputs + sh /etc/init.d/rc.interface + fi + + # Start any custom addons + if [ -f $EXTRAS_FILE ] + then + echo "[init] Starting addons script: $EXTRAS_FILE" + sh $EXTRAS_FILE + else + echo "[init] Addons script not found: $EXTRAS_FILE" fi if [ $EXIT_ON_END == yes ] diff --git a/ROMFS/px4fmu_test/init.d/rcS b/ROMFS/px4fmu_test/init.d/rcS index 6aa1d3d46b..56482d1407 100644 --- a/ROMFS/px4fmu_test/init.d/rcS +++ b/ROMFS/px4fmu_test/init.d/rcS @@ -2,6 +2,7 @@ # # PX4FMU startup script for test hackery. # +uorb start if sercon then @@ -9,4 +10,68 @@ then # Try to get an USB console nshterm /dev/ttyACM0 & -fi \ No newline at end of file +fi + +# +# Try to mount the microSD card. +# +echo "[init] looking for microSD..." +if mount -t vfat /dev/mmcsd0 /fs/microsd +then + echo "[init] card mounted at /fs/microsd" + # Start playing the startup tune + tone_alarm start +else + echo "[init] no microSD card found" + # Play SOS + tone_alarm error +fi + +# +# Start a minimal system +# + +if [ -f /etc/extras/px4io-v2_default.bin ] +then + set io_file /etc/extras/px4io-v2_default.bin +else + set io_file /etc/extras/px4io-v1_default.bin +fi + +if px4io start +then + echo "PX4IO OK" +fi + +if px4io checkcrc $io_file +then + echo "PX4IO CRC OK" +else + echo "PX4IO CRC failure" + tone_alarm MBABGP + if px4io forceupdate 14662 $io_file + then + usleep 500000 + if px4io start + then + echo "PX4IO restart OK" + tone_alarm MSPAA + else + echo "PX4IO restart failed" + tone_alarm MNGGG + sleep 5 + reboot + fi + else + echo "PX4IO update failed" + tone_alarm MNGGG + fi +fi + +# +# The presence of this file suggests we're running a mount stress test +# +if [ -f /fs/microsd/mount_test_cmds.txt ] +then + tests mount +fi diff --git a/Tools/fsm_visualisation.py b/Tools/fsm_visualisation.py new file mode 100755 index 0000000000..c678ef0f4b --- /dev/null +++ b/Tools/fsm_visualisation.py @@ -0,0 +1,201 @@ +#!/usr/bin/env python3 + +"""fsm_visualisation.py: Create dot code and dokuwiki table from a state transition table + +convert dot code to png using graphviz: + +dot fsm.dot -Tpng -o fsm.png +""" + +import argparse +import re + +__author__ = "Julian Oes" + +def get_dot_header(): + + return """digraph finite_state_machine { + graph [ dpi = 300 ]; + ratio = 1.5 + node [shape = circle];""" + +def get_dot_footer(): + + return """}\n""" + +def main(): + + # parse input arguments + parser = argparse.ArgumentParser(description='Create dot code and dokuwiki table from a state transition table.') + parser.add_argument("-i", "--input-file", default=None, help="choose file to parse") + parser.add_argument("-d", "--dot-file", default=None, help="choose file for output dot file") + parser.add_argument("-t", "--table-file", default=None, help="choose file for output of table") + args = parser.parse_args() + + # open source file + if args.input_file == None: + exit('please specify file') + f = open(args.input_file,'r') + source = f.read() + + # search for state transition table and extract the table itself + # first look for StateTable::Tran + # then accept anything including newline until { + # but don't accept the definition (without ;) + # then extract anything inside the brackets until }; + match = re.search(r'StateTable::Tran(?:.|\n!;)*\{((?:.|\n)*?)\};', source) + + if not match: + exit('no state transition table found') + + table_source = match.group(1) + + # bookkeeping for error checking + num_errors_found = 0 + + states = [] + events = [] + + # first get all states and events + for table_line in table_source.split('\n'): + + match = re.search(r'/\*\s+\w+_STATE_(\w+)\s+\*/', table_line) + if match: + states.append(str(match.group(1))) + # go to next line + continue + + if len(states) == 1: + match = re.search(r'/\*\s+EVENT_(\w+)\s+\*/', table_line) + if match: + events.append(str(match.group(1))) + + print('Found %d states and %d events' % (len(states), len(events))) + + + # keep track of origin state + state = None + + # fill dot code in here + dot_code = '' + + # create table len(states)xlen(events) + transition_table = [[[] for x in range(len(states))] for y in range(len(events))] + + # now fill the transition table and write the dot code + for table_line in table_source.split('\n'): + + # get states + # from: /* NAV_STATE_NONE */ + # extract only "NONE" + match = re.search(r'/\*\s+\w+_STATE_(\w+)\s+\*/', table_line) + if match: + state = match.group(1) + state_index = states.index(state) + # go to next line + continue + + # can't advance without proper state + if state == None: + continue + + # get event and next state + # from /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY} + # extract "READY_REQUESTED" and "READY" if there is ACTION + match_action = re.search(r'/\*\s+EVENT_(\w+)\s+\*/\s+\{ACTION\((?:.|\n)*\w+_STATE_(\w+)', table_line) + + # get event and next state + # from /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE}, + # extract "NONE_REQUESTED" and "NAV_STATE_NONE" if there is NO_ACTION + match_no_action = re.search(r'/\*\s+EVENT_(\w+)\s+\*/\s+\{NO_ACTION(?:.|\n)*\w+_STATE_(\w+)', table_line) + + # ignore lines with brackets only + if match_action or match_no_action: + + # only write arrows for actions + if match_action: + event = match_action.group(1) + new_state = match_action.group(2) + dot_code += ' ' + state + ' -> ' + new_state + '[ label = "' + event + '"];\n' + + elif match_no_action: + event = match_no_action.group(1) + new_state = match_no_action.group(2) + + # check for state changes without action + if state != new_state: + print('Error: no action but state change:') + print('State: ' + state + ' changed to: ' + new_state) + print(table_line) + num_errors_found += 1 + + # check for wrong events + if event not in events: + print('Error: unknown event: ' + event) + print(table_line) + num_errors_found += 1 + + # check for wrong new states + if new_state not in states: + print('Error: unknown new state: ' + new_state) + print(table_line) + num_errors_found += 1 + + # save new state in transition table + event_index = events.index(event) + + # bold for action + if match_action: + transition_table[event_index][state_index] = '**' + new_state + '**' + else: + transition_table[event_index][state_index] = new_state + + + + # assemble dot code + dot_code = get_dot_header() + dot_code + get_dot_footer() + + # write or print dot file + if args.dot_file: + f = open(args.dot_file,'w') + f.write(dot_code) + print('Wrote dot file') + else: + print('##########Dot-start##########') + print(dot_code) + print('##########Dot-end############') + + + # assemble doku wiki table + table_code = '| ^ ' + # start with header of all states + for state in states: + table_code += state + ' ^ ' + + table_code += '\n' + + # add events and new states + for event, row in zip(events, transition_table): + table_code += '^ ' + event + ' | ' + for new_state in row: + table_code += new_state + ' | ' + table_code += '\n' + + # write or print wiki table + if args.table_file: + f = open(args.table_file,'w') + f.write(table_code) + print('Wrote table file') + else: + print('##########Table-start########') + print(table_code) + print('##########Table-end##########') + + # report obvous errors + if num_errors_found: + print('Obvious errors found: %d' % num_errors_found) + else: + print('No obvious errors found') + +if __name__ == '__main__': + main() diff --git a/Tools/px4params/dokuwikiout.py b/Tools/px4params/dokuwikiout.py index 33f76b415e..4d40a6201d 100644 --- a/Tools/px4params/dokuwikiout.py +++ b/Tools/px4params/dokuwikiout.py @@ -5,23 +5,33 @@ class DokuWikiOutput(output.Output): result = "" for group in groups: result += "==== %s ====\n\n" % group.GetName() + result += "^ Name ^ Description ^ Min ^ Max ^ Default ^ Comment ^\n" for param in group.GetParams(): code = param.GetFieldValue("code") name = param.GetFieldValue("short_desc") - if code != name: - name = "%s (%s)" % (name, code) - result += "=== %s ===\n\n" % name - long_desc = param.GetFieldValue("long_desc") - if long_desc is not None: - result += "%s\n\n" % long_desc + name = name.replace("\n", "") + result += "| %s | %s " % (code, name) min_val = param.GetFieldValue("min") if min_val is not None: - result += "* Minimal value: %s\n" % min_val + result += "| %s " % min_val + else: + result += "|" max_val = param.GetFieldValue("max") if max_val is not None: - result += "* Maximal value: %s\n" % max_val + result += "| %s " % max_val + else: + result += "|" def_val = param.GetFieldValue("default") if def_val is not None: - result += "* Default value: %s\n" % def_val - result += "\n" + result += "| %s " % def_val + else: + result += "|" + long_desc = param.GetFieldValue("long_desc") + if long_desc is not None: + long_desc = long_desc.replace("\n", "") + result += "| %s " % long_desc + else: + result += "|" + result += "|\n" + result += "\n" return result diff --git a/Tools/px4params/dokuwikiout_listings.py b/Tools/px4params/dokuwikiout_listings.py new file mode 100644 index 0000000000..33f76b415e --- /dev/null +++ b/Tools/px4params/dokuwikiout_listings.py @@ -0,0 +1,27 @@ +import output + +class DokuWikiOutput(output.Output): + def Generate(self, groups): + result = "" + for group in groups: + result += "==== %s ====\n\n" % group.GetName() + for param in group.GetParams(): + code = param.GetFieldValue("code") + name = param.GetFieldValue("short_desc") + if code != name: + name = "%s (%s)" % (name, code) + result += "=== %s ===\n\n" % name + long_desc = param.GetFieldValue("long_desc") + if long_desc is not None: + result += "%s\n\n" % long_desc + min_val = param.GetFieldValue("min") + if min_val is not None: + result += "* Minimal value: %s\n" % min_val + max_val = param.GetFieldValue("max") + if max_val is not None: + result += "* Maximal value: %s\n" % max_val + def_val = param.GetFieldValue("default") + if def_val is not None: + result += "* Default value: %s\n" % def_val + result += "\n" + return result diff --git a/Tools/tests-host/Makefile b/Tools/tests-host/Makefile index 97410ff47c..7ab1454f01 100644 --- a/Tools/tests-host/Makefile +++ b/Tools/tests-host/Makefile @@ -10,11 +10,13 @@ LIBS=-lm #_DEPS = test.h #DEPS = $(patsubst %,$(IDIR)/%,$(_DEPS)) -_OBJ = mixer_test.o test_mixer.o mixer_simple.o mixer_multirotor.o mixer.o mixer_group.o mixer_load.o +_OBJ = mixer_test.o test_mixer.o mixer_simple.o mixer_multirotor.o \ + mixer.o mixer_group.o mixer_load.o test_conv.o pwm_limit.o hrt.o OBJ = $(patsubst %,$(ODIR)/%,$(_OBJ)) #$(DEPS) $(ODIR)/%.o: %.cpp + mkdir -p obj $(CC) -c -o $@ $< $(CFLAGS) $(ODIR)/%.o: ../../src/systemcmds/tests/%.cpp @@ -26,6 +28,12 @@ $(ODIR)/%.o: ../../src/modules/systemlib/%.cpp $(ODIR)/%.o: ../../src/modules/systemlib/mixer/%.cpp $(CC) -c -o $@ $< $(CFLAGS) +$(ODIR)/%.o: ../../src/modules/systemlib/pwm_limit/%.cpp + $(CC) -c -o $@ $< $(CFLAGS) + +$(ODIR)/%.o: ../../src/modules/systemlib/pwm_limit/%.c + $(CC) -c -o $@ $< $(CFLAGS) + $(ODIR)/%.o: ../../src/modules/systemlib/mixer/%.c $(CC) -c -o $@ $< $(CFLAGS) diff --git a/Tools/tests-host/hrt.cpp b/Tools/tests-host/hrt.cpp new file mode 100644 index 0000000000..01b5958b7c --- /dev/null +++ b/Tools/tests-host/hrt.cpp @@ -0,0 +1,16 @@ +#include +#include +#include +#include + +hrt_abstime hrt_absolute_time() { + struct timeval te; + gettimeofday(&te, NULL); // get current time + hrt_abstime us = static_cast(te.tv_sec) * 1e6 + te.tv_usec; // caculate us + return us; +} + +hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then) { + // not thread safe + return hrt_absolute_time() - *then; +} diff --git a/Tools/tests-host/mixer_test.cpp b/Tools/tests-host/mixer_test.cpp index 042322aadc..e311617f96 100644 --- a/Tools/tests-host/mixer_test.cpp +++ b/Tools/tests-host/mixer_test.cpp @@ -9,4 +9,6 @@ int main(int argc, char *argv[]) { "../../ROMFS/px4fmu_common/mixers/FMU_quad_w.mix"}; test_mixer(3, args); + + test_conv(1, args); } \ No newline at end of file diff --git a/Tools/tests-host/queue.h b/Tools/tests-host/queue.h new file mode 100644 index 0000000000..0fdb170db6 --- /dev/null +++ b/Tools/tests-host/queue.h @@ -0,0 +1,133 @@ +/************************************************************************ + * include/queue.h + * + * Copyright (C) 2007-2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ************************************************************************/ + +#ifndef __INCLUDE_QUEUE_H +#define __INCLUDE_QUEUE_H + +#ifndef FAR +#define FAR +#endif + +/************************************************************************ + * Included Files + ************************************************************************/ + +#include + +/************************************************************************ + * Pre-processor Definitions + ************************************************************************/ + +#define sq_init(q) do { (q)->head = NULL; (q)->tail = NULL; } while (0) +#define dq_init(q) do { (q)->head = NULL; (q)->tail = NULL; } while (0) + +#define sq_next(p) ((p)->flink) +#define dq_next(p) ((p)->flink) +#define dq_prev(p) ((p)->blink) + +#define sq_empty(q) ((q)->head == NULL) +#define dq_empty(q) ((q)->head == NULL) + +#define sq_peek(q) ((q)->head) +#define dq_peek(q) ((q)->head) + +/************************************************************************ + * Global Type Declarations + ************************************************************************/ + +struct sq_entry_s +{ + FAR struct sq_entry_s *flink; +}; +typedef struct sq_entry_s sq_entry_t; + +struct dq_entry_s +{ + FAR struct dq_entry_s *flink; + FAR struct dq_entry_s *blink; +}; +typedef struct dq_entry_s dq_entry_t; + +struct sq_queue_s +{ + FAR sq_entry_t *head; + FAR sq_entry_t *tail; +}; +typedef struct sq_queue_s sq_queue_t; + +struct dq_queue_s +{ + FAR dq_entry_t *head; + FAR dq_entry_t *tail; +}; +typedef struct dq_queue_s dq_queue_t; + +/************************************************************************ + * Global Function Prototypes + ************************************************************************/ + +#ifdef __cplusplus +#define EXTERN extern "C" +extern "C" { +#else +#define EXTERN extern +#endif + +EXTERN void sq_addfirst(FAR sq_entry_t *node, sq_queue_t *queue); +EXTERN void dq_addfirst(FAR dq_entry_t *node, dq_queue_t *queue); +EXTERN void sq_addlast(FAR sq_entry_t *node, sq_queue_t *queue); +EXTERN void dq_addlast(FAR dq_entry_t *node, dq_queue_t *queue); +EXTERN void sq_addafter(FAR sq_entry_t *prev, FAR sq_entry_t *node, + sq_queue_t *queue); +EXTERN void dq_addafter(FAR dq_entry_t *prev, FAR dq_entry_t *node, + dq_queue_t *queue); +EXTERN void dq_addbefore(FAR dq_entry_t *next, FAR dq_entry_t *node, + dq_queue_t *queue); + +EXTERN FAR sq_entry_t *sq_remafter(FAR sq_entry_t *node, sq_queue_t *queue); +EXTERN void sq_rem(FAR sq_entry_t *node, sq_queue_t *queue); +EXTERN void dq_rem(FAR dq_entry_t *node, dq_queue_t *queue); +EXTERN FAR sq_entry_t *sq_remlast(sq_queue_t *queue); +EXTERN FAR dq_entry_t *dq_remlast(dq_queue_t *queue); +EXTERN FAR sq_entry_t *sq_remfirst(sq_queue_t *queue); +EXTERN FAR dq_entry_t *dq_remfirst(dq_queue_t *queue); + +#undef EXTERN +#ifdef __cplusplus +} +#endif + +#endif /* __INCLUDE_QUEUE_H_ */ + diff --git a/makefiles/config_px4fmu-v1_backside.mk b/makefiles/config_px4fmu-v1_backside.mk deleted file mode 100644 index 6d2a9f7bd4..0000000000 --- a/makefiles/config_px4fmu-v1_backside.mk +++ /dev/null @@ -1,148 +0,0 @@ -# -# Makefile for the px4fmu_default configuration -# - -# -# Use the configuration's ROMFS. -# -ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common -ROMFS_OPTIONAL_FILES = $(PX4_BASE)/Images/px4io-v1_default.bin - -# -# Board support modules -# -MODULES += drivers/device -MODULES += drivers/stm32 -MODULES += drivers/stm32/adc -MODULES += drivers/stm32/tone_alarm -MODULES += drivers/led -MODULES += drivers/px4io -MODULES += drivers/px4fmu -MODULES += drivers/boards/px4fmu-v1 -MODULES += drivers/ardrone_interface -MODULES += drivers/l3gd20 -MODULES += drivers/bma180 -MODULES += drivers/mpu6000 -MODULES += drivers/hmc5883 -MODULES += drivers/ms5611 -MODULES += drivers/gps -MODULES += drivers/hil -MODULES += drivers/blinkm -MODULES += drivers/rgbled -MODULES += drivers/roboclaw -MODULES += drivers/airspeed -MODULES += drivers/ets_airspeed -MODULES += drivers/meas_airspeed -MODULES += modules/sensors - -# -# System commands -# -MODULES += systemcmds/eeprom -MODULES += systemcmds/ramtron -MODULES += systemcmds/bl_update -MODULES += systemcmds/boardinfo -MODULES += systemcmds/i2c -MODULES += systemcmds/mixer -MODULES += systemcmds/param -MODULES += systemcmds/perf -MODULES += systemcmds/preflight_check -MODULES += systemcmds/pwm -MODULES += systemcmds/esc_calib -MODULES += systemcmds/reboot -MODULES += systemcmds/top -MODULES += systemcmds/tests -MODULES += systemcmds/config -MODULES += systemcmds/nshterm - -# -# General system control -# -MODULES += modules/commander -MODULES += modules/navigator -MODULES += modules/mavlink -MODULES += modules/mavlink_onboard -MODULES += modules/gpio_led - -# -# Estimation modules (EKF/ SO3 / other filters) -# -MODULES += modules/att_pos_estimator_ekf - -# -# Vehicle Control -# -MODULES += modules/fixedwing_backside - -# -# Logging -# -MODULES += modules/sdlog2 - -# -# Unit tests -# -#MODULES += modules/unit_test -#MODULES += modules/commander/commander_tests - -# -# Library modules -# -MODULES += modules/systemlib -MODULES += modules/systemlib/mixer -MODULES += modules/controllib -MODULES += modules/uORB -MODULES += modules/dataman - -# -# Libraries -# -LIBRARIES += lib/mathlib/CMSIS -MODULES += lib/mathlib -MODULES += lib/mathlib/math/filter -MODULES += lib/ecl -MODULES += lib/external_lgpl -MODULES += lib/geo -MODULES += lib/conversion -MODULES += lib/launchdetection - -# -# Demo apps -# -#MODULES += examples/math_demo -# Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/hello_sky -#MODULES += examples/px4_simple_app - -# Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/daemon -#MODULES += examples/px4_daemon_app - -# Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/debug_values -#MODULES += examples/px4_mavlink_debug - -# Tutorial code from -# https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control -#MODULES += examples/fixedwing_control - -# Hardware test -#MODULES += examples/hwtest - -# -# Transitional support - add commands from the NuttX export archive. -# -# In general, these should move to modules over time. -# -# Each entry here is ... but we use a helper macro -# to make the table a bit more readable. -# -define _B - $(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4) -endef - -# command priority stack entrypoint -BUILTIN_COMMANDS := \ - $(call _B, sercon, , 2048, sercon_main ) \ - $(call _B, serdis, , 2048, serdis_main ) \ - $(call _B, sysinfo, , 2048, sysinfo_main ) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index fba50aaf81..51be7e1a12 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -36,13 +36,13 @@ MODULES += drivers/mkblctrl MODULES += drivers/airspeed MODULES += drivers/ets_airspeed MODULES += drivers/meas_airspeed +MODULES += drivers/frsky_telemetry MODULES += modules/sensors # # System commands # -MODULES += systemcmds/eeprom -MODULES += systemcmds/ramtron +MODULES += systemcmds/mtd MODULES += systemcmds/bl_update MODULES += systemcmds/boardinfo MODULES += systemcmds/i2c @@ -57,6 +57,7 @@ MODULES += systemcmds/top MODULES += systemcmds/tests MODULES += systemcmds/config MODULES += systemcmds/nshterm +MODULES += systemcmds/hw_ver # # General system control @@ -81,8 +82,8 @@ MODULES += modules/position_estimator_inav # MODULES += modules/fw_pos_control_l1 MODULES += modules/fw_att_control -MODULES += modules/multirotor_att_control -MODULES += modules/multirotor_pos_control +MODULES += modules/mc_att_control +MODULES += modules/mc_pos_control #MODULES += examples/flow_position_control #MODULES += examples/flow_speed_control diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index f0a9c0c065..ab05d4e3da 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -36,6 +36,7 @@ MODULES += drivers/roboclaw MODULES += drivers/airspeed MODULES += drivers/ets_airspeed MODULES += drivers/meas_airspeed +MODULES += drivers/frsky_telemetry MODULES += modules/sensors # Needs to be burned to the ground and re-written; for now, @@ -45,7 +46,6 @@ MODULES += modules/sensors # # System commands # -MODULES += systemcmds/ramtron MODULES += systemcmds/bl_update MODULES += systemcmds/boardinfo MODULES += systemcmds/mixer @@ -59,6 +59,8 @@ MODULES += systemcmds/top MODULES += systemcmds/tests MODULES += systemcmds/config MODULES += systemcmds/nshterm +MODULES += systemcmds/mtd +MODULES += systemcmds/hw_ver # # General system control @@ -83,8 +85,8 @@ MODULES += examples/flow_position_estimator #MODULES += modules/segway # XXX Needs GCC 4.7 fix MODULES += modules/fw_pos_control_l1 MODULES += modules/fw_att_control -MODULES += modules/multirotor_att_control -MODULES += modules/multirotor_pos_control +MODULES += modules/mc_att_control +MODULES += modules/mc_pos_control # # Logging diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index 0f60e88b5e..8623c0584b 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -6,23 +6,37 @@ # Use the configuration's ROMFS. # ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_test +ROMFS_OPTIONAL_FILES = $(PX4_BASE)/Images/px4io-v2_default.bin # # Board support modules # MODULES += drivers/device MODULES += drivers/stm32 +MODULES += drivers/stm32/adc +MODULES += drivers/stm32/tone_alarm MODULES += drivers/led MODULES += drivers/boards/px4fmu-v2 +MODULES += drivers/px4io MODULES += systemcmds/perf MODULES += systemcmds/reboot +MODULES += systemcmds/tests +MODULES += systemcmds/nshterm +MODULES += systemcmds/mtd +MODULES += systemcmds/hw_ver # # Library modules # MODULES += modules/systemlib +MODULES += modules/systemlib/mixer MODULES += modules/uORB +# +# Libraries +# +LIBRARIES += lib/mathlib/CMSIS + # # Transitional support - add commands from the NuttX export archive. # diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index e43b9c18e8..1dc96b3c3f 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -460,7 +460,7 @@ CONFIG_MMCSD_NSLOTS=1 CONFIG_MMCSD_SPI=y CONFIG_MMCSD_SPICLOCK=24000000 # CONFIG_MMCSD_SDIO is not set -# CONFIG_MTD is not set +CONFIG_MTD=y CONFIG_PIPES=y # CONFIG_PM is not set # CONFIG_POWER is not set @@ -482,6 +482,25 @@ CONFIG_USART1_SERIAL_CONSOLE=y # CONFIG_USART6_SERIAL_CONSOLE is not set # CONFIG_NO_SERIAL_CONSOLE is not set +# +# MTD Configuration +# +CONFIG_MTD_PARTITION=y +CONFIG_MTD_BYTE_WRITE=y + +# +# MTD Device Drivers +# +# CONFIG_RAMMTD is not set +# CONFIG_MTD_AT24XX is not set +# CONFIG_MTD_AT45DB is not set +# CONFIG_MTD_M25P is not set +# CONFIG_MTD_SMART is not set +# CONFIG_MTD_RAMTRON is not set +# CONFIG_MTD_SST25 is not set +# CONFIG_MTD_SST39FV is not set +# CONFIG_MTD_W25 is not set + # # USART1 Configuration # @@ -566,7 +585,7 @@ CONFIG_CDCACM_NWRREQS=4 CONFIG_CDCACM_NRDREQS=4 CONFIG_CDCACM_BULKIN_REQLEN=96 CONFIG_CDCACM_RXBUFSIZE=512 -CONFIG_CDCACM_TXBUFSIZE=512 +CONFIG_CDCACM_TXBUFSIZE=2048 CONFIG_CDCACM_VENDORID=0x26ac CONFIG_CDCACM_PRODUCTID=0x0010 CONFIG_CDCACM_VENDORSTR="3D Robotics" diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 110bcb3637..2a734c27e5 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -295,16 +295,16 @@ CONFIG_STM32_USART=y # U[S]ART Configuration # # CONFIG_USART1_RS485 is not set -# CONFIG_USART1_RXDMA is not set +CONFIG_USART1_RXDMA=y # CONFIG_USART2_RS485 is not set CONFIG_USART2_RXDMA=y # CONFIG_USART3_RS485 is not set CONFIG_USART3_RXDMA=y # CONFIG_UART4_RS485 is not set CONFIG_UART4_RXDMA=y -# CONFIG_UART5_RXDMA is not set +CONFIG_UART5_RXDMA=y # CONFIG_USART6_RS485 is not set -# CONFIG_USART6_RXDMA is not set +CONFIG_USART6_RXDMA=y # CONFIG_UART7_RS485 is not set # CONFIG_UART7_RXDMA is not set # CONFIG_UART8_RS485 is not set @@ -500,8 +500,8 @@ CONFIG_MTD=y # # MTD Configuration # -# CONFIG_MTD_PARTITION is not set -# CONFIG_MTD_BYTE_WRITE is not set +CONFIG_MTD_PARTITION=y +CONFIG_MTD_BYTE_WRITE=y # # MTD Device Drivers @@ -582,8 +582,8 @@ CONFIG_USART3_OFLOWCONTROL=y # # UART4 Configuration # -CONFIG_UART4_RXBUFSIZE=128 -CONFIG_UART4_TXBUFSIZE=128 +CONFIG_UART4_RXBUFSIZE=512 +CONFIG_UART4_TXBUFSIZE=512 CONFIG_UART4_BAUD=57600 CONFIG_UART4_BITS=8 CONFIG_UART4_PARITY=0 @@ -594,8 +594,8 @@ CONFIG_UART4_2STOP=0 # # USART6 Configuration # -CONFIG_USART6_RXBUFSIZE=256 -CONFIG_USART6_TXBUFSIZE=256 +CONFIG_USART6_RXBUFSIZE=512 +CONFIG_USART6_TXBUFSIZE=512 CONFIG_USART6_BAUD=57600 CONFIG_USART6_BITS=8 CONFIG_USART6_PARITY=0 @@ -662,7 +662,7 @@ CONFIG_CDCACM_NWRREQS=4 CONFIG_CDCACM_NRDREQS=4 CONFIG_CDCACM_BULKIN_REQLEN=96 CONFIG_CDCACM_RXBUFSIZE=512 -CONFIG_CDCACM_TXBUFSIZE=512 +CONFIG_CDCACM_TXBUFSIZE=2048 CONFIG_CDCACM_VENDORID=0x26ac CONFIG_CDCACM_PRODUCTID=0x0011 CONFIG_CDCACM_VENDORSTR="3D Robotics" diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index 5e45cc936c..f73a3ef01e 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -91,7 +91,7 @@ Airspeed::Airspeed(int bus, int address, unsigned conversion_interval) : _comms_errors(perf_alloc(PC_COUNT, "airspeed_comms_errors")) { // 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)); diff --git a/src/drivers/boards/px4fmu-v1/board_config.h b/src/drivers/boards/px4fmu-v1/board_config.h index 6f71662846..02c26b5c0b 100644 --- a/src/drivers/boards/px4fmu-v1/board_config.h +++ b/src/drivers/boards/px4fmu-v1/board_config.h @@ -60,6 +60,7 @@ __BEGIN_DECLS /* PX4IO connection configuration */ #define PX4IO_SERIAL_DEVICE "/dev/ttyS2" +#define UDID_START 0x1FFF7A10 //#ifdef CONFIG_STM32_SPI2 //# error "SPI2 is not supported on this board" diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index a19ed9d247..7cfca7656e 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -52,6 +52,8 @@ __BEGIN_DECLS /* these headers are not C++ safe */ #include #include + +#define UDID_START 0x1FFF7A10 /**************************************************************************************************** * Definitions @@ -73,7 +75,7 @@ __BEGIN_DECLS /* PX4FMU GPIOs ***********************************************************************************/ /* LEDs */ -#define GPIO_LED1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN12) +#define GPIO_LED1 (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN12) /* External interrupts */ #define GPIO_EXTI_GYRO_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN0) @@ -85,7 +87,7 @@ __BEGIN_DECLS #define GPIO_GYRO_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN0) #define GPIO_MAG_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN1) #define GPIO_ACCEL_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN4) -#define GPIO_EXTI_MPU_DRDY (GPIO_INPUT|GPIO_PULLDOWN|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15) +#define GPIO_EXTI_MPU_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15) /* SPI1 off */ #define GPIO_SPI1_SCK_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN5) @@ -96,7 +98,7 @@ __BEGIN_DECLS #define GPIO_SPI_CS_GYRO_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN13) #define GPIO_SPI_CS_ACCEL_MAG_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN15) #define GPIO_SPI_CS_BARO_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTD|GPIO_PIN7) -#define GPIO_SPI_CS_MPU (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN2) +#define GPIO_SPI_CS_MPU_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN2) /* SPI chip selects */ #define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c index 269ec238eb..71414d62c7 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c @@ -282,7 +282,7 @@ __EXPORT int nsh_archinitialize(void) SPI_SELECT(spi1, PX4_SPIDEV_MPU, false); up_udelay(20); - message("[boot] Successfully initialized SPI port 1\n"); + message("[boot] Initialized SPI port 1 (SENSORS)\n"); /* Get the SPI port for the FRAM */ @@ -294,20 +294,23 @@ __EXPORT int nsh_archinitialize(void) return -ENODEV; } - /* Default SPI2 to 37.5 MHz (F4 max) and de-assert the known chip selects. */ - SPI_SETFREQUENCY(spi2, 375000000); + /* Default SPI2 to 37.5 MHz (40 MHz rounded to nearest valid divider, F4 max) + * and de-assert the known chip selects. */ + + // XXX start with 10.4 MHz in FRAM usage and go up to 37.5 once validated + SPI_SETFREQUENCY(spi2, 12 * 1000 * 1000); SPI_SETBITS(spi2, 8); SPI_SETMODE(spi2, SPIDEV_MODE3); SPI_SELECT(spi2, SPIDEV_FLASH, false); - message("[boot] Successfully initialized SPI port 2\n"); + message("[boot] Initialized SPI port 2 (RAMTRON FRAM)\n"); #ifdef CONFIG_MMCSD /* First, get an instance of the SDIO interface */ sdio = sdio_initialize(CONFIG_NSH_MMCSDSLOTNO); if (!sdio) { - message("nsh_archinitialize: Failed to initialize SDIO slot %d\n", + message("[boot] Failed to initialize SDIO slot %d\n", CONFIG_NSH_MMCSDSLOTNO); return -ENODEV; } @@ -315,7 +318,7 @@ __EXPORT int nsh_archinitialize(void) /* Now bind the SDIO interface to the MMC/SD driver */ int ret = mmcsd_slotinitialize(CONFIG_NSH_MMCSDMINOR, sdio); if (ret != OK) { - message("nsh_archinitialize: Failed to bind SDIO to the MMC/SD driver: %d\n", ret); + message("[boot] Failed to bind SDIO to the MMC/SD driver: %d\n", ret); return ret; } diff --git a/src/drivers/boards/px4io-v1/board_config.h b/src/drivers/boards/px4io-v1/board_config.h index c3f39addfe..1be4877bad 100644 --- a/src/drivers/boards/px4io-v1/board_config.h +++ b/src/drivers/boards/px4io-v1/board_config.h @@ -58,11 +58,11 @@ /* PX4IO GPIOs **********************************************************************/ /* LEDs */ -#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ +#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|\ GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN14) -#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ +#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|\ GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN15) -#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\ +#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|\ GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) /* Safety switch button *************************************************************/ diff --git a/src/drivers/boards/px4io-v2/board_config.h b/src/drivers/boards/px4io-v2/board_config.h index 8da5552110..ef9bb5cadd 100644 --- a/src/drivers/boards/px4io-v2/board_config.h +++ b/src/drivers/boards/px4io-v2/board_config.h @@ -74,9 +74,9 @@ /* LEDS **********************************************************************/ -#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14) -#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15) -#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13) +#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) /* Safety switch button *******************************************************/ @@ -114,7 +114,7 @@ /* XXX these should be UART pins */ #define GPIO_SBUS_INPUT (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN11) #define GPIO_SBUS_OUTPUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) -#define GPIO_SBUS_OENABLE (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN4) +#define GPIO_SBUS_OENABLE (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN4) /* * High-resolution timer diff --git a/src/drivers/boards/px4io-v2/px4iov2_init.c b/src/drivers/boards/px4io-v2/px4iov2_init.c index ccd01edf56..9f8c0eeb27 100644 --- a/src/drivers/boards/px4io-v2/px4iov2_init.c +++ b/src/drivers/boards/px4io-v2/px4iov2_init.c @@ -124,8 +124,6 @@ __EXPORT void stm32_boardinitialize(void) stm32_configgpio(GPIO_ADC_VSERVO); stm32_configgpio(GPIO_SBUS_INPUT); /* xxx alternate function */ - - stm32_gpiowrite(GPIO_SBUS_OUTPUT, false); stm32_configgpio(GPIO_SBUS_OUTPUT); /* sbus output enable is active low - disable it by default */ diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 51f916f37b..88da94b1ed 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -189,6 +189,10 @@ ORB_DECLARE(output_pwm); /** get the maximum PWM value the output will send */ #define PWM_SERVO_GET_MAX_PWM _IOC(_PWM_SERVO_BASE, 19) +/** set the number of servos in (unsigned)arg - allows change of + * split between servos and GPIO */ +#define PWM_SERVO_SET_COUNT _IOC(_PWM_SERVO_BASE, 20) + /** set a single servo to a specific value */ #define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo) diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c new file mode 100644 index 0000000000..e201ecbb3d --- /dev/null +++ b/src/drivers/frsky_telemetry/frsky_data.c @@ -0,0 +1,289 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * Author: Stefan Rado + * + * 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 frsky_data.c + * @author Stefan Rado + * + * FrSky telemetry implementation. + * + */ + +#include "frsky_data.h" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +/* FrSky sensor hub data IDs */ +#define FRSKY_ID_GPS_ALT_BP 0x01 +#define FRSKY_ID_TEMP1 0x02 +#define FRSKY_ID_RPM 0x03 +#define FRSKY_ID_FUEL 0x04 +#define FRSKY_ID_TEMP2 0x05 +#define FRSKY_ID_VOLTS 0x06 +#define FRSKY_ID_GPS_ALT_AP 0x09 +#define FRSKY_ID_BARO_ALT_BP 0x10 +#define FRSKY_ID_GPS_SPEED_BP 0x11 +#define FRSKY_ID_GPS_LONG_BP 0x12 +#define FRSKY_ID_GPS_LAT_BP 0x13 +#define FRSKY_ID_GPS_COURS_BP 0x14 +#define FRSKY_ID_GPS_DAY_MONTH 0x15 +#define FRSKY_ID_GPS_YEAR 0x16 +#define FRSKY_ID_GPS_HOUR_MIN 0x17 +#define FRSKY_ID_GPS_SEC 0x18 +#define FRSKY_ID_GPS_SPEED_AP 0x19 +#define FRSKY_ID_GPS_LONG_AP 0x1A +#define FRSKY_ID_GPS_LAT_AP 0x1B +#define FRSKY_ID_GPS_COURS_AP 0x1C +#define FRSKY_ID_BARO_ALT_AP 0x21 +#define FRSKY_ID_GPS_LONG_EW 0x22 +#define FRSKY_ID_GPS_LAT_NS 0x23 +#define FRSKY_ID_ACCEL_X 0x24 +#define FRSKY_ID_ACCEL_Y 0x25 +#define FRSKY_ID_ACCEL_Z 0x26 +#define FRSKY_ID_CURRENT 0x28 +#define FRSKY_ID_VARIO 0x30 +#define FRSKY_ID_VFAS 0x39 +#define FRSKY_ID_VOLTS_BP 0x3A +#define FRSKY_ID_VOLTS_AP 0x3B + +#define frac(f) (f - (int)f) + +static int battery_sub = -1; +static int sensor_sub = -1; +static int global_position_sub = -1; +static int vehicle_status_sub = -1; + +/** + * Initializes the uORB subscriptions. + */ +void frsky_init() +{ + battery_sub = orb_subscribe(ORB_ID(battery_status)); + global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); +} + +/** + * Sends a 0x5E start/stop byte. + */ +static void frsky_send_startstop(int uart) +{ + static const uint8_t c = 0x5E; + write(uart, &c, sizeof(c)); +} + +/** + * Sends one byte, performing byte-stuffing if necessary. + */ +static void frsky_send_byte(int uart, uint8_t value) +{ + const uint8_t x5E[] = { 0x5D, 0x3E }; + const uint8_t x5D[] = { 0x5D, 0x3D }; + + switch (value) { + case 0x5E: + write(uart, x5E, sizeof(x5E)); + break; + + case 0x5D: + write(uart, x5D, sizeof(x5D)); + break; + + default: + write(uart, &value, sizeof(value)); + break; + } +} + +/** + * Sends one data id/value pair. + */ +static void frsky_send_data(int uart, uint8_t id, int16_t data) +{ + /* Cast data to unsigned, because signed shift might behave incorrectly */ + uint16_t udata = data; + + frsky_send_startstop(uart); + + frsky_send_byte(uart, id); + frsky_send_byte(uart, udata); /* LSB */ + frsky_send_byte(uart, udata >> 8); /* MSB */ +} + +/** + * Sends frame 1 (every 200ms): + * acceleration values, barometer altitude, temperature, battery voltage & current + */ +void frsky_send_frame1(int uart) +{ + /* get a local copy of the current sensor values */ + struct sensor_combined_s raw; + memset(&raw, 0, sizeof(raw)); + orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); + + /* get a local copy of the battery data */ + struct battery_status_s battery; + memset(&battery, 0, sizeof(battery)); + orb_copy(ORB_ID(battery_status), battery_sub, &battery); + + /* send formatted frame */ + frsky_send_data(uart, FRSKY_ID_ACCEL_X, + roundf(raw.accelerometer_m_s2[0] * 1000.0f)); + frsky_send_data(uart, FRSKY_ID_ACCEL_Y, + roundf(raw.accelerometer_m_s2[1] * 1000.0f)); + frsky_send_data(uart, FRSKY_ID_ACCEL_Z, + roundf(raw.accelerometer_m_s2[2] * 1000.0f)); + + frsky_send_data(uart, FRSKY_ID_BARO_ALT_BP, + raw.baro_alt_meter); + frsky_send_data(uart, FRSKY_ID_BARO_ALT_AP, + roundf(frac(raw.baro_alt_meter) * 100.0f)); + + frsky_send_data(uart, FRSKY_ID_TEMP1, + roundf(raw.baro_temp_celcius)); + + frsky_send_data(uart, FRSKY_ID_VFAS, + roundf(battery.voltage_v * 10.0f)); + frsky_send_data(uart, FRSKY_ID_CURRENT, + (battery.current_a < 0) ? 0 : roundf(battery.current_a * 10.0f)); + + frsky_send_startstop(uart); +} + +/** + * Formats the decimal latitude/longitude to the required degrees/minutes/seconds. + */ +static float frsky_format_gps(float dec) +{ + float dms_deg = (int) dec; + float dec_deg = dec - dms_deg; + float dms_min = (int) (dec_deg * 60); + float dec_min = (dec_deg * 60) - dms_min; + float dms_sec = dec_min * 60; + + return (dms_deg * 100.0f) + dms_min + (dms_sec / 100.0f); +} + +/** + * Sends frame 2 (every 1000ms): + * GPS course, latitude, longitude, ground speed, GPS altitude, remaining battery level + */ +void frsky_send_frame2(int uart) +{ + /* get a local copy of the global position data */ + struct vehicle_global_position_s global_pos; + memset(&global_pos, 0, sizeof(global_pos)); + orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos); + + /* get a local copy of the vehicle status data */ + struct vehicle_status_s vehicle_status; + memset(&vehicle_status, 0, sizeof(vehicle_status)); + orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status); + + /* send formatted frame */ + float course = 0, lat = 0, lon = 0, speed = 0, alt = 0; + char lat_ns = 0, lon_ew = 0; + int sec = 0; + if (global_pos.valid) { + time_t time_gps = global_pos.time_gps_usec / 1000000; + struct tm *tm_gps = gmtime(&time_gps); + + course = (global_pos.yaw + M_PI_F) / M_PI_F * 180.0f; + lat = frsky_format_gps(abs(global_pos.lat)); + lat_ns = (global_pos.lat < 0) ? 'S' : 'N'; + lon = frsky_format_gps(abs(global_pos.lon)); + lon_ew = (global_pos.lon < 0) ? 'W' : 'E'; + speed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e) + * 25.0f / 46.0f; + alt = global_pos.alt; + sec = tm_gps->tm_sec; + } + + frsky_send_data(uart, FRSKY_ID_GPS_COURS_BP, course); + frsky_send_data(uart, FRSKY_ID_GPS_COURS_AP, frac(course) * 1000.0f); + + frsky_send_data(uart, FRSKY_ID_GPS_LAT_BP, lat); + frsky_send_data(uart, FRSKY_ID_GPS_LAT_AP, frac(lat) * 10000.0f); + frsky_send_data(uart, FRSKY_ID_GPS_LAT_NS, lat_ns); + + frsky_send_data(uart, FRSKY_ID_GPS_LONG_BP, lon); + frsky_send_data(uart, FRSKY_ID_GPS_LONG_AP, frac(lon) * 10000.0f); + frsky_send_data(uart, FRSKY_ID_GPS_LONG_EW, lon_ew); + + frsky_send_data(uart, FRSKY_ID_GPS_SPEED_BP, speed); + frsky_send_data(uart, FRSKY_ID_GPS_SPEED_AP, frac(speed) * 100.0f); + + frsky_send_data(uart, FRSKY_ID_GPS_ALT_BP, alt); + frsky_send_data(uart, FRSKY_ID_GPS_ALT_AP, frac(alt) * 100.0f); + + frsky_send_data(uart, FRSKY_ID_FUEL, + roundf(vehicle_status.battery_remaining * 100.0f)); + + frsky_send_data(uart, FRSKY_ID_GPS_SEC, sec); + + frsky_send_startstop(uart); +} + +/** + * Sends frame 3 (every 5000ms): + * GPS date & time + */ +void frsky_send_frame3(int uart) +{ + /* get a local copy of the battery data */ + struct vehicle_global_position_s global_pos; + memset(&global_pos, 0, sizeof(global_pos)); + orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos); + + /* send formatted frame */ + time_t time_gps = global_pos.time_gps_usec / 1000000; + struct tm *tm_gps = gmtime(&time_gps); + uint16_t hour_min = (tm_gps->tm_min << 8) | (tm_gps->tm_hour & 0xff); + frsky_send_data(uart, FRSKY_ID_GPS_DAY_MONTH, tm_gps->tm_mday); + frsky_send_data(uart, FRSKY_ID_GPS_YEAR, tm_gps->tm_year); + frsky_send_data(uart, FRSKY_ID_GPS_HOUR_MIN, hour_min); + frsky_send_data(uart, FRSKY_ID_GPS_SEC, tm_gps->tm_sec); + + frsky_send_startstop(uart); +} diff --git a/src/lib/mathlib/math/generic/Vector.cpp b/src/drivers/frsky_telemetry/frsky_data.h similarity index 79% rename from src/lib/mathlib/math/generic/Vector.cpp rename to src/drivers/frsky_telemetry/frsky_data.h index 7ea6496bb1..a7d9eee533 100644 --- a/src/lib/mathlib/math/generic/Vector.cpp +++ b/src/drivers/frsky_telemetry/frsky_data.h @@ -1,6 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * Author: Stefan Rado * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,9 +33,19 @@ ****************************************************************************/ /** - * @file Vector.cpp + * @file frsky_data.h + * @author Stefan Rado + * + * FrSky telemetry implementation. * - * math vector */ +#ifndef _FRSKY_DATA_H +#define _FRSKY_DATA_H -#include "Vector.hpp" +// Public functions +void frsky_init(void); +void frsky_send_frame1(int uart); +void frsky_send_frame2(int uart); +void frsky_send_frame3(int uart); + +#endif /* _FRSKY_TELEMETRY_H */ diff --git a/src/drivers/frsky_telemetry/frsky_telemetry.c b/src/drivers/frsky_telemetry/frsky_telemetry.c new file mode 100644 index 0000000000..7b08ca69e9 --- /dev/null +++ b/src/drivers/frsky_telemetry/frsky_telemetry.c @@ -0,0 +1,266 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * Author: Stefan Rado + * + * 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 frsky_telemetry.c + * @author Stefan Rado + * + * FrSky telemetry implementation. + * + * This daemon emulates an FrSky sensor hub by periodically sending data + * packets to an attached FrSky receiver. + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "frsky_data.h" + + +/* thread state */ +static volatile bool thread_should_exit = false; +static volatile bool thread_running = false; +static int frsky_task; + +/* functions */ +static int frsky_open_uart(const char *uart_name, struct termios *uart_config_original); +static void usage(void); +static int frsky_telemetry_thread_main(int argc, char *argv[]); +__EXPORT int frsky_telemetry_main(int argc, char *argv[]); + + +/** + * Opens the UART device and sets all required serial parameters. + */ +static int frsky_open_uart(const char *uart_name, struct termios *uart_config_original) +{ + /* Open UART */ + const int uart = open(uart_name, O_WRONLY | O_NOCTTY); + + if (uart < 0) { + err(1, "Error opening port: %s", uart_name); + } + + /* 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); + close(uart); + return -1; + } + + /* Fill the struct for the new configuration */ + struct termios uart_config; + tcgetattr(uart, &uart_config); + + /* Disable output post-processing */ + uart_config.c_oflag &= ~OPOST; + + /* Set baud rate */ + 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); + 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); + close(uart); + return -1; + } + + return uart; +} + +/** + * Print command usage information + */ +static void usage() +{ + fprintf(stderr, + "usage: frsky_telemetry start [-d ]\n" + " frsky_telemetry stop\n" + " frsky_telemetry status\n"); + exit(1); +} + +/** + * The daemon thread. + */ +static int frsky_telemetry_thread_main(int argc, char *argv[]) +{ + /* Default values for arguments */ + char *device_name = "/dev/ttyS1"; /* USART2 */ + + /* Work around some stupidity in task_create's argv handling */ + argc -= 2; + argv += 2; + + int ch; + while ((ch = getopt(argc, argv, "d:")) != EOF) { + switch (ch) { + case 'd': + device_name = optarg; + break; + + default: + usage(); + break; + } + } + + /* 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); + + if (uart < 0) + err(1, "could not open %s", device_name); + + /* Subscribe to topics */ + frsky_init(); + + thread_running = true; + + /* Main thread loop */ + unsigned int iteration = 0; + while (!thread_should_exit) { + + /* Sleep 200 ms */ + usleep(200000); + + /* Send frame 1 (every 200ms): acceleration values, altitude (vario), temperatures, current & voltages, RPM */ + frsky_send_frame1(uart); + + /* Send frame 2 (every 1000ms): course, latitude, longitude, speed, altitude (GPS), fuel level */ + if (iteration % 5 == 0) + { + frsky_send_frame2(uart); + } + + /* Send frame 3 (every 5000ms): date, time */ + if (iteration % 25 == 0) + { + frsky_send_frame3(uart); + + iteration = 0; + } + + iteration++; + } + + /* Reset the UART flags to original state */ + tcsetattr(uart, TCSANOW, &uart_config_original); + close(uart); + + thread_running = false; + return 0; +} + +/** + * The main command function. + * Processes command line arguments and starts the daemon. + */ +int frsky_telemetry_main(int argc, char *argv[]) +{ + if (argc < 2) { + warnx("missing command"); + usage(); + } + + if (!strcmp(argv[1], "start")) { + + /* this is not an error */ + if (thread_running) + errx(0, "frsky_telemetry already running"); + + thread_should_exit = false; + frsky_task = task_spawn_cmd("frsky_telemetry", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2048, + frsky_telemetry_thread_main, + (const char **)argv); + + while (!thread_running) { + usleep(200); + } + + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + + /* this is not an error */ + if (!thread_running) + errx(0, "frsky_telemetry already stopped"); + + thread_should_exit = true; + + while (thread_running) { + usleep(200000); + warnx("."); + } + + warnx("terminated."); + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + errx(0, "running"); + + } else { + errx(1, "not running"); + } + } + + warnx("unrecognized command"); + usage(); + /* not getting here */ + return 0; +} diff --git a/src/systemcmds/eeprom/module.mk b/src/drivers/frsky_telemetry/module.mk similarity index 89% rename from src/systemcmds/eeprom/module.mk rename to src/drivers/frsky_telemetry/module.mk index 07f3945be2..1632c74f75 100644 --- a/src/systemcmds/eeprom/module.mk +++ b/src/drivers/frsky_telemetry/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 @@ -32,8 +32,10 @@ ############################################################################ # -# EEPROM file system driver +# FrSky telemetry application. # -MODULE_COMMAND = eeprom -SRCS = 24xxxx_mtd.c eeprom.c +MODULE_COMMAND = frsky_telemetry + +SRCS = frsky_data.c \ + frsky_telemetry.c diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index fc500a9ecd..6b72d560fa 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -85,7 +85,7 @@ static const int ERROR = -1; class GPS : public device::CDev { public: - GPS(const char *uart_path); + GPS(const char *uart_path, bool fake_gps); virtual ~GPS(); virtual int init(); @@ -112,6 +112,7 @@ private: struct vehicle_gps_position_s _report; ///< uORB topic for gps position orb_advert_t _report_pub; ///< uORB pub for gps position float _rate; ///< position update rate + bool _fake_gps; ///< fake gps output /** @@ -156,7 +157,7 @@ GPS *g_dev; } -GPS::GPS(const char *uart_path) : +GPS::GPS(const char *uart_path, bool fake_gps) : CDev("gps", GPS_DEVICE_PATH), _task_should_exit(false), _healthy(false), @@ -164,7 +165,8 @@ GPS::GPS(const char *uart_path) : _mode(GPS_DRIVER_MODE_UBX), _Helper(nullptr), _report_pub(-1), - _rate(0.0f) + _rate(0.0f), + _fake_gps(fake_gps) { /* store port name */ strncpy(_port, uart_path, sizeof(_port)); @@ -264,98 +266,133 @@ GPS::task_main() /* loop handling received serial bytes and also configuring in between */ while (!_task_should_exit) { - if (_Helper != nullptr) { - delete(_Helper); - /* set to zero to ensure parser is not used while not instantiated */ - _Helper = nullptr; - } + if (_fake_gps) { - switch (_mode) { - case GPS_DRIVER_MODE_UBX: - _Helper = new UBX(_serial_fd, &_report); - break; + _report.timestamp_position = hrt_absolute_time(); + _report.lat = (int32_t)47.378301e7f; + _report.lon = (int32_t)8.538777e7f; + _report.alt = (int32_t)400e3f; + _report.timestamp_variance = hrt_absolute_time(); + _report.s_variance_m_s = 10.0f; + _report.p_variance_m = 10.0f; + _report.c_variance_rad = 0.1f; + _report.fix_type = 3; + _report.eph_m = 10.0f; + _report.epv_m = 10.0f; + _report.timestamp_velocity = hrt_absolute_time(); + _report.vel_n_m_s = 0.0f; + _report.vel_e_m_s = 0.0f; + _report.vel_d_m_s = 0.0f; + _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 = 0.0f; + _report.vel_ned_valid = true; - case GPS_DRIVER_MODE_MTK: - _Helper = new MTK(_serial_fd, &_report); - break; + //no time and satellite information simulated - default: - break; - } + if (_report_pub > 0) { + orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); - unlock(); - - if (_Helper->configure(_baudrate) == 0) { - unlock(); - - // GPS is obviously detected successfully, reset statistics - _Helper->reset_update_rates(); - - while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) { -// lock(); - /* opportunistic publishing - else invalid data would end up on the bus */ - 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); - } - - last_rate_count++; - - /* measure update rate every 5 seconds */ - if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) { - _rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f); - last_rate_measurement = hrt_absolute_time(); - last_rate_count = 0; - _Helper->store_update_rates(); - _Helper->reset_update_rates(); - } - - if (!_healthy) { - char *mode_str = "unknown"; - - switch (_mode) { - case GPS_DRIVER_MODE_UBX: - mode_str = "UBX"; - break; - - case GPS_DRIVER_MODE_MTK: - mode_str = "MTK"; - break; - - default: - break; - } - - warnx("module found: %s", mode_str); - _healthy = true; - } + } else { + _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); } - if (_healthy) { - warnx("module lost"); - _healthy = false; - _rate = 0.0f; + usleep(2e5); + + } else { + + if (_Helper != nullptr) { + delete(_Helper); + /* set to zero to ensure parser is not used while not instantiated */ + _Helper = nullptr; + } + + switch (_mode) { + case GPS_DRIVER_MODE_UBX: + _Helper = new UBX(_serial_fd, &_report); + break; + + case GPS_DRIVER_MODE_MTK: + _Helper = new MTK(_serial_fd, &_report); + break; + + default: + break; + } + + unlock(); + + if (_Helper->configure(_baudrate) == 0) { + unlock(); + + // GPS is obviously detected successfully, reset statistics + _Helper->reset_update_rates(); + + while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) { + // lock(); + /* opportunistic publishing - else invalid data would end up on the bus */ + 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); + } + + last_rate_count++; + + /* measure update rate every 5 seconds */ + if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) { + _rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f); + last_rate_measurement = hrt_absolute_time(); + last_rate_count = 0; + _Helper->store_update_rates(); + _Helper->reset_update_rates(); + } + + if (!_healthy) { + char *mode_str = "unknown"; + + switch (_mode) { + case GPS_DRIVER_MODE_UBX: + mode_str = "UBX"; + break; + + case GPS_DRIVER_MODE_MTK: + mode_str = "MTK"; + break; + + default: + break; + } + + warnx("module found: %s", mode_str); + _healthy = true; + } + } + + if (_healthy) { + warnx("module lost"); + _healthy = false; + _rate = 0.0f; + } + + lock(); } lock(); - } - lock(); + /* select next mode */ + switch (_mode) { + case GPS_DRIVER_MODE_UBX: + _mode = GPS_DRIVER_MODE_MTK; + break; - /* select next mode */ - switch (_mode) { - case GPS_DRIVER_MODE_UBX: - _mode = GPS_DRIVER_MODE_MTK; - break; + case GPS_DRIVER_MODE_MTK: + _mode = GPS_DRIVER_MODE_UBX; + break; - case GPS_DRIVER_MODE_MTK: - _mode = GPS_DRIVER_MODE_UBX; - break; - - default: - break; + default: + break; + } } } @@ -417,7 +454,7 @@ namespace gps GPS *g_dev; -void start(const char *uart_path); +void start(const char *uart_path, bool fake_gps); void stop(); void test(); void reset(); @@ -427,7 +464,7 @@ void info(); * Start the driver. */ void -start(const char *uart_path) +start(const char *uart_path, bool fake_gps) { int fd; @@ -435,7 +472,7 @@ start(const char *uart_path) errx(1, "already started"); /* create the driver */ - g_dev = new GPS(uart_path); + g_dev = new GPS(uart_path, fake_gps); if (g_dev == nullptr) goto fail; @@ -527,6 +564,7 @@ gps_main(int argc, char *argv[]) /* set to default */ char *device_name = GPS_DEFAULT_UART_PORT; + bool fake_gps = false; /* * Start/load the driver. @@ -542,7 +580,13 @@ gps_main(int argc, char *argv[]) } } - gps::start(device_name); + /* Detect fake gps option */ + for (int i = 2; i < argc; i++) { + if (!strcmp(argv[i], "-f")) + fake_gps = true; + } + + gps::start(device_name, fake_gps); } if (!strcmp(argv[1], "stop")) @@ -567,5 +611,5 @@ gps_main(int argc, char *argv[]) gps::info(); out: - errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n]"); + errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n][-f]"); } diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index c1d73dd87f..0a047f38f1 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 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 @@ -193,9 +193,10 @@ HIL::~HIL() } while (_task != -1); } - /* clean up the alternate device node */ - if (_primary_pwm_device) - unregister_driver(PWM_OUTPUT_DEVICE_PATH); + // XXX already claimed with CDEV + // /* clean up the alternate device node */ + // if (_primary_pwm_device) + // unregister_driver(PWM_OUTPUT_DEVICE_PATH); g_hil = nullptr; } diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index d3b99ae66e..9b9c11af29 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -849,42 +849,24 @@ HMC5883::collect() /* scale values for output */ - /* - * 1) Scale raw value to SI units using scaling from datasheet. - * 2) Subtract static offset (in SI units) - * 3) Scale the statically calibrated values with a linear - * dynamically obtained factor - * - * Note: the static sensor offset is the number the sensor outputs - * at a nominally 'zero' input. Therefore the offset has to - * be subtracted. - * - * Example: A gyro outputs a value of 74 at zero angular rate - * the offset is 74 from the origin and subtracting - * 74 from all measurements centers them around zero. - */ - #ifdef PX4_I2C_BUS_ONBOARD if (_bus == PX4_I2C_BUS_ONBOARD) { - /* to align the sensor axes with the board, x and y need to be flipped */ - new_report.x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; - /* flip axes and negate value for y */ - new_report.y = ((-report.x * _range_scale) - _scale.y_offset) * _scale.y_scale; - /* z remains z */ - new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; - } else { -#endif - /* the standard external mag by 3DR has x pointing to the right, y pointing backwards, and z down, - * therefore switch x and y and invert y */ - new_report.x = ((-report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; - /* flip axes and negate value for y */ - new_report.y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale; - /* z remains z */ - new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; -#ifdef PX4_I2C_BUS_ONBOARD - } + // convert onboard so it matches offboard for the + // scaling below + report.y = -report.y; + report.x = -report.x; + } #endif + /* the standard external mag by 3DR has x pointing to the + * right, y pointing backwards, and z down, therefore switch x + * and y and invert y */ + new_report.x = ((-report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; + /* flip axes and negate value for y */ + new_report.y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale; + /* z remains z */ + new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; + if (_mag_topic != -1) { /* publish it */ orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report); @@ -910,6 +892,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) struct mag_report report; ssize_t sz; int ret = 1; + uint8_t good_count = 0; // XXX do something smarter here int fd = (int)enable; @@ -932,32 +915,17 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) 1.0f, }; - float avg_excited[3] = {0.0f, 0.0f, 0.0f}; - unsigned i; + float sum_excited[3] = {0.0f, 0.0f, 0.0f}; + + /* expected axis scaling. The datasheet says that 766 will + * be places in the X and Y axes and 713 in the Z + * axis. Experiments show that in fact 766 is placed in X, + * and 713 in Y and Z. This is relative to a base of 660 + * LSM/Ga, giving 1.16 and 1.08 */ + float expected_cal[3] = { 1.16f, 1.08f, 1.08f }; warnx("starting mag scale calibration"); - /* do a simple demand read */ - sz = read(filp, (char *)&report, sizeof(report)); - - if (sz != sizeof(report)) { - warn("immediate read failed"); - ret = 1; - goto out; - } - - warnx("current measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); - warnx("time: %lld", report.timestamp); - warnx("sampling 500 samples for scaling offset"); - - /* set the queue depth to 10 */ - /* don't do this for now, it can lead to a crash in start() respectively work_queue() */ -// if (OK != ioctl(filp, SENSORIOCSQUEUEDEPTH, 10)) { -// warn("failed to set queue depth"); -// ret = 1; -// goto out; -// } - /* start the sensor polling at 50 Hz */ if (OK != ioctl(filp, SENSORIOCSPOLLRATE, 50)) { warn("failed to set 2Hz poll rate"); @@ -965,8 +933,9 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) goto out; } - /* Set to 2.5 Gauss */ - if (OK != ioctl(filp, MAGIOCSRANGE, 2)) { + /* 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"); ret = 1; goto out; @@ -990,8 +959,8 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) goto out; } - /* read the sensor 10x and report each value */ - for (i = 0; i < 500; i++) { + // discard 10 samples to let the sensor settle + for (uint8_t i = 0; i < 10; i++) { struct pollfd fds; /* wait for data to be ready */ @@ -1009,32 +978,69 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) if (sz != sizeof(report)) { warn("periodic read failed"); + ret = -EIO; goto out; + } + } - } else { - avg_excited[0] += report.x; - avg_excited[1] += report.y; - avg_excited[2] += report.z; + /* read the sensor up to 50x, stopping when we have 10 good values */ + for (uint8_t i = 0; i < 50 && good_count < 10; i++) { + struct pollfd fds; + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + ret = ::poll(&fds, 1, 2000); + + if (ret != 1) { + warn("timed out waiting for sensor data"); + goto out; + } + + /* now go get it */ + sz = ::read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + warn("periodic read failed"); + ret = -EIO; + goto out; + } + float cal[3] = {fabsf(expected_cal[0] / report.x), + fabsf(expected_cal[1] / report.y), + fabsf(expected_cal[2] / report.z)}; + + if (cal[0] > 0.7f && cal[0] < 1.35f && + cal[1] > 0.7f && cal[1] < 1.35f && + cal[2] > 0.7f && cal[2] < 1.35f) { + good_count++; + sum_excited[0] += cal[0]; + 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]); } - avg_excited[0] /= i; - avg_excited[1] /= i; - avg_excited[2] /= i; + if (good_count < 5) { + warn("failed calibration"); + ret = -EIO; + goto out; + } - warnx("done. Performed %u reads", i); - warnx("measurement avg: %.6f %.6f %.6f", (double)avg_excited[0], (double)avg_excited[1], (double)avg_excited[2]); +#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]; - /* calculate axis scaling */ - scaling[0] = fabsf(1.16f / avg_excited[0]); - /* second axis inverted */ - scaling[1] = fabsf(1.16f / -avg_excited[1]); - scaling[2] = fabsf(1.08f / avg_excited[2]); + 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]); @@ -1165,6 +1171,8 @@ int HMC5883::set_excitement(unsigned enable) conf_reg &= ~0x03; } + // ::printf("set_excitement enable=%d regA=0x%x\n", (int)enable, (unsigned)conf_reg); + ret = write_reg(ADDR_CONF_A, conf_reg); if (OK != ret) @@ -1173,6 +1181,8 @@ int HMC5883::set_excitement(unsigned enable) uint8_t conf_reg_ret; read_reg(ADDR_CONF_A, conf_reg_ret); + //print_info(); + return !(conf_reg == conf_reg_ret); } @@ -1211,6 +1221,10 @@ HMC5883::print_info() perf_print_counter(_comms_errors); perf_print_counter(_buffer_overflows); printf("poll interval: %u ticks\n", _measure_ticks); + printf("offsets (%.2f %.2f %.2f)\n", (double)_scale.x_offset, (double)_scale.y_offset, (double)_scale.z_offset); + printf("scaling (%.2f %.2f %.2f) 1/range_scale %.2f range_ga %.2f\n", + (double)_scale.x_scale, (double)_scale.y_scale, (double)_scale.z_scale, + (double)1.0/_range_scale, (double)_range_ga); _reports->print_info("report queue"); } diff --git a/src/drivers/hott/hott_sensors/hott_sensors.cpp b/src/drivers/hott/hott_sensors/hott_sensors.cpp index e322c6349e..a3d3a39333 100644 --- a/src/drivers/hott/hott_sensors/hott_sensors.cpp +++ b/src/drivers/hott/hott_sensors/hott_sensors.cpp @@ -211,7 +211,7 @@ hott_sensors_main(int argc, char *argv[]) thread_should_exit = false; deamon_task = task_spawn_cmd(daemon_name, SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 40, + SCHED_PRIORITY_DEFAULT, 1024, hott_sensors_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); diff --git a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp index 042d9f8162..d293f9954e 100644 --- a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp +++ b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp @@ -237,7 +237,7 @@ hott_telemetry_main(int argc, char *argv[]) thread_should_exit = false; deamon_task = task_spawn_cmd(daemon_name, SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 40, + SCHED_PRIORITY_DEFAULT, 2048, hott_telemetry_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 3cd6d67203..9251cff7b2 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -77,7 +77,6 @@ #include #include #include -#include #include #include @@ -178,31 +177,26 @@ MEASAirspeed::collect() return ret; } - //uint16_t diff_pres_pa = (val[1]) | ((val[0] & ~(0xC0)) << 8); - uint16_t temp = (val[3] & 0xE0) << 8 | val[2]; - - // XXX leaving this in until new calculation method has been cross-checked - //diff_pres_pa = abs(diff_pres_pa - (16384 / 2.0f)); - //diff_pres_pa -= _diff_pres_offset; int16_t dp_raw = 0, dT_raw = 0; dp_raw = (val[0] << 8) + val[1]; - dp_raw = 0x3FFF & dp_raw; //mask the used bits + /* mask the used bits */ + dp_raw = 0x3FFF & dp_raw; dT_raw = (val[2] << 8) + val[3]; dT_raw = (0xFFE0 & dT_raw) >> 5; float temperature = ((200 * dT_raw) / 2047) - 50; - // XXX we may want to smooth out the readings to remove noise. - - // Calculate differential pressure. As its centered around 8000 - // and can go positive or negative, enforce absolute value -// uint16_t diff_press_pa = abs(dp_raw - (16384 / 2.0f)); + /* calculate differential pressure. As its centered around 8000 + * and can go positive or negative, enforce absolute value + */ const float P_min = -1.0f; const float P_max = 1.0f; - float diff_press_pa = math::max(0.0f, fabsf( ( ((float)dp_raw - 0.1f*16383.0f) * (P_max-P_min)/(0.8f*16383.0f) + P_min) * 6894.8f) - _diff_pres_offset); + float diff_press_pa = fabsf( ( ((float)dp_raw - 0.1f*16383.0f) * (P_max-P_min)/(0.8f*16383.0f) + P_min) * 6894.8f) - _diff_pres_offset; + if (diff_press_pa < 0.0f) + diff_press_pa = 0.0f; struct differential_pressure_s report; - // Track maximum differential pressure measured (so we can work out top speed). + /* 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; } @@ -390,7 +384,7 @@ test() err(1, "immediate read failed"); warnx("single read"); - warnx("diff pressure: %d pa", report.differential_pressure_pa); + warnx("diff pressure: %d pa", (double)report.differential_pressure_pa); /* start the sensor polling at 2Hz */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 87788824a9..6326cf7fcb 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -124,6 +124,8 @@ protected: int32_t _TEMP; int64_t _OFF; int64_t _SENS; + float _P; + float _T; /* altitude conversion calibration */ unsigned _msl_pressure; /* in kPa */ @@ -623,6 +625,8 @@ MS5611::collect() /* pressure calculation, result in Pa */ int32_t P = (((raw * _SENS) >> 21) - _OFF) >> 15; + _P = P * 0.01f; + _T = _TEMP * 0.01f; /* generate a new report */ report.temperature = _TEMP / 100.0f; @@ -695,6 +699,8 @@ MS5611::print_info() printf("TEMP: %d\n", _TEMP); printf("SENS: %lld\n", _SENS); printf("OFF: %lld\n", _OFF); + printf("P: %.3f\n", _P); + printf("T: %.3f\n", _T); printf("MSL pressure: %10.4f\n", (double)(_msl_pressure / 100.f)); printf("factory_setup %u\n", _prom.factory_setup); diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index b878d29bcc..c067d363b7 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -65,6 +65,7 @@ #include #include #include +#include #include #include @@ -224,10 +225,10 @@ PX4FMU::PX4FMU() : _armed(false), _pwm_on(false), _mixers(nullptr), - _failsafe_pwm( {0}), - _disarmed_pwm( {0}), - _num_failsafe_set(0), - _num_disarmed_set(0) + _failsafe_pwm({0}), + _disarmed_pwm({0}), + _num_failsafe_set(0), + _num_disarmed_set(0) { for (unsigned i = 0; i < _max_actuators; i++) { _min_pwm[i] = PWM_DEFAULT_MIN; @@ -575,7 +576,7 @@ PX4FMU::task_main() if (i >= outputs.noutputs || !isfinite(outputs.output[i]) || outputs.output[i] < -1.0f || - outputs.output[i] > 1.0f) { + outputs.output[i] > 1.0f) { /* * 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 @@ -933,7 +934,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; } - /* FALLTHROUGH */ + /* FALLTHROUGH */ case PWM_SERVO_SET(3): case PWM_SERVO_SET(2): if (_mode < MODE_4PWM) { @@ -941,7 +942,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; } - /* FALLTHROUGH */ + /* FALLTHROUGH */ case PWM_SERVO_SET(1): case PWM_SERVO_SET(0): if (arg <= 2100) { @@ -960,7 +961,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; } - /* FALLTHROUGH */ + /* FALLTHROUGH */ case PWM_SERVO_GET(3): case PWM_SERVO_GET(2): if (_mode < MODE_4PWM) { @@ -968,7 +969,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; } - /* FALLTHROUGH */ + /* FALLTHROUGH */ case PWM_SERVO_GET(1): case PWM_SERVO_GET(0): *(servo_position_t *)arg = up_pwm_servo_get(cmd - PWM_SERVO_GET(0)); @@ -1005,6 +1006,40 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; + case PWM_SERVO_SET_COUNT: { + /* change the number of outputs that are enabled for + * PWM. This is used to change the split between GPIO + * and PWM under control of the flight config + * parameters. Note that this does not allow for + * changing a set of pins to be used for serial on + * FMUv1 + */ + switch (arg) { + case 0: + set_mode(MODE_NONE); + break; + + case 2: + set_mode(MODE_2PWM); + break; + + case 4: + set_mode(MODE_4PWM); + break; + +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) + case 6: + set_mode(MODE_6PWM); + break; +#endif + + default: + ret = -EINVAL; + break; + } + break; + } + case MIXERIOCRESET: if (_mixers != nullptr) { delete _mixers; @@ -1107,10 +1142,12 @@ PX4FMU::sensor_reset(int ms) stm32_configgpio(GPIO_SPI_CS_GYRO_OFF); stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG_OFF); stm32_configgpio(GPIO_SPI_CS_BARO_OFF); + stm32_configgpio(GPIO_SPI_CS_MPU_OFF); stm32_gpiowrite(GPIO_SPI_CS_GYRO_OFF, 0); stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG_OFF, 0); stm32_gpiowrite(GPIO_SPI_CS_BARO_OFF, 0); + stm32_gpiowrite(GPIO_SPI_CS_MPU_OFF, 0); stm32_configgpio(GPIO_SPI1_SCK_OFF); stm32_configgpio(GPIO_SPI1_MISO_OFF); @@ -1123,10 +1160,12 @@ PX4FMU::sensor_reset(int ms) stm32_configgpio(GPIO_GYRO_DRDY_OFF); stm32_configgpio(GPIO_MAG_DRDY_OFF); stm32_configgpio(GPIO_ACCEL_DRDY_OFF); + stm32_configgpio(GPIO_EXTI_MPU_DRDY_OFF); stm32_gpiowrite(GPIO_GYRO_DRDY_OFF, 0); stm32_gpiowrite(GPIO_MAG_DRDY_OFF, 0); stm32_gpiowrite(GPIO_ACCEL_DRDY_OFF, 0); + stm32_gpiowrite(GPIO_EXTI_MPU_DRDY_OFF, 0); /* set the sensor rail off */ stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN); @@ -1159,6 +1198,13 @@ PX4FMU::sensor_reset(int ms) stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); + + // // XXX bring up the EXTI pins again + // stm32_configgpio(GPIO_GYRO_DRDY); + // stm32_configgpio(GPIO_MAG_DRDY); + // stm32_configgpio(GPIO_ACCEL_DRDY); + // stm32_configgpio(GPIO_EXTI_MPU_DRDY); + #endif #endif } @@ -1431,7 +1477,6 @@ void sensor_reset(int ms) { int fd; - int ret; fd = open(PX4FMU_DEVICE_PATH, O_RDWR); @@ -1591,6 +1636,15 @@ fmu_main(int argc, char *argv[]) errx(0, "FMU driver stopped"); } + if (!strcmp(verb, "id")) { + char id[12]; + (void)get_board_serial(id); + + errx(0, "Board serial:\n %02X%02X%02X%02X %02X%02X%02X%02X %02X%02X%02X%02X", + (unsigned)id[0], (unsigned)id[1], (unsigned)id[2], (unsigned)id[3], (unsigned)id[4], (unsigned)id[5], + (unsigned)id[6], (unsigned)id[7], (unsigned)id[8], (unsigned)id[9], (unsigned)id[10], (unsigned)id[11]); + } + if (fmu_start() != OK) errx(1, "failed to start the FMU driver"); @@ -1647,6 +1701,7 @@ fmu_main(int argc, char *argv[]) sensor_reset(0); warnx("resettet default time"); } + exit(0); } diff --git a/src/drivers/px4fmu/module.mk b/src/drivers/px4fmu/module.mk index d918abd572..05bc7a5b33 100644 --- a/src/drivers/px4fmu/module.mk +++ b/src/drivers/px4fmu/module.mk @@ -3,5 +3,4 @@ # MODULE_COMMAND = fmu -SRCS = fmu.cpp \ - ../../modules/systemlib/pwm_limit/pwm_limit.c +SRCS = fmu.cpp diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index cbdd5acc44..df847a64da 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -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 @@ -35,7 +35,7 @@ * @file px4io.cpp * Driver for the PX4IO board. * - * PX4IO is connected via I2C. + * PX4IO is connected via I2C or DMA enabled high-speed UART. */ #include @@ -270,7 +270,8 @@ private: orb_advert_t _to_servorail; ///< servorail status orb_advert_t _to_safety; ///< status of safety - actuator_outputs_s _outputs; /// 0) { - orb_publish(ORB_ID(servorail_status), _to_servorail, &servorail_status); + orb_publish(ORB_ID(servorail_status), _to_servorail, &_servorail_status); } else { - _to_servorail = orb_advertise(ORB_ID(servorail_status), &servorail_status); + _to_servorail = orb_advertise(ORB_ID(servorail_status), &_servorail_status); } } @@ -1450,6 +1454,13 @@ PX4IO::io_publish_raw_rc() rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN; } + /* set RSSI */ + + if (rc_val.input_source != RC_INPUT_SOURCE_PX4IO_SBUS) { + // XXX the correct scaling needs to be validated here + rc_val.rssi = (_servorail_status.rssi_v / 3.3f) * UINT8_MAX; + } + /* lazily advertise on first publication */ if (_to_input_rc == 0) { _to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_val); @@ -1664,7 +1675,18 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) total_len++; } - int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2); + int ret; + + for (int i = 0; i < 30; i++) { + /* failed, but give it a 2nd shot */ + ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2); + + if (ret) { + usleep(333); + } else { + break; + } + } /* print mixer chunk */ if (debuglevel > 5 || ret) { @@ -1688,7 +1710,21 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) msg->text[0] = '\n'; msg->text[1] = '\0'; - int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, (sizeof(px4io_mixdata) + 2) / 2); + int ret; + + for (int i = 0; i < 30; i++) { + /* failed, but give it a 2nd shot */ + ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, (sizeof(px4io_mixdata) + 2) / 2); + + if (ret) { + usleep(333); + } else { + break; + } + } + + if (ret) + return ret; retries--; @@ -1798,7 +1834,7 @@ PX4IO::print_status() printf("\n"); - if (raw_inputs > 0) { + if ((flags & PX4IO_P_STATUS_FLAGS_RC_PPM)) { int frame_len = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_RC_DATA); printf("RC data (PPM frame len) %u us\n", frame_len); @@ -2355,8 +2391,10 @@ start(int argc, char *argv[]) /* create the driver - it will set g_dev */ (void)new PX4IO(interface); - if (g_dev == nullptr) + if (g_dev == nullptr) { + delete interface; errx(1, "driver alloc failed"); + } if (OK != g_dev->init()) { delete g_dev; @@ -2419,6 +2457,69 @@ detect(int argc, char *argv[]) } } +void +checkcrc(int argc, char *argv[]) +{ + bool keep_running = false; + + if (g_dev == nullptr) { + /* allocate the interface */ + device::Device *interface = get_interface(); + + /* create the driver - it will set g_dev */ + (void)new PX4IO(interface); + + if (g_dev == nullptr) + errx(1, "driver alloc failed"); + } else { + /* its already running, don't kill the driver */ + keep_running = true; + } + + /* + check IO CRC against CRC of a file + */ + if (argc < 2) { + printf("usage: px4io checkcrc filename\n"); + exit(1); + } + int fd = open(argv[1], O_RDONLY); + if (fd == -1) { + printf("open of %s failed - %d\n", argv[1], errno); + exit(1); + } + const uint32_t app_size_max = 0xf000; + uint32_t fw_crc = 0; + uint32_t nbytes = 0; + while (true) { + uint8_t buf[16]; + int n = read(fd, buf, sizeof(buf)); + if (n <= 0) break; + fw_crc = crc32part(buf, n, fw_crc); + nbytes += n; + } + close(fd); + while (nbytes < app_size_max) { + uint8_t b = 0xff; + fw_crc = crc32part(&b, 1, fw_crc); + nbytes++; + } + + int ret = g_dev->ioctl(nullptr, PX4IO_CHECK_CRC, fw_crc); + + if (!keep_running) { + delete g_dev; + g_dev = nullptr; + } + + if (ret != OK) { + printf("check CRC failed - %d\n", ret); + exit(1); + } + printf("CRCs match\n"); + exit(0); +} + void bind(int argc, char *argv[]) { @@ -2569,17 +2670,17 @@ monitor(void) read(0, &c, 1); if (cancels-- == 0) { - printf("\033[H"); /* move cursor home and clear screen */ + printf("\033[2J\033[H"); /* move cursor home and clear screen */ exit(0); } } if (g_dev != nullptr) { - printf("\033[H"); /* move cursor home and clear screen */ + printf("\033[2J\033[H"); /* move cursor home and clear screen */ (void)g_dev->print_status(); (void)g_dev->print_debug(); - printf("[ Use 'px4io debug ' for more output. Hit three times to exit monitor mode ]\n"); + printf("\n\n\n[ Use 'px4io debug ' for more output. Hit three times to exit monitor mode ]\n"); } else { errx(1, "driver not loaded, exiting"); @@ -2613,12 +2714,16 @@ px4io_main(int argc, char *argv[]) if (!strcmp(argv[1], "detect")) detect(argc - 1, argv + 1); + if (!strcmp(argv[1], "checkcrc")) + checkcrc(argc - 1, argv + 1); + if (!strcmp(argv[1], "update")) { if (g_dev != nullptr) { printf("[px4io] loaded, detaching first\n"); /* stop the driver */ delete g_dev; + g_dev = nullptr; } PX4IO_Uploader *up; @@ -2691,18 +2796,30 @@ px4io_main(int argc, char *argv[]) } if (g_dev == nullptr) { warnx("px4io is not started, still attempting upgrade"); - } else { - uint16_t arg = atol(argv[2]); - int ret = g_dev->ioctl(nullptr, PX4IO_REBOOT_BOOTLOADER, arg); - if (ret != OK) { - printf("reboot failed - %d\n", ret); - exit(1); - } - // tear down the px4io instance - delete g_dev; + /* allocate the interface */ + device::Device *interface = get_interface(); + + /* create the driver - it will set g_dev */ + (void)new PX4IO(interface); + + if (g_dev == nullptr) { + delete interface; + errx(1, "driver alloc failed"); + } } + uint16_t arg = atol(argv[2]); + int ret = g_dev->ioctl(nullptr, PX4IO_REBOOT_BOOTLOADER, arg); + if (ret != OK) { + printf("reboot failed - %d\n", ret); + exit(1); + } + + // tear down the px4io instance + delete g_dev; + g_dev = nullptr; + // upload the specified firmware const char *fn[2]; fn[0] = argv[3]; @@ -2760,6 +2877,7 @@ px4io_main(int argc, char *argv[]) /* stop the driver */ delete g_dev; + g_dev = nullptr; exit(0); } @@ -2798,49 +2916,6 @@ px4io_main(int argc, char *argv[]) exit(0); } - if (!strcmp(argv[1], "checkcrc")) { - /* - check IO CRC against CRC of a file - */ - if (argc <= 2) { - printf("usage: px4io checkcrc filename\n"); - exit(1); - } - if (g_dev == nullptr) { - printf("px4io is not started\n"); - exit(1); - } - int fd = open(argv[2], O_RDONLY); - if (fd == -1) { - printf("open of %s failed - %d\n", argv[2], errno); - exit(1); - } - const uint32_t app_size_max = 0xf000; - uint32_t fw_crc = 0; - uint32_t nbytes = 0; - while (true) { - uint8_t buf[16]; - int n = read(fd, buf, sizeof(buf)); - if (n <= 0) break; - fw_crc = crc32part(buf, n, fw_crc); - nbytes += n; - } - close(fd); - while (nbytes < app_size_max) { - uint8_t b = 0xff; - fw_crc = crc32part(&b, 1, fw_crc); - nbytes++; - } - - int ret = g_dev->ioctl(nullptr, PX4IO_CHECK_CRC, fw_crc); - if (ret != OK) { - printf("check CRC failed - %d\n", ret); - exit(1); - } - printf("CRCs match\n"); - exit(0); - } - if (!strcmp(argv[1], "rx_dsm") || !strcmp(argv[1], "rx_dsm_10bit") || !strcmp(argv[1], "rx_dsm_11bit") || diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index 41f93a8ee1..dd8abbac5a 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -51,6 +51,7 @@ #include #include #include +#include #include @@ -120,8 +121,15 @@ PX4IO_Uploader::upload(const char *filenames[]) cfsetspeed(&t, 115200); tcsetattr(_io_fd, TCSANOW, &t); - /* look for the bootloader */ - ret = sync(); + /* look for the bootloader for 150 ms */ + for (int i = 0; i < 15; i++) { + ret = sync(); + if (ret == OK) { + break; + } else { + usleep(10000); + } + } if (ret != OK) { /* this is immediately fatal */ @@ -226,6 +234,11 @@ PX4IO_Uploader::upload(const char *filenames[]) close(_fw_fd); close(_io_fd); _io_fd = -1; + + // sleep for enough time for the IO chip to boot. This makes + // forceupdate more reliably startup IO again after update + up_udelay(100*1000); + return ret; } diff --git a/src/drivers/px4io/uploader.h b/src/drivers/px4io/uploader.h index 22387a3e23..55f63eef96 100644 --- a/src/drivers/px4io/uploader.h +++ b/src/drivers/px4io/uploader.h @@ -91,7 +91,7 @@ private: void drain(); int send(uint8_t c); int send(uint8_t *p, unsigned count); - int get_sync(unsigned timeout = 1000); + int get_sync(unsigned timeout = 40); int sync(); int get_info(int param, uint32_t &val); int erase(); diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index 727c86e02a..4f58891ed3 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -559,7 +559,7 @@ RGBLED::get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b) void rgbled_usage() { - warnx("missing command: try 'start', 'test', 'info', 'off', 'rgb 30 40 50'"); + warnx("missing command: try 'start', 'test', 'info', 'off', 'stop', 'rgb 30 40 50'"); warnx("options:"); warnx(" -b i2cbus (%d)", PX4_I2C_BUS_LED); warnx(" -a addr (0x%x)", ADDR); @@ -643,7 +643,7 @@ rgbled_main(int argc, char *argv[]) if (g_rgbled == nullptr) { warnx("not started"); rgbled_usage(); - exit(0); + exit(1); } if (!strcmp(verb, "test")) { @@ -669,7 +669,7 @@ rgbled_main(int argc, char *argv[]) exit(0); } - if (!strcmp(verb, "off")) { + if (!strcmp(verb, "off") || !strcmp(verb, "stop")) { fd = open(RGBLED_DEVICE_PATH, 0); if (fd == -1) { @@ -681,6 +681,12 @@ rgbled_main(int argc, char *argv[]) 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 "); diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c index b286e00077..067d773649 100644 --- a/src/examples/fixedwing_control/main.c +++ b/src/examples/fixedwing_control/main.c @@ -181,11 +181,7 @@ void control_heading(const struct vehicle_global_position_s *pos, const struct v * Calculate heading error of current position to desired position */ - /* - * PX4 uses 1e7 scaled integers to represent global coordinates for max resolution, - * so they need to be scaled by 1e7 and converted to IEEE double precision floating point. - */ - float bearing = get_bearing_to_next_waypoint(pos->lat/1e7d, pos->lon/1e7d, sp->lat/1e7d, sp->lon/1e7d); + float bearing = get_bearing_to_next_waypoint(pos->lat, pos->lon, sp->lat, sp->lon); /* calculate heading error */ float yaw_err = att->yaw - bearing; diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp index b078562c2d..614877b183 100644 --- a/src/lib/conversion/rotation.cpp +++ b/src/lib/conversion/rotation.cpp @@ -41,22 +41,11 @@ #include "rotation.h" __EXPORT void -get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix) +get_rot_matrix(enum Rotation rot, math::Matrix<3,3> *rot_matrix) { - /* first set to zero */ - rot_matrix->Matrix::zero(3, 3); - float roll = M_DEG_TO_RAD_F * (float)rot_lookup[rot].roll; float pitch = M_DEG_TO_RAD_F * (float)rot_lookup[rot].pitch; float yaw = M_DEG_TO_RAD_F * (float)rot_lookup[rot].yaw; - math::EulerAngles euler(roll, pitch, yaw); - - math::Dcm R(euler); - - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; j++) { - (*rot_matrix)(i, j) = R(i, j); - } - } + rot_matrix->from_euler(roll, pitch, yaw); } diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h index 85c63c0fcb..0c56494c5e 100644 --- a/src/lib/conversion/rotation.h +++ b/src/lib/conversion/rotation.h @@ -116,6 +116,6 @@ const rot_lookup_t rot_lookup[] = { * Get the rotation matrix */ __EXPORT void -get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix); +get_rot_matrix(enum Rotation rot, math::Matrix<3,3> *rot_matrix); #endif /* ROTATION_H_ */ diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index b66d1dba53..9584924ccc 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -174,7 +174,7 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max); /* Apply PI rate controller and store non-limited output */ - _last_output = (_bodyrate_setpoint * _k_ff +_rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed + _last_output = (_bodyrate_setpoint * _k_ff +_rate_error * _k_p + integrator_constrained) * scaler * scaler; //scaler is proportional to 1/airspeed // warnx("pitch: _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p); // warnx("roll: _last_output %.4f", (double)_last_output); return math::constrain(_last_output, -1.0f, 1.0f); diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 9e7d35f680..2e86c72dc1 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -141,7 +141,7 @@ float ECL_RollController::control_bodyrate(float pitch, //warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max); /* Apply PI rate controller and store non-limited output */ - _last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed + _last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * scaler * scaler; //scaler is proportional to 1/airspeed return math::constrain(_last_output, -1.0f, 1.0f); } diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 5e22007277..255776765b 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -158,7 +158,7 @@ float ECL_YawController::control_bodyrate(float roll, float pitch, float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max); /* Apply PI rate controller and store non-limited output */ - _last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained + _rate_setpoint * k_ff) * scaler * scaler; //scaler is proportional to 1/airspeed + _last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * scaler * scaler; //scaler is proportional to 1/airspeed //warnx("yaw:_last_output: %.4f, _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_last_output, (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p); diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp index 11def23710..3b68a0a4e8 100644 --- a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp +++ b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp @@ -83,8 +83,8 @@ float ECL_L1_Pos_Controller::crosstrack_error(void) return _crosstrack_error; } -void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, const math::Vector2f &vector_B, const math::Vector2f &vector_curr_position, - const math::Vector2f &ground_speed_vector) +void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector<2> &vector_A, const math::Vector<2> &vector_B, const math::Vector<2> &vector_curr_position, + const math::Vector<2> &ground_speed_vector) { /* this follows the logic presented in [1] */ @@ -94,7 +94,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c float ltrack_vel; /* get the direction between the last (visited) and next waypoint */ - _target_bearing = get_bearing_to_next_waypoint(vector_curr_position.getX(), vector_curr_position.getY(), vector_B.getX(), vector_B.getY()); + _target_bearing = get_bearing_to_next_waypoint(vector_curr_position(0), vector_curr_position(1), vector_B(0), vector_B(1)); /* enforce a minimum ground speed of 0.1 m/s to avoid singularities */ float ground_speed = math::max(ground_speed_vector.length(), 0.1f); @@ -103,7 +103,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c _L1_distance = _L1_ratio * ground_speed; /* calculate vector from A to B */ - math::Vector2f vector_AB = get_local_planar_vector(vector_A, vector_B); + math::Vector<2> vector_AB = get_local_planar_vector(vector_A, vector_B); /* * check if waypoints are on top of each other. If yes, @@ -116,7 +116,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c vector_AB.normalize(); /* calculate the vector from waypoint A to the aircraft */ - math::Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); + math::Vector<2> vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); /* calculate crosstrack error (output only) */ _crosstrack_error = vector_AB % vector_A_to_airplane; @@ -130,7 +130,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c float alongTrackDist = vector_A_to_airplane * vector_AB; /* estimate airplane position WRT to B */ - math::Vector2f vector_B_to_P_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized(); + math::Vector<2> vector_B_to_P_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized(); /* calculate angle of airplane position vector relative to line) */ @@ -143,14 +143,14 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c /* calculate eta to fly to waypoint A */ /* unit vector from waypoint A to current position */ - math::Vector2f vector_A_to_airplane_unit = vector_A_to_airplane.normalized(); + math::Vector<2> vector_A_to_airplane_unit = vector_A_to_airplane.normalized(); /* velocity across / orthogonal to line */ xtrack_vel = ground_speed_vector % (-vector_A_to_airplane_unit); /* velocity along line */ ltrack_vel = ground_speed_vector * (-vector_A_to_airplane_unit); eta = atan2f(xtrack_vel, ltrack_vel); /* bearing from current position to L1 point */ - _nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX()); + _nav_bearing = atan2f(-vector_A_to_airplane_unit(1) , -vector_A_to_airplane_unit(0)); /* * If the AB vector and the vector from B to airplane point in the same @@ -174,7 +174,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c ltrack_vel = ground_speed_vector * (-vector_B_to_P_unit); eta = atan2f(xtrack_vel, ltrack_vel); /* bearing from current position to L1 point */ - _nav_bearing = atan2f(-vector_B_to_P_unit.getY() , -vector_B_to_P_unit.getX()); + _nav_bearing = atan2f(-vector_B_to_P_unit(1) , -vector_B_to_P_unit(0)); } else { @@ -194,7 +194,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c float eta1 = asinf(sine_eta1); eta = eta1 + eta2; /* bearing from current position to L1 point */ - _nav_bearing = atan2f(vector_AB.getY(), vector_AB.getX()) + eta1; + _nav_bearing = atan2f(vector_AB(1), vector_AB(0)) + eta1; } @@ -209,8 +209,8 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c _bearing_error = eta; } -void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, const math::Vector2f &vector_curr_position, float radius, int8_t loiter_direction, - const math::Vector2f &ground_speed_vector) +void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector<2> &vector_A, const math::Vector<2> &vector_curr_position, float radius, int8_t loiter_direction, + const math::Vector<2> &ground_speed_vector) { /* the complete guidance logic in this section was proposed by [2] */ @@ -220,7 +220,7 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, cons float K_velocity = 2.0f * _L1_damping * omega; /* update bearing to next waypoint */ - _target_bearing = get_bearing_to_next_waypoint(vector_curr_position.getX(), vector_curr_position.getY(), vector_A.getX(), vector_A.getY()); + _target_bearing = get_bearing_to_next_waypoint(vector_curr_position(0), vector_curr_position(1), vector_A(0), vector_A(1)); /* ground speed, enforce minimum of 0.1 m/s to avoid singularities */ float ground_speed = math::max(ground_speed_vector.length() , 0.1f); @@ -229,10 +229,10 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, cons _L1_distance = _L1_ratio * ground_speed; /* calculate the vector from waypoint A to current position */ - math::Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); + math::Vector<2> vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); /* store the normalized vector from waypoint A to current position */ - math::Vector2f vector_A_to_airplane_unit = (vector_A_to_airplane).normalized(); + math::Vector<2> vector_A_to_airplane_unit = (vector_A_to_airplane).normalized(); /* calculate eta angle towards the loiter center */ @@ -287,19 +287,19 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, cons /* angle between requested and current velocity vector */ _bearing_error = eta; /* bearing from current position to L1 point */ - _nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX()); + _nav_bearing = atan2f(-vector_A_to_airplane_unit(1) , -vector_A_to_airplane_unit(0)); } else { _lateral_accel = lateral_accel_sp_circle; _circle_mode = true; _bearing_error = 0.0f; /* bearing from current position to L1 point */ - _nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX()); + _nav_bearing = atan2f(-vector_A_to_airplane_unit(1) , -vector_A_to_airplane_unit(0)); } } -void ECL_L1_Pos_Controller::navigate_heading(float navigation_heading, float current_heading, const math::Vector2f &ground_speed_vector) +void ECL_L1_Pos_Controller::navigate_heading(float navigation_heading, float current_heading, const math::Vector<2> &ground_speed_vector) { /* the complete guidance logic in this section was proposed by [2] */ @@ -352,14 +352,11 @@ void ECL_L1_Pos_Controller::navigate_level_flight(float current_heading) } -math::Vector2f ECL_L1_Pos_Controller::get_local_planar_vector(const math::Vector2f &origin, const math::Vector2f &target) const +math::Vector<2> ECL_L1_Pos_Controller::get_local_planar_vector(const math::Vector<2> &origin, const math::Vector<2> &target) const { /* this is an approximation for small angles, proposed by [2] */ - math::Vector2f out; - - out.setX(math::radians((target.getX() - origin.getX()))); - out.setY(math::radians((target.getY() - origin.getY())*cosf(math::radians(origin.getX())))); + math::Vector<2> out(math::radians((target(0) - origin(0))), math::radians((target(1) - origin(1))*cosf(math::radians(origin(0))))); return out * static_cast(CONSTANTS_RADIUS_OF_EARTH); } diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.h b/src/lib/ecl/l1/ecl_l1_pos_controller.h index 7a3c42a925..5c0804a392 100644 --- a/src/lib/ecl/l1/ecl_l1_pos_controller.h +++ b/src/lib/ecl/l1/ecl_l1_pos_controller.h @@ -160,8 +160,8 @@ public: * * @return sets _lateral_accel setpoint */ - void navigate_waypoints(const math::Vector2f &vector_A, const math::Vector2f &vector_B, const math::Vector2f &vector_curr_position, - const math::Vector2f &ground_speed); + void navigate_waypoints(const math::Vector<2> &vector_A, const math::Vector<2> &vector_B, const math::Vector<2> &vector_curr_position, + const math::Vector<2> &ground_speed); /** @@ -172,8 +172,8 @@ public: * * @return sets _lateral_accel setpoint */ - void navigate_loiter(const math::Vector2f &vector_A, const math::Vector2f &vector_curr_position, float radius, int8_t loiter_direction, - const math::Vector2f &ground_speed_vector); + void navigate_loiter(const math::Vector<2> &vector_A, const math::Vector<2> &vector_curr_position, float radius, int8_t loiter_direction, + const math::Vector<2> &ground_speed_vector); /** @@ -185,7 +185,7 @@ public: * * @return sets _lateral_accel setpoint */ - void navigate_heading(float navigation_heading, float current_heading, const math::Vector2f &ground_speed); + void navigate_heading(float navigation_heading, float current_heading, const math::Vector<2> &ground_speed); /** @@ -260,7 +260,7 @@ private: * @param wp The point to convert to into the local coordinates, in WGS84 coordinates * @return The vector in meters pointing from the reference position to the coordinates */ - math::Vector2f get_local_planar_vector(const math::Vector2f &origin, const math::Vector2f &target) const; + math::Vector<2> get_local_planar_vector(const math::Vector<2> &origin, const math::Vector<2> &target) const; }; diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 5a56dce65e..0f28bccadd 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -30,7 +30,7 @@ using namespace math; * */ -void TECS::update_50hz(float baro_altitude, float airspeed, const math::Dcm &rotMat, const math::Vector3 &accel_body, const math::Vector3 &accel_earth) +void TECS::update_50hz(float baro_altitude, float airspeed, const math::Matrix<3,3> &rotMat, const math::Vector<3> &accel_body, const math::Vector<3> &accel_earth) { // Implement third order complementary filter for height and height rate // estimted height rate = _integ2_state @@ -282,7 +282,7 @@ void TECS::_update_energies(void) _SKEdot = _integ5_state * _vel_dot; } -void TECS::_update_throttle(float throttle_cruise, const math::Dcm &rotMat) +void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotMat) { // Calculate total energy values _STE_error = _SPE_dem - _SPE_est + _SKE_dem - _SKE_est; @@ -505,7 +505,7 @@ void TECS::_update_STE_rate_lim(void) _STEdot_min = - _minSinkRate * CONSTANTS_ONE_G; } -void TECS::update_pitch_throttle(const math::Dcm &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO, +void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO, float throttle_min, float throttle_max, float throttle_cruise, float pitch_limit_min, float pitch_limit_max) { diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index 4fc009da9e..d1ebacda16 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -71,10 +71,10 @@ public: // Update of the estimated height and height rate internal state // Update of the inertial speed rate internal state // Should be called at 50Hz or greater - void update_50hz(float baro_altitude, float airspeed, const math::Dcm &rotMat, const math::Vector3 &accel_body, const math::Vector3 &accel_earth); + void update_50hz(float baro_altitude, float airspeed, const math::Matrix<3,3> &rotMat, const math::Vector<3> &accel_body, const math::Vector<3> &accel_earth); // Update the control loop calculations - void update_pitch_throttle(const math::Dcm &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO, + void update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO, float throttle_min, float throttle_max, float throttle_cruise, float pitch_limit_min, float pitch_limit_max); // demanded throttle in percentage @@ -348,7 +348,7 @@ private: void _update_energies(void); // Update Demanded Throttle - void _update_throttle(float throttle_cruise, const math::Dcm &rotMat); + void _update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotMat); // Detect Bad Descent void _detect_bad_descent(void); diff --git a/src/lib/mathlib/math/Dcm.cpp b/src/lib/mathlib/math/Dcm.cpp deleted file mode 100644 index f509f7081b..0000000000 --- a/src/lib/mathlib/math/Dcm.cpp +++ /dev/null @@ -1,174 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Dcm.cpp - * - * math direction cosine matrix - */ - -#include - -#include "Dcm.hpp" -#include "Quaternion.hpp" -#include "EulerAngles.hpp" -#include "Vector3.hpp" - -namespace math -{ - -Dcm::Dcm() : - Matrix(Matrix::identity(3)) -{ -} - -Dcm::Dcm(float c00, float c01, float c02, - float c10, float c11, float c12, - float c20, float c21, float c22) : - Matrix(3, 3) -{ - Dcm &dcm = *this; - dcm(0, 0) = c00; - dcm(0, 1) = c01; - dcm(0, 2) = c02; - dcm(1, 0) = c10; - dcm(1, 1) = c11; - dcm(1, 2) = c12; - dcm(2, 0) = c20; - dcm(2, 1) = c21; - dcm(2, 2) = c22; -} - -Dcm::Dcm(const float data[3][3]) : - Matrix(3, 3) -{ - Dcm &dcm = *this; - /* set rotation matrix */ - for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) - dcm(i, j) = data[i][j]; -} - -Dcm::Dcm(const float *data) : - Matrix(3, 3, data) -{ -} - -Dcm::Dcm(const Quaternion &q) : - Matrix(3, 3) -{ - Dcm &dcm = *this; - double a = q.getA(); - double b = q.getB(); - double c = q.getC(); - double d = q.getD(); - double aSq = a * a; - double bSq = b * b; - double cSq = c * c; - double dSq = d * d; - dcm(0, 0) = aSq + bSq - cSq - dSq; - dcm(0, 1) = 2.0 * (b * c - a * d); - dcm(0, 2) = 2.0 * (a * c + b * d); - dcm(1, 0) = 2.0 * (b * c + a * d); - dcm(1, 1) = aSq - bSq + cSq - dSq; - dcm(1, 2) = 2.0 * (c * d - a * b); - dcm(2, 0) = 2.0 * (b * d - a * c); - dcm(2, 1) = 2.0 * (a * b + c * d); - dcm(2, 2) = aSq - bSq - cSq + dSq; -} - -Dcm::Dcm(const EulerAngles &euler) : - Matrix(3, 3) -{ - Dcm &dcm = *this; - double cosPhi = cos(euler.getPhi()); - double sinPhi = sin(euler.getPhi()); - double cosThe = cos(euler.getTheta()); - double sinThe = sin(euler.getTheta()); - double cosPsi = cos(euler.getPsi()); - double sinPsi = sin(euler.getPsi()); - - dcm(0, 0) = cosThe * cosPsi; - dcm(0, 1) = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi; - dcm(0, 2) = sinPhi * sinPsi + cosPhi * sinThe * cosPsi; - - dcm(1, 0) = cosThe * sinPsi; - dcm(1, 1) = cosPhi * cosPsi + sinPhi * sinThe * sinPsi; - dcm(1, 2) = -sinPhi * cosPsi + cosPhi * sinThe * sinPsi; - - dcm(2, 0) = -sinThe; - dcm(2, 1) = sinPhi * cosThe; - dcm(2, 2) = cosPhi * cosThe; -} - -Dcm::Dcm(const Dcm &right) : - Matrix(right) -{ -} - -Dcm::~Dcm() -{ -} - -int __EXPORT dcmTest() -{ - printf("Test DCM\t\t: "); - // default ctor - ASSERT(matrixEqual(Dcm(), - Matrix::identity(3))); - // quaternion ctor - ASSERT(matrixEqual( - Dcm(Quaternion(0.983347f, 0.034271f, 0.106021f, 0.143572f)), - Dcm(0.9362934f, -0.2750958f, 0.2183507f, - 0.2896295f, 0.9564251f, -0.0369570f, - -0.1986693f, 0.0978434f, 0.9751703f))); - // euler angle ctor - ASSERT(matrixEqual( - Dcm(EulerAngles(0.1f, 0.2f, 0.3f)), - Dcm(0.9362934f, -0.2750958f, 0.2183507f, - 0.2896295f, 0.9564251f, -0.0369570f, - -0.1986693f, 0.0978434f, 0.9751703f))); - // rotations - Vector3 vB(1, 2, 3); - ASSERT(vectorEqual(Vector3(-2.0f, 1.0f, 3.0f), - Dcm(EulerAngles(0.0f, 0.0f, M_PI_2_F))*vB)); - ASSERT(vectorEqual(Vector3(3.0f, 2.0f, -1.0f), - Dcm(EulerAngles(0.0f, M_PI_2_F, 0.0f))*vB)); - ASSERT(vectorEqual(Vector3(1.0f, -3.0f, 2.0f), - Dcm(EulerAngles(M_PI_2_F, 0.0f, 0.0f))*vB)); - ASSERT(vectorEqual(Vector3(3.0f, 2.0f, -1.0f), - Dcm(EulerAngles( - M_PI_2_F, M_PI_2_F, M_PI_2_F))*vB)); - printf("PASS\n"); - return 0; -} -} // namespace math diff --git a/src/lib/mathlib/math/Dcm.hpp b/src/lib/mathlib/math/Dcm.hpp deleted file mode 100644 index df8970d3aa..0000000000 --- a/src/lib/mathlib/math/Dcm.hpp +++ /dev/null @@ -1,108 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Dcm.hpp - * - * math direction cosine matrix - */ - -//#pragma once - -#include "Vector.hpp" -#include "Matrix.hpp" - -namespace math -{ - -class Quaternion; -class EulerAngles; - -/** - * This is a Tait Bryan, Body 3-2-1 sequence. - * (yaw)-(pitch)-(roll) - * The Dcm transforms a vector in the body frame - * to the navigation frame, typically represented - * as C_nb. C_bn can be obtained through use - * of the transpose() method. - */ -class __EXPORT Dcm : public Matrix -{ -public: - /** - * default ctor - */ - Dcm(); - - /** - * scalar ctor - */ - Dcm(float c00, float c01, float c02, - float c10, float c11, float c12, - float c20, float c21, float c22); - - /** - * data ctor - */ - Dcm(const float *data); - - /** - * array ctor - */ - Dcm(const float data[3][3]); - - /** - * quaternion ctor - */ - Dcm(const Quaternion &q); - - /** - * euler angles ctor - */ - Dcm(const EulerAngles &euler); - - /** - * copy ctor (deep) - */ - Dcm(const Dcm &right); - - /** - * dtor - */ - virtual ~Dcm(); -}; - -int __EXPORT dcmTest(); - -} // math - diff --git a/src/lib/mathlib/math/EulerAngles.cpp b/src/lib/mathlib/math/EulerAngles.cpp deleted file mode 100644 index e733d23bb0..0000000000 --- a/src/lib/mathlib/math/EulerAngles.cpp +++ /dev/null @@ -1,126 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Vector.cpp - * - * math vector - */ - -#include "test/test.hpp" - -#include "EulerAngles.hpp" -#include "Quaternion.hpp" -#include "Dcm.hpp" -#include "Vector3.hpp" - -namespace math -{ - -EulerAngles::EulerAngles() : - Vector(3) -{ - setPhi(0.0f); - setTheta(0.0f); - setPsi(0.0f); -} - -EulerAngles::EulerAngles(float phi, float theta, float psi) : - Vector(3) -{ - setPhi(phi); - setTheta(theta); - setPsi(psi); -} - -EulerAngles::EulerAngles(const Quaternion &q) : - Vector(3) -{ - (*this) = EulerAngles(Dcm(q)); -} - -EulerAngles::EulerAngles(const Dcm &dcm) : - Vector(3) -{ - setTheta(asinf(-dcm(2, 0))); - - if (fabsf(getTheta() - M_PI_2_F) < 1.0e-3f) { - setPhi(0.0f); - setPsi(atan2f(dcm(1, 2) - dcm(0, 1), - dcm(0, 2) + dcm(1, 1)) + getPhi()); - - } else if (fabsf(getTheta() + M_PI_2_F) < 1.0e-3f) { - setPhi(0.0f); - setPsi(atan2f(dcm(1, 2) - dcm(0, 1), - dcm(0, 2) + dcm(1, 1)) - getPhi()); - - } else { - setPhi(atan2f(dcm(2, 1), dcm(2, 2))); - setPsi(atan2f(dcm(1, 0), dcm(0, 0))); - } -} - -EulerAngles::~EulerAngles() -{ -} - -int __EXPORT eulerAnglesTest() -{ - printf("Test EulerAngles\t: "); - EulerAngles euler(0.1f, 0.2f, 0.3f); - - // test ctor - ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler)); - ASSERT(equal(euler.getPhi(), 0.1f)); - ASSERT(equal(euler.getTheta(), 0.2f)); - ASSERT(equal(euler.getPsi(), 0.3f)); - - // test dcm ctor - euler = Dcm(EulerAngles(0.1f, 0.2f, 0.3f)); - ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler)); - - // test quat ctor - euler = Quaternion(EulerAngles(0.1f, 0.2f, 0.3f)); - ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler)); - - // test assignment - euler.setPhi(0.4f); - euler.setTheta(0.5f); - euler.setPsi(0.6f); - ASSERT(vectorEqual(Vector3(0.4f, 0.5f, 0.6f), euler)); - - printf("PASS\n"); - return 0; -} - -} // namespace math diff --git a/src/lib/mathlib/math/Matrix.cpp b/src/lib/mathlib/math/Matrix.cpp deleted file mode 100644 index ebd1aeda3d..0000000000 --- a/src/lib/mathlib/math/Matrix.cpp +++ /dev/null @@ -1,193 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Matrix.cpp - * - * matrix code - */ - -#include "test/test.hpp" -#include - -#include "Matrix.hpp" - -namespace math -{ - -static const float data_testA[] = { - 1, 2, 3, - 4, 5, 6 -}; -static Matrix testA(2, 3, data_testA); - -static const float data_testB[] = { - 0, 1, 3, - 7, -1, 2 -}; -static Matrix testB(2, 3, data_testB); - -static const float data_testC[] = { - 0, 1, - 2, 1, - 3, 2 -}; -static Matrix testC(3, 2, data_testC); - -static const float data_testD[] = { - 0, 1, 2, - 2, 1, 4, - 5, 2, 0 -}; -static Matrix testD(3, 3, data_testD); - -static const float data_testE[] = { - 1, -1, 2, - 0, 2, 3, - 2, -1, 1 -}; -static Matrix testE(3, 3, data_testE); - -static const float data_testF[] = { - 3.777e006f, 2.915e007f, 0.000e000f, - 2.938e007f, 2.267e008f, 0.000e000f, - 0.000e000f, 0.000e000f, 6.033e008f -}; -static Matrix testF(3, 3, data_testF); - -int __EXPORT matrixTest() -{ - matrixAddTest(); - matrixSubTest(); - matrixMultTest(); - matrixInvTest(); - matrixDivTest(); - return 0; -} - -int matrixAddTest() -{ - printf("Test Matrix Add\t\t: "); - Matrix r = testA + testB; - float data_test[] = { - 1.0f, 3.0f, 6.0f, - 11.0f, 4.0f, 8.0f - }; - ASSERT(matrixEqual(Matrix(2, 3, data_test), r)); - printf("PASS\n"); - return 0; -} - -int matrixSubTest() -{ - printf("Test Matrix Sub\t\t: "); - Matrix r = testA - testB; - float data_test[] = { - 1.0f, 1.0f, 0.0f, - -3.0f, 6.0f, 4.0f - }; - ASSERT(matrixEqual(Matrix(2, 3, data_test), r)); - printf("PASS\n"); - return 0; -} - -int matrixMultTest() -{ - printf("Test Matrix Mult\t: "); - Matrix r = testC * testB; - float data_test[] = { - 7.0f, -1.0f, 2.0f, - 7.0f, 1.0f, 8.0f, - 14.0f, 1.0f, 13.0f - }; - ASSERT(matrixEqual(Matrix(3, 3, data_test), r)); - printf("PASS\n"); - return 0; -} - -int matrixInvTest() -{ - printf("Test Matrix Inv\t\t: "); - Matrix origF = testF; - Matrix r = testF.inverse(); - float data_test[] = { - -0.0012518f, 0.0001610f, 0.0000000f, - 0.0001622f, -0.0000209f, 0.0000000f, - 0.0000000f, 0.0000000f, 1.6580e-9f - }; - ASSERT(matrixEqual(Matrix(3, 3, data_test), r)); - // make sure F in unchanged - ASSERT(matrixEqual(origF, testF)); - printf("PASS\n"); - return 0; -} - -int matrixDivTest() -{ - printf("Test Matrix Div\t\t: "); - Matrix r = testD / testE; - float data_test[] = { - 0.2222222f, 0.5555556f, -0.1111111f, - 0.0f, 1.0f, 1.0, - -4.1111111f, 1.2222222f, 4.5555556f - }; - ASSERT(matrixEqual(Matrix(3, 3, data_test), r)); - printf("PASS\n"); - return 0; -} - -bool matrixEqual(const Matrix &a, const Matrix &b, float eps) -{ - if (a.getRows() != b.getRows()) { - printf("row number not equal a: %d, b:%d\n", a.getRows(), b.getRows()); - return false; - - } else if (a.getCols() != b.getCols()) { - printf("column number not equal a: %d, b:%d\n", a.getCols(), b.getCols()); - return false; - } - - bool ret = true; - - for (size_t i = 0; i < a.getRows(); i++) - for (size_t j = 0; j < a.getCols(); j++) { - if (!equal(a(i, j), b(i, j), eps)) { - printf("element mismatch (%d, %d)\n", i, j); - ret = false; - } - } - - return ret; -} - -} // namespace math diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index f19db15eca..ea0cf4ca1e 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -1,6 +1,9 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin + * Pavel Kirienko + * Lorenz Meier * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,30 +35,401 @@ ****************************************************************************/ /** - * @file Matrix.h + * @file Matrix.hpp * - * matrix code + * Matrix class */ -#pragma once +#ifndef MATRIX_HPP +#define MATRIX_HPP -#include - -#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU) -#include "arm/Matrix.hpp" -#else -#include "generic/Matrix.hpp" -#endif +#include +#include "../CMSIS/Include/arm_math.h" namespace math { -class Matrix; -int matrixTest(); -int matrixAddTest(); -int matrixSubTest(); -int matrixMultTest(); -int matrixInvTest(); -int matrixDivTest(); -int matrixArmTest(); -bool matrixEqual(const Matrix &a, const Matrix &b, float eps = 1.0e-5f); -} // namespace math + +template +class __EXPORT Matrix; + +// MxN matrix with float elements +template +class __EXPORT MatrixBase +{ +public: + /** + * matrix data[row][col] + */ + float data[M][N]; + + /** + * struct for using arm_math functions + */ + arm_matrix_instance_f32 arm_mat; + + /** + * trivial ctor + * note that this ctor will not initialize elements + */ + MatrixBase() { + arm_mat = {M, N, &data[0][0]}; + } + + /** + * copyt ctor + */ + MatrixBase(const MatrixBase &m) { + arm_mat = {M, N, &data[0][0]}; + memcpy(data, m.data, sizeof(data)); + } + + MatrixBase(const float *d) { + arm_mat = {M, N, &data[0][0]}; + memcpy(data, d, sizeof(data)); + } + + MatrixBase(const float d[M][N]) { + arm_mat = {M, N, &data[0][0]}; + memcpy(data, d, sizeof(data)); + } + + /** + * set data + */ + void set(const float *d) { + memcpy(data, d, sizeof(data)); + } + + /** + * set data + */ + void set(const float d[M][N]) { + memcpy(data, d, sizeof(data)); + } + + /** + * access by index + */ + float &operator()(const unsigned int row, const unsigned int col) { + return data[row][col]; + } + + /** + * access by index + */ + float operator()(const unsigned int row, const unsigned int col) const { + return data[row][col]; + } + + /** + * get rows number + */ + unsigned int get_rows() const { + return M; + } + + /** + * get columns number + */ + unsigned int get_cols() const { + return N; + } + + /** + * test for equality + */ + bool operator ==(const Matrix &m) const { + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) + if (data[i][j] != m.data[i][j]) + return false; + + return true; + } + + /** + * test for inequality + */ + bool operator !=(const Matrix &m) const { + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) + if (data[i][j] != m.data[i][j]) + return true; + + return false; + } + + /** + * set to value + */ + const Matrix &operator =(const Matrix &m) { + memcpy(data, m.data, sizeof(data)); + return *static_cast*>(this); + } + + /** + * negation + */ + Matrix operator -(void) const { + Matrix res; + + for (unsigned int i = 0; i < N; i++) + for (unsigned int j = 0; j < M; j++) + res.data[i][j] = -data[i][j]; + + return res; + } + + /** + * addition + */ + Matrix operator +(const Matrix &m) const { + Matrix res; + + for (unsigned int i = 0; i < N; i++) + for (unsigned int j = 0; j < M; j++) + res.data[i][j] = data[i][j] + m.data[i][j]; + + return res; + } + + Matrix &operator +=(const Matrix &m) { + for (unsigned int i = 0; i < N; i++) + for (unsigned int j = 0; j < M; j++) + data[i][j] += m.data[i][j]; + + return *static_cast*>(this); + } + + /** + * subtraction + */ + Matrix operator -(const Matrix &m) const { + Matrix res; + + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) + res.data[i][j] = data[i][j] - m.data[i][j]; + + return res; + } + + Matrix &operator -=(const Matrix &m) { + for (unsigned int i = 0; i < N; i++) + for (unsigned int j = 0; j < M; j++) + data[i][j] -= m.data[i][j]; + + return *static_cast*>(this); + } + + /** + * uniform scaling + */ + Matrix operator *(const float num) const { + Matrix res; + + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) + res.data[i][j] = data[i][j] * num; + + return res; + } + + Matrix &operator *=(const float num) { + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) + data[i][j] *= num; + + return *static_cast*>(this); + } + + Matrix operator /(const float num) const { + Matrix res; + + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) + res[i][j] = data[i][j] / num; + + return res; + } + + Matrix &operator /=(const float num) { + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) + data[i][j] /= num; + + return *static_cast*>(this); + } + + /** + * multiplication by another matrix + */ + template + Matrix operator *(const Matrix &m) const { + Matrix res; + arm_mat_mult_f32(&arm_mat, &m.arm_mat, &res.arm_mat); + return res; + } + + /** + * transpose the matrix + */ + Matrix transposed(void) const { + Matrix res; + arm_mat_trans_f32(&this->arm_mat, &res.arm_mat); + return res; + } + + /** + * invert the matrix + */ + Matrix inversed(void) const { + Matrix res; + arm_mat_inverse_f32(&this->arm_mat, &res.arm_mat); + return res; + } + + /** + * set zero matrix + */ + void zero(void) { + memset(data, 0, sizeof(data)); + } + + /** + * set identity matrix + */ + void identity(void) { + memset(data, 0, sizeof(data)); + unsigned int n = (M < N) ? M : N; + + for (unsigned int i = 0; i < n; i++) + data[i][i] = 1; + } + + void print(void) { + for (unsigned int i = 0; i < M; i++) { + printf("[ "); + + for (unsigned int j = 0; j < N; j++) + printf("%.3f\t", data[i][j]); + + printf(" ]\n"); + } + } +}; + +template +class __EXPORT Matrix : public MatrixBase +{ +public: + using MatrixBase::operator *; + + Matrix() : MatrixBase() {} + + Matrix(const Matrix &m) : MatrixBase(m) {} + + Matrix(const float *d) : MatrixBase(d) {} + + Matrix(const float d[M][N]) : MatrixBase(d) {} + + /** + * set to value + */ + const Matrix &operator =(const Matrix &m) { + memcpy(this->data, m.data, sizeof(this->data)); + return *this; + } + + /** + * multiplication by a vector + */ + Vector operator *(const Vector &v) const { + Vector res; + arm_mat_mult_f32(&this->arm_mat, &v.arm_col, &res.arm_col); + return res; + } +}; + +template <> +class __EXPORT Matrix<3, 3> : public MatrixBase<3, 3> +{ +public: + using MatrixBase<3, 3>::operator *; + + Matrix() : MatrixBase<3, 3>() {} + + Matrix(const Matrix<3, 3> &m) : MatrixBase<3, 3>(m) {} + + Matrix(const float *d) : MatrixBase<3, 3>(d) {} + + Matrix(const float d[3][3]) : MatrixBase<3, 3>(d) {} + + /** + * set to value + */ + const Matrix<3, 3> &operator =(const Matrix<3, 3> &m) { + memcpy(this->data, m.data, sizeof(this->data)); + return *this; + } + + /** + * multiplication by a vector + */ + Vector<3> operator *(const Vector<3> &v) const { + Vector<3> res(data[0][0] * v.data[0] + data[0][1] * v.data[1] + data[0][2] * v.data[2], + data[1][0] * v.data[0] + data[1][1] * v.data[1] + data[1][2] * v.data[2], + data[2][0] * v.data[0] + data[2][1] * v.data[1] + data[2][2] * v.data[2]); + return res; + } + + /** + * create a rotation matrix from given euler angles + * based on http://gentlenav.googlecode.com/files/EulerAngles.pdf + */ + void from_euler(float roll, float pitch, float yaw) { + float cp = cosf(pitch); + float sp = sinf(pitch); + float sr = sinf(roll); + float cr = cosf(roll); + float sy = sinf(yaw); + float cy = cosf(yaw); + + data[0][0] = cp * cy; + data[0][1] = (sr * sp * cy) - (cr * sy); + data[0][2] = (cr * sp * cy) + (sr * sy); + data[1][0] = cp * sy; + data[1][1] = (sr * sp * sy) + (cr * cy); + data[1][2] = (cr * sp * sy) - (sr * cy); + data[2][0] = -sp; + data[2][1] = sr * cp; + data[2][2] = cr * cp; + } + + /** + * get euler angles from rotation matrix + */ + Vector<3> to_euler(void) const { + Vector<3> euler; + euler.data[1] = asinf(-data[2][0]); + + if (fabsf(euler.data[1] - M_PI_2_F) < 1.0e-3f) { + euler.data[0] = 0.0f; + euler.data[2] = atan2f(data[1][2] - data[0][1], data[0][2] + data[1][1]) + euler.data[0]; + + } else if (fabsf(euler.data[1] + M_PI_2_F) < 1.0e-3f) { + euler.data[0] = 0.0f; + euler.data[2] = atan2f(data[1][2] - data[0][1], data[0][2] + data[1][1]) - euler.data[0]; + + } else { + euler.data[0] = atan2f(data[2][1], data[2][2]); + euler.data[2] = atan2f(data[1][0], data[0][0]); + } + + return euler; + } +}; + +} + +#endif // MATRIX_HPP diff --git a/src/lib/mathlib/math/Quaternion.cpp b/src/lib/mathlib/math/Quaternion.cpp deleted file mode 100644 index 02fec4ca66..0000000000 --- a/src/lib/mathlib/math/Quaternion.cpp +++ /dev/null @@ -1,174 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Quaternion.cpp - * - * math vector - */ - -#include "test/test.hpp" - - -#include "Quaternion.hpp" -#include "Dcm.hpp" -#include "EulerAngles.hpp" - -namespace math -{ - -Quaternion::Quaternion() : - Vector(4) -{ - setA(1.0f); - setB(0.0f); - setC(0.0f); - setD(0.0f); -} - -Quaternion::Quaternion(float a, float b, - float c, float d) : - Vector(4) -{ - setA(a); - setB(b); - setC(c); - setD(d); -} - -Quaternion::Quaternion(const float *data) : - Vector(4, data) -{ -} - -Quaternion::Quaternion(const Vector &v) : - Vector(v) -{ -} - -Quaternion::Quaternion(const Dcm &dcm) : - Vector(4) -{ - // avoiding singularities by not using - // division equations - setA(0.5 * sqrt(1.0 + - double(dcm(0, 0) + dcm(1, 1) + dcm(2, 2)))); - setB(0.5 * sqrt(1.0 + - double(dcm(0, 0) - dcm(1, 1) - dcm(2, 2)))); - setC(0.5 * sqrt(1.0 + - double(-dcm(0, 0) + dcm(1, 1) - dcm(2, 2)))); - setD(0.5 * sqrt(1.0 + - double(-dcm(0, 0) - dcm(1, 1) + dcm(2, 2)))); -} - -Quaternion::Quaternion(const EulerAngles &euler) : - Vector(4) -{ - double cosPhi_2 = cos(double(euler.getPhi()) / 2.0); - double sinPhi_2 = sin(double(euler.getPhi()) / 2.0); - double cosTheta_2 = cos(double(euler.getTheta()) / 2.0); - double sinTheta_2 = sin(double(euler.getTheta()) / 2.0); - double cosPsi_2 = cos(double(euler.getPsi()) / 2.0); - double sinPsi_2 = sin(double(euler.getPsi()) / 2.0); - setA(cosPhi_2 * cosTheta_2 * cosPsi_2 + - sinPhi_2 * sinTheta_2 * sinPsi_2); - setB(sinPhi_2 * cosTheta_2 * cosPsi_2 - - cosPhi_2 * sinTheta_2 * sinPsi_2); - setC(cosPhi_2 * sinTheta_2 * cosPsi_2 + - sinPhi_2 * cosTheta_2 * sinPsi_2); - setD(cosPhi_2 * cosTheta_2 * sinPsi_2 - - sinPhi_2 * sinTheta_2 * cosPsi_2); -} - -Quaternion::Quaternion(const Quaternion &right) : - Vector(right) -{ -} - -Quaternion::~Quaternion() -{ -} - -Vector Quaternion::derivative(const Vector &w) -{ -#ifdef QUATERNION_ASSERT - ASSERT(w.getRows() == 3); -#endif - float dataQ[] = { - getA(), -getB(), -getC(), -getD(), - getB(), getA(), -getD(), getC(), - getC(), getD(), getA(), -getB(), - getD(), -getC(), getB(), getA() - }; - Vector v(4); - v(0) = 0.0f; - v(1) = w(0); - v(2) = w(1); - v(3) = w(2); - Matrix Q(4, 4, dataQ); - return Q * v * 0.5f; -} - -int __EXPORT quaternionTest() -{ - printf("Test Quaternion\t\t: "); - // test default ctor - Quaternion q; - ASSERT(equal(q.getA(), 1.0f)); - ASSERT(equal(q.getB(), 0.0f)); - ASSERT(equal(q.getC(), 0.0f)); - ASSERT(equal(q.getD(), 0.0f)); - // test float ctor - q = Quaternion(0.1825742f, 0.3651484f, 0.5477226f, 0.7302967f); - ASSERT(equal(q.getA(), 0.1825742f)); - ASSERT(equal(q.getB(), 0.3651484f)); - ASSERT(equal(q.getC(), 0.5477226f)); - ASSERT(equal(q.getD(), 0.7302967f)); - // test euler ctor - q = Quaternion(EulerAngles(0.1f, 0.2f, 0.3f)); - ASSERT(vectorEqual(q, Quaternion(0.983347f, 0.034271f, 0.106021f, 0.143572f))); - // test dcm ctor - q = Quaternion(Dcm()); - ASSERT(vectorEqual(q, Quaternion(1.0f, 0.0f, 0.0f, 0.0f))); - // TODO test derivative - // test accessors - q.setA(0.1f); - q.setB(0.2f); - q.setC(0.3f); - q.setD(0.4f); - ASSERT(vectorEqual(q, Quaternion(0.1f, 0.2f, 0.3f, 0.4f))); - printf("PASS\n"); - return 0; -} - -} // namespace math diff --git a/src/lib/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp index 048a55d332..21d05c7ef1 100644 --- a/src/lib/mathlib/math/Quaternion.hpp +++ b/src/lib/mathlib/math/Quaternion.hpp @@ -1,6 +1,9 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin + * Pavel Kirienko + * Lorenz Meier * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,82 +37,129 @@ /** * @file Quaternion.hpp * - * math quaternion lib + * Quaternion class */ -#pragma once +#ifndef QUATERNION_HPP +#define QUATERNION_HPP +#include +#include "../CMSIS/Include/arm_math.h" #include "Vector.hpp" #include "Matrix.hpp" namespace math { -class Dcm; -class EulerAngles; - -class __EXPORT Quaternion : public Vector +class __EXPORT Quaternion : public Vector<4> { public: + /** + * trivial ctor + */ + Quaternion() : Vector<4>() {} /** - * default ctor + * copy ctor */ - Quaternion(); + Quaternion(const Quaternion &q) : Vector<4>(q) {} /** - * ctor from floats + * casting from vector */ - Quaternion(float a, float b, float c, float d); + Quaternion(const Vector<4> &v) : Vector<4>(v) {} /** - * ctor from data + * setting ctor */ - Quaternion(const float *data); + Quaternion(const float d[4]) : Vector<4>(d) {} /** - * ctor from Vector + * setting ctor */ - Quaternion(const Vector &v); + Quaternion(const float a0, const float b0, const float c0, const float d0): Vector<4>(a0, b0, c0, d0) {} + + using Vector<4>::operator *; /** - * ctor from EulerAngles + * multiplication */ - Quaternion(const EulerAngles &euler); - - /** - * ctor from Dcm - */ - Quaternion(const Dcm &dcm); - - /** - * deep copy ctor - */ - Quaternion(const Quaternion &right); - - /** - * dtor - */ - virtual ~Quaternion(); + const Quaternion operator *(const Quaternion &q) const { + return Quaternion( + data[0] * q.data[0] - data[1] * q.data[1] - data[2] * q.data[2] - data[3] * q.data[3], + data[0] * q.data[1] + data[1] * q.data[0] + data[2] * q.data[3] - data[3] * q.data[2], + data[0] * q.data[2] - data[1] * q.data[3] + data[2] * q.data[0] + data[3] * q.data[1], + data[0] * q.data[3] + data[1] * q.data[2] - data[2] * q.data[1] + data[3] * q.data[0]); + } /** * derivative */ - Vector derivative(const Vector &w); + const Quaternion derivative(const Vector<3> &w) { + float dataQ[] = { + data[0], -data[1], -data[2], -data[3], + data[1], data[0], -data[3], data[2], + data[2], data[3], data[0], -data[1], + data[3], -data[2], data[1], data[0] + }; + Matrix<4, 4> Q(dataQ); + Vector<4> v(0.0f, w.data[0], w.data[1], w.data[2]); + return Q * v * 0.5f; + } /** - * accessors + * imaginary part of quaternion */ - void setA(float a) { (*this)(0) = a; } - void setB(float b) { (*this)(1) = b; } - void setC(float c) { (*this)(2) = c; } - void setD(float d) { (*this)(3) = d; } - const float &getA() const { return (*this)(0); } - const float &getB() const { return (*this)(1); } - const float &getC() const { return (*this)(2); } - const float &getD() const { return (*this)(3); } + Vector<3> imag(void) { + return Vector<3>(&data[1]); + } + + /** + * set quaternion to rotation defined by euler angles + */ + void from_euler(float roll, float pitch, float yaw) { + double cosPhi_2 = cos(double(roll) / 2.0); + double sinPhi_2 = sin(double(roll) / 2.0); + double cosTheta_2 = cos(double(pitch) / 2.0); + double sinTheta_2 = sin(double(pitch) / 2.0); + double cosPsi_2 = cos(double(yaw) / 2.0); + double sinPsi_2 = sin(double(yaw) / 2.0); + data[0] = cosPhi_2 * cosTheta_2 * cosPsi_2 + sinPhi_2 * sinTheta_2 * sinPsi_2; + data[1] = sinPhi_2 * cosTheta_2 * cosPsi_2 - cosPhi_2 * sinTheta_2 * sinPsi_2; + data[2] = cosPhi_2 * sinTheta_2 * cosPsi_2 + sinPhi_2 * cosTheta_2 * sinPsi_2; + data[3] = cosPhi_2 * cosTheta_2 * sinPsi_2 - sinPhi_2 * sinTheta_2 * cosPsi_2; + } + + void from_dcm(const Matrix<3, 3> &m) { + // avoiding singularities by not using division equations + data[0] = 0.5f * sqrtf(1.0f + m.data[0][0] + m.data[1][1] + m.data[2][2]); + data[1] = 0.5f * sqrtf(1.0f + m.data[0][0] - m.data[1][1] - m.data[2][2]); + data[2] = 0.5f * sqrtf(1.0f - m.data[0][0] + m.data[1][1] - m.data[2][2]); + data[3] = 0.5f * sqrtf(1.0f - m.data[0][0] - m.data[1][1] + m.data[2][2]); + } + + /** + * create rotation matrix for the quaternion + */ + Matrix<3, 3> to_dcm(void) const { + Matrix<3, 3> R; + float aSq = data[0] * data[0]; + float bSq = data[1] * data[1]; + float cSq = data[2] * data[2]; + float dSq = data[3] * data[3]; + R.data[0][0] = aSq + bSq - cSq - dSq; + R.data[0][1] = 2.0f * (data[1] * data[2] - data[0] * data[3]); + R.data[0][2] = 2.0f * (data[0] * data[2] + data[1] * data[3]); + R.data[1][0] = 2.0f * (data[1] * data[2] + data[0] * data[3]); + R.data[1][1] = aSq - bSq + cSq - dSq; + R.data[1][2] = 2.0f * (data[2] * data[3] - data[0] * data[1]); + R.data[2][0] = 2.0f * (data[1] * data[3] - data[0] * data[2]); + R.data[2][1] = 2.0f * (data[0] * data[1] + data[2] * data[3]); + R.data[2][2] = aSq - bSq - cSq + dSq; + return R; + } }; -int __EXPORT quaternionTest(); -} // math +} +#endif // QUATERNION_HPP diff --git a/src/lib/mathlib/math/Vector.cpp b/src/lib/mathlib/math/Vector.cpp deleted file mode 100644 index 35158a396c..0000000000 --- a/src/lib/mathlib/math/Vector.cpp +++ /dev/null @@ -1,100 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Vector.cpp - * - * math vector - */ - -#include "test/test.hpp" - -#include "Vector.hpp" - -namespace math -{ - -static const float data_testA[] = {1, 3}; -static const float data_testB[] = {4, 1}; - -static Vector testA(2, data_testA); -static Vector testB(2, data_testB); - -int __EXPORT vectorTest() -{ - vectorAddTest(); - vectorSubTest(); - return 0; -} - -int vectorAddTest() -{ - printf("Test Vector Add\t\t: "); - Vector r = testA + testB; - float data_test[] = {5.0f, 4.0f}; - ASSERT(vectorEqual(Vector(2, data_test), r)); - printf("PASS\n"); - return 0; -} - -int vectorSubTest() -{ - printf("Test Vector Sub\t\t: "); - Vector r(2); - r = testA - testB; - float data_test[] = { -3.0f, 2.0f}; - ASSERT(vectorEqual(Vector(2, data_test), r)); - printf("PASS\n"); - return 0; -} - -bool vectorEqual(const Vector &a, const Vector &b, float eps) -{ - if (a.getRows() != b.getRows()) { - printf("row number not equal a: %d, b:%d\n", a.getRows(), b.getRows()); - return false; - } - - bool ret = true; - - for (size_t i = 0; i < a.getRows(); i++) { - if (!equal(a(i), b(i), eps)) { - printf("element mismatch (%d)\n", i); - ret = false; - } - } - - return ret; -} - -} // namespace math diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp index 73de793d51..c7323c2153 100644 --- a/src/lib/mathlib/math/Vector.hpp +++ b/src/lib/mathlib/math/Vector.hpp @@ -1,6 +1,9 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin + * Pavel Kirienko + * Lorenz Meier * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,26 +35,466 @@ ****************************************************************************/ /** - * @file Vector.h + * @file Vector.hpp * - * math vector + * Vector class */ -#pragma once +#ifndef VECTOR_HPP +#define VECTOR_HPP -#include - -#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU) -#include "arm/Vector.hpp" -#else -#include "generic/Vector.hpp" -#endif +#include +#include +#include "../CMSIS/Include/arm_math.h" namespace math { -class Vector; -int __EXPORT vectorTest(); -int __EXPORT vectorAddTest(); -int __EXPORT vectorSubTest(); -bool vectorEqual(const Vector &a, const Vector &b, float eps = 1.0e-5f); -} // math + +template +class __EXPORT Vector; + +template +class __EXPORT VectorBase +{ +public: + /** + * vector data + */ + float data[N]; + + /** + * struct for using arm_math functions, represents column vector + */ + arm_matrix_instance_f32 arm_col; + + /** + * trivial ctor + * note that this ctor will not initialize elements + */ + VectorBase() { + arm_col = {N, 1, &data[0]}; + } + + /** + * copy ctor + */ + VectorBase(const VectorBase &v) { + arm_col = {N, 1, &data[0]}; + memcpy(data, v.data, sizeof(data)); + } + + /** + * setting ctor + */ + VectorBase(const float d[N]) { + arm_col = {N, 1, &data[0]}; + memcpy(data, d, sizeof(data)); + } + + /** + * set data + */ + void set(const float d[N]) { + memcpy(data, d, sizeof(data)); + } + + /** + * access to elements by index + */ + float &operator()(const unsigned int i) { + return data[i]; + } + + /** + * access to elements by index + */ + float operator()(const unsigned int i) const { + return data[i]; + } + + /** + * get vector size + */ + unsigned int get_size() const { + return N; + } + + /** + * test for equality + */ + bool operator ==(const Vector &v) const { + for (unsigned int i = 0; i < N; i++) + if (data[i] != v.data[i]) + return false; + + return true; + } + + /** + * test for inequality + */ + bool operator !=(const Vector &v) const { + for (unsigned int i = 0; i < N; i++) + if (data[i] != v.data[i]) + return true; + + return false; + } + + /** + * set to value + */ + const Vector &operator =(const Vector &v) { + memcpy(data, v.data, sizeof(data)); + return *static_cast*>(this); + } + + /** + * negation + */ + const Vector operator -(void) const { + Vector res; + + for (unsigned int i = 0; i < N; i++) + res.data[i] = -data[i]; + + return res; + } + + /** + * addition + */ + const Vector operator +(const Vector &v) const { + Vector res; + + for (unsigned int i = 0; i < N; i++) + res.data[i] = data[i] + v.data[i]; + + return res; + } + + /** + * subtraction + */ + const Vector operator -(const Vector &v) const { + Vector res; + + for (unsigned int i = 0; i < N; i++) + res.data[i] = data[i] - v.data[i]; + + return res; + } + + /** + * uniform scaling + */ + const Vector operator *(const float num) const { + Vector res; + + for (unsigned int i = 0; i < N; i++) + res.data[i] = data[i] * num; + + return res; + } + + /** + * uniform scaling + */ + const Vector operator /(const float num) const { + Vector res; + + for (unsigned int i = 0; i < N; i++) + res.data[i] = data[i] / num; + + return res; + } + + /** + * addition + */ + const Vector &operator +=(const Vector &v) { + for (unsigned int i = 0; i < N; i++) + data[i] += v.data[i]; + + return *static_cast*>(this); + } + + /** + * subtraction + */ + const Vector &operator -=(const Vector &v) { + for (unsigned int i = 0; i < N; i++) + data[i] -= v.data[i]; + + return *static_cast*>(this); + } + + /** + * uniform scaling + */ + const Vector &operator *=(const float num) { + for (unsigned int i = 0; i < N; i++) + data[i] *= num; + + return *static_cast*>(this); + } + + /** + * uniform scaling + */ + const Vector &operator /=(const float num) { + for (unsigned int i = 0; i < N; i++) + data[i] /= num; + + return *static_cast*>(this); + } + + /** + * dot product + */ + float operator *(const Vector &v) const { + float res = 0.0f; + + for (unsigned int i = 0; i < N; i++) + res += data[i] * v.data[i]; + + return res; + } + + /** + * element by element multiplication + */ + const Vector emult(const Vector &v) const { + Vector res; + + for (unsigned int i = 0; i < N; i++) + res.data[i] = data[i] * v.data[i]; + + return res; + } + + /** + * element by element division + */ + const Vector edivide(const Vector &v) const { + Vector res; + + for (unsigned int i = 0; i < N; i++) + res.data[i] = data[i] / v.data[i]; + + return res; + } + + /** + * gets the length of this vector squared + */ + float length_squared() const { + float res = 0.0f; + + for (unsigned int i = 0; i < N; i++) + res += data[i] * data[i]; + + return res; + } + + /** + * gets the length of this vector + */ + float length() const { + float res = 0.0f; + + for (unsigned int i = 0; i < N; i++) + res += data[i] * data[i]; + + return sqrtf(res); + } + + /** + * normalizes this vector + */ + void normalize() { + *this /= length(); + } + + /** + * returns the normalized version of this vector + */ + Vector normalized() const { + return *this / length(); + } + + /** + * set zero vector + */ + void zero(void) { + memset(data, 0, sizeof(data)); + } + + void print(void) { + printf("[ "); + + for (unsigned int i = 0; i < N; i++) + printf("%.3f\t", data[i]); + + printf("]\n"); + } +}; + +template +class __EXPORT Vector : public VectorBase +{ +public: + Vector() : VectorBase() {} + + Vector(const Vector &v) : VectorBase(v) {} + + Vector(const float d[N]) : VectorBase(d) {} + + /** + * set to value + */ + const Vector &operator =(const Vector &v) { + memcpy(this->data, v.data, sizeof(this->data)); + return *this; + } +}; + +template <> +class __EXPORT Vector<2> : public VectorBase<2> +{ +public: + Vector() : VectorBase<2>() {} + + // simple copy is 1.6 times faster than memcpy + Vector(const Vector<2> &v) : VectorBase<2>() { + data[0] = v.data[0]; + data[1] = v.data[1]; + } + + Vector(const float d[2]) : VectorBase<2>() { + data[0] = d[0]; + data[1] = d[1]; + } + + Vector(const float x, const float y) : VectorBase<2>() { + data[0] = x; + data[1] = y; + } + + /** + * set data + */ + void set(const float d[2]) { + data[0] = d[0]; + data[1] = d[1]; + } + + /** + * set to value + */ + const Vector<2> &operator =(const Vector<2> &v) { + data[0] = v.data[0]; + data[1] = v.data[1]; + return *this; + } + + float operator %(const Vector<2> &v) const { + return data[0] * v.data[1] - data[1] * v.data[0]; + } +}; + +template <> +class __EXPORT Vector<3> : public VectorBase<3> +{ +public: + Vector() : VectorBase<3>() {} + + // simple copy is 1.6 times faster than memcpy + Vector(const Vector<3> &v) : VectorBase<3>() { + for (unsigned int i = 0; i < 3; i++) + data[i] = v.data[i]; + } + + Vector(const float d[3]) : VectorBase<3>() { + for (unsigned int i = 0; i < 3; i++) + data[i] = d[i]; + } + + Vector(const float x, const float y, const float z) : VectorBase<3>() { + data[0] = x; + data[1] = y; + data[2] = z; + } + + /** + * set data + */ + void set(const float d[3]) { + for (unsigned int i = 0; i < 3; i++) + data[i] = d[i]; + } + + /** + * set to value + */ + const Vector<3> &operator =(const Vector<3> &v) { + for (unsigned int i = 0; i < 3; i++) + data[i] = v.data[i]; + + return *this; + } + + Vector<3> operator %(const Vector<3> &v) const { + return Vector<3>( + data[1] * v.data[2] - data[2] * v.data[1], + data[2] * v.data[0] - data[0] * v.data[2], + data[0] * v.data[1] - data[1] * v.data[0] + ); + } +}; + +template <> +class __EXPORT Vector<4> : public VectorBase<4> +{ +public: + Vector() : VectorBase() {} + + Vector(const Vector<4> &v) : VectorBase<4>() { + for (unsigned int i = 0; i < 4; i++) + data[i] = v.data[i]; + } + + Vector(const float d[4]) : VectorBase<4>() { + for (unsigned int i = 0; i < 4; i++) + data[i] = d[i]; + } + + Vector(const float x0, const float x1, const float x2, const float x3) : VectorBase() { + data[0] = x0; + data[1] = x1; + data[2] = x2; + data[3] = x3; + } + + /** + * set data + */ + void set(const float d[4]) { + for (unsigned int i = 0; i < 4; i++) + data[i] = d[i]; + } + + /** + * set to value + */ + const Vector<4> &operator =(const Vector<4> &v) { + for (unsigned int i = 0; i < 4; i++) + data[i] = v.data[i]; + + return *this; + } +}; + +} + +#endif // VECTOR_HPP diff --git a/src/lib/mathlib/math/Vector2f.cpp b/src/lib/mathlib/math/Vector2f.cpp deleted file mode 100644 index 68e741817d..0000000000 --- a/src/lib/mathlib/math/Vector2f.cpp +++ /dev/null @@ -1,103 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Vector2f.cpp - * - * math vector - */ - -#include "test/test.hpp" - -#include "Vector2f.hpp" - -namespace math -{ - -Vector2f::Vector2f() : - Vector(2) -{ -} - -Vector2f::Vector2f(const Vector &right) : - Vector(right) -{ -#ifdef VECTOR_ASSERT - ASSERT(right.getRows() == 2); -#endif -} - -Vector2f::Vector2f(float x, float y) : - Vector(2) -{ - setX(x); - setY(y); -} - -Vector2f::Vector2f(const float *data) : - Vector(2, data) -{ -} - -Vector2f::~Vector2f() -{ -} - -float Vector2f::cross(const Vector2f &b) const -{ - const Vector2f &a = *this; - return a(0)*b(1) - a(1)*b(0); -} - -float Vector2f::operator %(const Vector2f &v) const -{ - return cross(v); -} - -float Vector2f::operator *(const Vector2f &v) const -{ - return dot(v); -} - -int __EXPORT vector2fTest() -{ - printf("Test Vector2f\t\t: "); - // test float ctor - Vector2f v(1, 2); - ASSERT(equal(v(0), 1)); - ASSERT(equal(v(1), 2)); - printf("PASS\n"); - return 0; -} - -} // namespace math diff --git a/src/lib/mathlib/math/Vector3.cpp b/src/lib/mathlib/math/Vector3.cpp deleted file mode 100644 index dcb85600ef..0000000000 --- a/src/lib/mathlib/math/Vector3.cpp +++ /dev/null @@ -1,99 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Vector3.cpp - * - * math vector - */ - -#include "test/test.hpp" - -#include "Vector3.hpp" - -namespace math -{ - -Vector3::Vector3() : - Vector(3) -{ -} - -Vector3::Vector3(const Vector &right) : - Vector(right) -{ -#ifdef VECTOR_ASSERT - ASSERT(right.getRows() == 3); -#endif -} - -Vector3::Vector3(float x, float y, float z) : - Vector(3) -{ - setX(x); - setY(y); - setZ(z); -} - -Vector3::Vector3(const float *data) : - Vector(3, data) -{ -} - -Vector3::~Vector3() -{ -} - -Vector3 Vector3::cross(const Vector3 &b) const -{ - const Vector3 &a = *this; - Vector3 result; - result(0) = a(1) * b(2) - a(2) * b(1); - result(1) = a(2) * b(0) - a(0) * b(2); - result(2) = a(0) * b(1) - a(1) * b(0); - return result; -} - -int __EXPORT vector3Test() -{ - printf("Test Vector3\t\t: "); - // test float ctor - Vector3 v(1, 2, 3); - ASSERT(equal(v(0), 1)); - ASSERT(equal(v(1), 2)); - ASSERT(equal(v(2), 3)); - printf("PASS\n"); - return 0; -} - -} // namespace math diff --git a/src/lib/mathlib/math/arm/Matrix.hpp b/src/lib/mathlib/math/arm/Matrix.hpp deleted file mode 100644 index 1945bb02db..0000000000 --- a/src/lib/mathlib/math/arm/Matrix.hpp +++ /dev/null @@ -1,292 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Matrix.h - * - * matrix code - */ - -#pragma once - - -#include -#include -#include -#include -#include -#include - -#include "../Vector.hpp" -#include "../Matrix.hpp" - -// arm specific -#include "../../CMSIS/Include/arm_math.h" - -namespace math -{ - -class __EXPORT Matrix -{ -public: - // constructor - Matrix(size_t rows, size_t cols) : - _matrix() { - arm_mat_init_f32(&_matrix, - rows, cols, - (float *)calloc(rows * cols, sizeof(float))); - } - Matrix(size_t rows, size_t cols, const float *data) : - _matrix() { - arm_mat_init_f32(&_matrix, - rows, cols, - (float *)malloc(rows * cols * sizeof(float))); - memcpy(getData(), data, getSize()); - } - // deconstructor - virtual ~Matrix() { - delete [] _matrix.pData; - } - // copy constructor (deep) - Matrix(const Matrix &right) : - _matrix() { - arm_mat_init_f32(&_matrix, - right.getRows(), right.getCols(), - (float *)malloc(right.getRows()* - right.getCols()*sizeof(float))); - memcpy(getData(), right.getData(), - getSize()); - } - // assignment - inline Matrix &operator=(const Matrix &right) { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == right.getRows()); - ASSERT(getCols() == right.getCols()); -#endif - - if (this != &right) { - memcpy(getData(), right.getData(), - right.getSize()); - } - - return *this; - } - // element accessors - inline float &operator()(size_t i, size_t j) { -#ifdef MATRIX_ASSERT - ASSERT(i < getRows()); - ASSERT(j < getCols()); -#endif - return getData()[i * getCols() + j]; - } - inline const float &operator()(size_t i, size_t j) const { -#ifdef MATRIX_ASSERT - ASSERT(i < getRows()); - ASSERT(j < getCols()); -#endif - return getData()[i * getCols() + j]; - } - // output - inline void print() const { - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - float sig; - int exponent; - float num = (*this)(i, j); - float2SigExp(num, sig, exponent); - printf("%6.3fe%03d ", (double)sig, exponent); - } - - printf("\n"); - } - } - // boolean ops - inline bool operator==(const Matrix &right) const { - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - if (fabsf((*this)(i, j) - right(i, j)) > 1e-30f) - return false; - } - } - - return true; - } - // scalar ops - inline Matrix operator+(float right) const { - Matrix result(getRows(), getCols()); - arm_offset_f32((float *)getData(), right, - (float *)result.getData(), getRows()*getCols()); - return result; - } - inline Matrix operator-(float right) const { - Matrix result(getRows(), getCols()); - arm_offset_f32((float *)getData(), -right, - (float *)result.getData(), getRows()*getCols()); - return result; - } - inline Matrix operator*(float right) const { - Matrix result(getRows(), getCols()); - arm_mat_scale_f32(&_matrix, right, - &(result._matrix)); - return result; - } - inline Matrix operator/(float right) const { - Matrix result(getRows(), getCols()); - arm_mat_scale_f32(&_matrix, 1.0f / right, - &(result._matrix)); - return result; - } - // vector ops - inline Vector operator*(const Vector &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getCols() == right.getRows()); -#endif - Matrix resultMat = (*this) * - Matrix(right.getRows(), 1, right.getData()); - return Vector(getRows(), resultMat.getData()); - } - // matrix ops - inline Matrix operator+(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == right.getRows()); - ASSERT(getCols() == right.getCols()); -#endif - Matrix result(getRows(), getCols()); - arm_mat_add_f32(&_matrix, &(right._matrix), - &(result._matrix)); - return result; - } - inline Matrix operator-(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == right.getRows()); - ASSERT(getCols() == right.getCols()); -#endif - Matrix result(getRows(), getCols()); - arm_mat_sub_f32(&_matrix, &(right._matrix), - &(result._matrix)); - return result; - } - inline Matrix operator*(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getCols() == right.getRows()); -#endif - Matrix result(getRows(), right.getCols()); - arm_mat_mult_f32(&_matrix, &(right._matrix), - &(result._matrix)); - return result; - } - inline Matrix operator/(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(right.getRows() == right.getCols()); - ASSERT(getCols() == right.getCols()); -#endif - return (*this) * right.inverse(); - } - // other functions - inline Matrix transpose() const { - Matrix result(getCols(), getRows()); - arm_mat_trans_f32(&_matrix, &(result._matrix)); - return result; - } - inline void swapRows(size_t a, size_t b) { - if (a == b) return; - - for (size_t j = 0; j < getCols(); j++) { - float tmp = (*this)(a, j); - (*this)(a, j) = (*this)(b, j); - (*this)(b, j) = tmp; - } - } - inline void swapCols(size_t a, size_t b) { - if (a == b) return; - - for (size_t i = 0; i < getRows(); i++) { - float tmp = (*this)(i, a); - (*this)(i, a) = (*this)(i, b); - (*this)(i, b) = tmp; - } - } - /** - * inverse based on LU factorization with partial pivotting - */ - Matrix inverse() const { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == getCols()); -#endif - Matrix result(getRows(), getCols()); - Matrix work = (*this); - arm_mat_inverse_f32(&(work._matrix), - &(result._matrix)); - return result; - } - inline void setAll(const float &val) { - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - (*this)(i, j) = val; - } - } - } - inline void set(const float *data) { - memcpy(getData(), data, getSize()); - } - inline size_t getRows() const { return _matrix.numRows; } - inline size_t getCols() const { return _matrix.numCols; } - inline static Matrix identity(size_t size) { - Matrix result(size, size); - - for (size_t i = 0; i < size; i++) { - result(i, i) = 1.0f; - } - - return result; - } - inline static Matrix zero(size_t size) { - Matrix result(size, size); - result.setAll(0.0f); - return result; - } - inline static Matrix zero(size_t m, size_t n) { - Matrix result(m, n); - result.setAll(0.0f); - return result; - } -protected: - inline size_t getSize() const { return sizeof(float) * getRows() * getCols(); } - inline float *getData() { return _matrix.pData; } - inline const float *getData() const { return _matrix.pData; } - inline void setData(float *data) { _matrix.pData = data; } -private: - arm_matrix_instance_f32 _matrix; -}; - -} // namespace math diff --git a/src/lib/mathlib/math/arm/Vector.hpp b/src/lib/mathlib/math/arm/Vector.hpp deleted file mode 100644 index 52220fc152..0000000000 --- a/src/lib/mathlib/math/arm/Vector.hpp +++ /dev/null @@ -1,236 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Vector.h - * - * math vector - */ - -#pragma once - -#include -#include -#include -#include -#include -#include - -#include "../Vector.hpp" -#include "../test/test.hpp" - -// arm specific -#include "../../CMSIS/Include/arm_math.h" - -namespace math -{ - -class __EXPORT Vector -{ -public: - // constructor - Vector(size_t rows) : - _rows(rows), - _data((float *)calloc(rows, sizeof(float))) { - } - Vector(size_t rows, const float *data) : - _rows(rows), - _data((float *)malloc(getSize())) { - memcpy(getData(), data, getSize()); - } - // deconstructor - virtual ~Vector() { - delete [] getData(); - } - // copy constructor (deep) - Vector(const Vector &right) : - _rows(right.getRows()), - _data((float *)malloc(getSize())) { - memcpy(getData(), right.getData(), - right.getSize()); - } - // assignment - inline Vector &operator=(const Vector &right) { -#ifdef VECTOR_ASSERT - ASSERT(getRows() == right.getRows()); -#endif - - if (this != &right) { - memcpy(getData(), right.getData(), - right.getSize()); - } - - return *this; - } - // element accessors - inline float &operator()(size_t i) { -#ifdef VECTOR_ASSERT - ASSERT(i < getRows()); -#endif - return getData()[i]; - } - inline const float &operator()(size_t i) const { -#ifdef VECTOR_ASSERT - ASSERT(i < getRows()); -#endif - return getData()[i]; - } - // output - inline void print() const { - for (size_t i = 0; i < getRows(); i++) { - float sig; - int exponent; - float num = (*this)(i); - float2SigExp(num, sig, exponent); - printf("%6.3fe%03d ", (double)sig, exponent); - } - - printf("\n"); - } - // boolean ops - inline bool operator==(const Vector &right) const { - for (size_t i = 0; i < getRows(); i++) { - if (fabsf(((*this)(i) - right(i))) > 1e-30f) - return false; - } - - return true; - } - // scalar ops - inline Vector operator+(float right) const { - Vector result(getRows()); - arm_offset_f32((float *)getData(), - right, result.getData(), - getRows()); - return result; - } - inline Vector operator-(float right) const { - Vector result(getRows()); - arm_offset_f32((float *)getData(), - -right, result.getData(), - getRows()); - return result; - } - inline Vector operator*(float right) const { - Vector result(getRows()); - arm_scale_f32((float *)getData(), - right, result.getData(), - getRows()); - return result; - } - inline Vector operator/(float right) const { - Vector result(getRows()); - arm_scale_f32((float *)getData(), - 1.0f / right, result.getData(), - getRows()); - return result; - } - // vector ops - inline Vector operator+(const Vector &right) const { -#ifdef VECTOR_ASSERT - ASSERT(getRows() == right.getRows()); -#endif - Vector result(getRows()); - arm_add_f32((float *)getData(), - (float *)right.getData(), - result.getData(), - getRows()); - return result; - } - inline Vector operator-(const Vector &right) const { -#ifdef VECTOR_ASSERT - ASSERT(getRows() == right.getRows()); -#endif - Vector result(getRows()); - arm_sub_f32((float *)getData(), - (float *)right.getData(), - result.getData(), - getRows()); - return result; - } - inline Vector operator-(void) const { - Vector result(getRows()); - arm_negate_f32((float *)getData(), - result.getData(), - getRows()); - return result; - } - // other functions - inline float dot(const Vector &right) const { - float result = 0; - arm_dot_prod_f32((float *)getData(), - (float *)right.getData(), - getRows(), - &result); - return result; - } - inline float norm() const { - return sqrtf(dot(*this)); - } - inline float length() const { - return norm(); - } - inline Vector unit() const { - return (*this) / norm(); - } - inline Vector normalized() const { - return unit(); - } - inline void normalize() { - (*this) = (*this) / norm(); - } - inline static Vector zero(size_t rows) { - Vector result(rows); - // calloc returns zeroed memory - return result; - } - inline void setAll(const float &val) { - for (size_t i = 0; i < getRows(); i++) { - (*this)(i) = val; - } - } - inline void set(const float *data) { - memcpy(getData(), data, getSize()); - } - inline size_t getRows() const { return _rows; } - inline const float *getData() const { return _data; } -protected: - inline size_t getSize() const { return sizeof(float) * getRows(); } - inline float *getData() { return _data; } - inline void setData(float *data) { _data = data; } -private: - size_t _rows; - float *_data; -}; - -} // math diff --git a/src/lib/mathlib/math/generic/Matrix.hpp b/src/lib/mathlib/math/generic/Matrix.hpp deleted file mode 100644 index 5601a3447b..0000000000 --- a/src/lib/mathlib/math/generic/Matrix.hpp +++ /dev/null @@ -1,437 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Matrix.h - * - * matrix code - */ - -#pragma once - - -#include -#include -#include -#include -#include -#include - -#include "../Vector.hpp" -#include "../Matrix.hpp" - -namespace math -{ - -class __EXPORT Matrix -{ -public: - // constructor - Matrix(size_t rows, size_t cols) : - _rows(rows), - _cols(cols), - _data((float *)calloc(rows *cols, sizeof(float))) { - } - Matrix(size_t rows, size_t cols, const float *data) : - _rows(rows), - _cols(cols), - _data((float *)malloc(getSize())) { - memcpy(getData(), data, getSize()); - } - // deconstructor - virtual ~Matrix() { - delete [] getData(); - } - // copy constructor (deep) - Matrix(const Matrix &right) : - _rows(right.getRows()), - _cols(right.getCols()), - _data((float *)malloc(getSize())) { - memcpy(getData(), right.getData(), - right.getSize()); - } - // assignment - inline Matrix &operator=(const Matrix &right) { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == right.getRows()); - ASSERT(getCols() == right.getCols()); -#endif - - if (this != &right) { - memcpy(getData(), right.getData(), - right.getSize()); - } - - return *this; - } - // element accessors - inline float &operator()(size_t i, size_t j) { -#ifdef MATRIX_ASSERT - ASSERT(i < getRows()); - ASSERT(j < getCols()); -#endif - return getData()[i * getCols() + j]; - } - inline const float &operator()(size_t i, size_t j) const { -#ifdef MATRIX_ASSERT - ASSERT(i < getRows()); - ASSERT(j < getCols()); -#endif - return getData()[i * getCols() + j]; - } - // output - inline void print() const { - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - float sig; - int exp; - float num = (*this)(i, j); - float2SigExp(num, sig, exp); - printf("%6.3fe%03.3d,", (double)sig, exp); - } - - printf("\n"); - } - } - // boolean ops - inline bool operator==(const Matrix &right) const { - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - if (fabsf((*this)(i, j) - right(i, j)) > 1e-30f) - return false; - } - } - - return true; - } - // scalar ops - inline Matrix operator+(const float &right) const { - Matrix result(getRows(), getCols()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(i, j) = (*this)(i, j) + right; - } - } - - return result; - } - inline Matrix operator-(const float &right) const { - Matrix result(getRows(), getCols()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(i, j) = (*this)(i, j) - right; - } - } - - return result; - } - inline Matrix operator*(const float &right) const { - Matrix result(getRows(), getCols()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(i, j) = (*this)(i, j) * right; - } - } - - return result; - } - inline Matrix operator/(const float &right) const { - Matrix result(getRows(), getCols()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(i, j) = (*this)(i, j) / right; - } - } - - return result; - } - // vector ops - inline Vector operator*(const Vector &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getCols() == right.getRows()); -#endif - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(i) += (*this)(i, j) * right(j); - } - } - - return result; - } - // matrix ops - inline Matrix operator+(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == right.getRows()); - ASSERT(getCols() == right.getCols()); -#endif - Matrix result(getRows(), getCols()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(i, j) = (*this)(i, j) + right(i, j); - } - } - - return result; - } - inline Matrix operator-(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == right.getRows()); - ASSERT(getCols() == right.getCols()); -#endif - Matrix result(getRows(), getCols()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(i, j) = (*this)(i, j) - right(i, j); - } - } - - return result; - } - inline Matrix operator*(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getCols() == right.getRows()); -#endif - Matrix result(getRows(), right.getCols()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < right.getCols(); j++) { - for (size_t k = 0; k < right.getRows(); k++) { - result(i, j) += (*this)(i, k) * right(k, j); - } - } - } - - return result; - } - inline Matrix operator/(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(right.getRows() == right.getCols()); - ASSERT(getCols() == right.getCols()); -#endif - return (*this) * right.inverse(); - } - // other functions - inline Matrix transpose() const { - Matrix result(getCols(), getRows()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(j, i) = (*this)(i, j); - } - } - - return result; - } - inline void swapRows(size_t a, size_t b) { - if (a == b) return; - - for (size_t j = 0; j < getCols(); j++) { - float tmp = (*this)(a, j); - (*this)(a, j) = (*this)(b, j); - (*this)(b, j) = tmp; - } - } - inline void swapCols(size_t a, size_t b) { - if (a == b) return; - - for (size_t i = 0; i < getRows(); i++) { - float tmp = (*this)(i, a); - (*this)(i, a) = (*this)(i, b); - (*this)(i, b) = tmp; - } - } - /** - * inverse based on LU factorization with partial pivotting - */ - Matrix inverse() const { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == getCols()); -#endif - size_t N = getRows(); - Matrix L = identity(N); - const Matrix &A = (*this); - Matrix U = A; - Matrix P = identity(N); - - //printf("A:\n"); A.print(); - - // for all diagonal elements - for (size_t n = 0; n < N; n++) { - - // if diagonal is zero, swap with row below - if (fabsf(U(n, n)) < 1e-8f) { - //printf("trying pivot for row %d\n",n); - for (size_t i = 0; i < N; i++) { - if (i == n) continue; - - //printf("\ttrying row %d\n",i); - if (fabsf(U(i, n)) > 1e-8f) { - //printf("swapped %d\n",i); - U.swapRows(i, n); - P.swapRows(i, n); - } - } - } - -#ifdef MATRIX_ASSERT - //printf("A:\n"); A.print(); - //printf("U:\n"); U.print(); - //printf("P:\n"); P.print(); - //fflush(stdout); - ASSERT(fabsf(U(n, n)) > 1e-8f); -#endif - - // failsafe, return zero matrix - if (fabsf(U(n, n)) < 1e-8f) { - return Matrix::zero(n); - } - - // for all rows below diagonal - for (size_t i = (n + 1); i < N; i++) { - L(i, n) = U(i, n) / U(n, n); - - // add i-th row and n-th row - // multiplied by: -a(i,n)/a(n,n) - for (size_t k = n; k < N; k++) { - U(i, k) -= L(i, n) * U(n, k); - } - } - } - - //printf("L:\n"); L.print(); - //printf("U:\n"); U.print(); - - // solve LY=P*I for Y by forward subst - Matrix Y = P; - - // for all columns of Y - for (size_t c = 0; c < N; c++) { - // for all rows of L - for (size_t i = 0; i < N; i++) { - // for all columns of L - for (size_t j = 0; j < i; j++) { - // for all existing y - // subtract the component they - // contribute to the solution - Y(i, c) -= L(i, j) * Y(j, c); - } - - // divide by the factor - // on current - // term to be solved - // Y(i,c) /= L(i,i); - // but L(i,i) = 1.0 - } - } - - //printf("Y:\n"); Y.print(); - - // solve Ux=y for x by back subst - Matrix X = Y; - - // for all columns of X - for (size_t c = 0; c < N; c++) { - // for all rows of U - for (size_t k = 0; k < N; k++) { - // have to go in reverse order - size_t i = N - 1 - k; - - // for all columns of U - for (size_t j = i + 1; j < N; j++) { - // for all existing x - // subtract the component they - // contribute to the solution - X(i, c) -= U(i, j) * X(j, c); - } - - // divide by the factor - // on current - // term to be solved - X(i, c) /= U(i, i); - } - } - - //printf("X:\n"); X.print(); - return X; - } - inline void setAll(const float &val) { - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - (*this)(i, j) = val; - } - } - } - inline void set(const float *data) { - memcpy(getData(), data, getSize()); - } - inline size_t getRows() const { return _rows; } - inline size_t getCols() const { return _cols; } - inline static Matrix identity(size_t size) { - Matrix result(size, size); - - for (size_t i = 0; i < size; i++) { - result(i, i) = 1.0f; - } - - return result; - } - inline static Matrix zero(size_t size) { - Matrix result(size, size); - result.setAll(0.0f); - return result; - } - inline static Matrix zero(size_t m, size_t n) { - Matrix result(m, n); - result.setAll(0.0f); - return result; - } -protected: - inline size_t getSize() const { return sizeof(float) * getRows() * getCols(); } - inline float *getData() { return _data; } - inline const float *getData() const { return _data; } - inline void setData(float *data) { _data = data; } -private: - size_t _rows; - size_t _cols; - float *_data; -}; - -} // namespace math diff --git a/src/lib/mathlib/math/generic/Vector.hpp b/src/lib/mathlib/math/generic/Vector.hpp deleted file mode 100644 index 8cfdc676d7..0000000000 --- a/src/lib/mathlib/math/generic/Vector.hpp +++ /dev/null @@ -1,245 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Vector.h - * - * math vector - */ - -#pragma once - -#include -#include -#include -#include -#include -#include - -#include "../Vector.hpp" - -namespace math -{ - -class __EXPORT Vector -{ -public: - // constructor - Vector(size_t rows) : - _rows(rows), - _data((float *)calloc(rows, sizeof(float))) { - } - Vector(size_t rows, const float *data) : - _rows(rows), - _data((float *)malloc(getSize())) { - memcpy(getData(), data, getSize()); - } - // deconstructor - virtual ~Vector() { - delete [] getData(); - } - // copy constructor (deep) - Vector(const Vector &right) : - _rows(right.getRows()), - _data((float *)malloc(getSize())) { - memcpy(getData(), right.getData(), - right.getSize()); - } - // assignment - inline Vector &operator=(const Vector &right) { -#ifdef VECTOR_ASSERT - ASSERT(getRows() == right.getRows()); -#endif - - if (this != &right) { - memcpy(getData(), right.getData(), - right.getSize()); - } - - return *this; - } - // element accessors - inline float &operator()(size_t i) { -#ifdef VECTOR_ASSERT - ASSERT(i < getRows()); -#endif - return getData()[i]; - } - inline const float &operator()(size_t i) const { -#ifdef VECTOR_ASSERT - ASSERT(i < getRows()); -#endif - return getData()[i]; - } - // output - inline void print() const { - for (size_t i = 0; i < getRows(); i++) { - float sig; - int exp; - float num = (*this)(i); - float2SigExp(num, sig, exp); - printf("%6.3fe%03.3d,", (double)sig, exp); - } - - printf("\n"); - } - // boolean ops - inline bool operator==(const Vector &right) const { - for (size_t i = 0; i < getRows(); i++) { - if (fabsf(((*this)(i) - right(i))) > 1e-30f) - return false; - } - - return true; - } - // scalar ops - inline Vector operator+(const float &right) const { - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - result(i) = (*this)(i) + right; - } - - return result; - } - inline Vector operator-(const float &right) const { - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - result(i) = (*this)(i) - right; - } - - return result; - } - inline Vector operator*(const float &right) const { - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - result(i) = (*this)(i) * right; - } - - return result; - } - inline Vector operator/(const float &right) const { - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - result(i) = (*this)(i) / right; - } - - return result; - } - // vector ops - inline Vector operator+(const Vector &right) const { -#ifdef VECTOR_ASSERT - ASSERT(getRows() == right.getRows()); -#endif - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - result(i) = (*this)(i) + right(i); - } - - return result; - } - inline Vector operator-(const Vector &right) const { -#ifdef VECTOR_ASSERT - ASSERT(getRows() == right.getRows()); -#endif - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - result(i) = (*this)(i) - right(i); - } - - return result; - } - inline Vector operator-(void) const { - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - result(i) = -((*this)(i)); - } - - return result; - } - // other functions - inline float dot(const Vector &right) const { - float result = 0; - - for (size_t i = 0; i < getRows(); i++) { - result += (*this)(i) * (*this)(i); - } - - return result; - } - inline float norm() const { - return sqrtf(dot(*this)); - } - inline float length() const { - return norm(); - } - inline Vector unit() const { - return (*this) / norm(); - } - inline Vector normalized() const { - return unit(); - } - inline void normalize() { - (*this) = (*this) / norm(); - } - inline static Vector zero(size_t rows) { - Vector result(rows); - // calloc returns zeroed memory - return result; - } - inline void setAll(const float &val) { - for (size_t i = 0; i < getRows(); i++) { - (*this)(i) = val; - } - } - inline void set(const float *data) { - memcpy(getData(), data, getSize()); - } - inline size_t getRows() const { return _rows; } -protected: - inline size_t getSize() const { return sizeof(float) * getRows(); } - inline float *getData() { return _data; } - inline const float *getData() const { return _data; } - inline void setData(float *data) { _data = data; } -private: - size_t _rows; - float *_data; -}; - -} // math diff --git a/src/lib/mathlib/mathlib.h b/src/lib/mathlib/mathlib.h index 40ffb22bc4..9e03855c52 100644 --- a/src/lib/mathlib/mathlib.h +++ b/src/lib/mathlib/mathlib.h @@ -41,13 +41,9 @@ #pragma once -#include "math/Dcm.hpp" -#include "math/EulerAngles.hpp" +#include "math/Vector.hpp" #include "math/Matrix.hpp" #include "math/Quaternion.hpp" -#include "math/Vector.hpp" -#include "math/Vector3.hpp" -#include "math/Vector2f.hpp" #include "math/Limits.hpp" #endif @@ -56,4 +52,4 @@ #include "CMSIS/Include/arm_math.h" -#endif \ No newline at end of file +#endif diff --git a/src/lib/mathlib/module.mk b/src/lib/mathlib/module.mk index 72bc7db8a1..191e2da739 100644 --- a/src/lib/mathlib/module.mk +++ b/src/lib/mathlib/module.mk @@ -35,13 +35,6 @@ # Math library # SRCS = math/test/test.cpp \ - math/Vector.cpp \ - math/Vector2f.cpp \ - math/Vector3.cpp \ - math/EulerAngles.cpp \ - math/Quaternion.cpp \ - math/Dcm.cpp \ - math/Matrix.cpp \ math/Limits.cpp # @@ -49,13 +42,3 @@ SRCS = math/test/test.cpp \ # current makefile name, since app.mk needs it. # APP_MAKEFILE := $(lastword $(MAKEFILE_LIST)) - -ifeq ($(CONFIG_ARCH_CORTEXM4)$(CONFIG_ARCH_FPU),yy) -INCLUDE_DIRS += math/arm -SRCS += math/arm/Vector.cpp \ - math/arm/Matrix.cpp -else -#INCLUDE_DIRS += math/generic -#SRCS += math/generic/Vector.cpp \ -# math/generic/Matrix.cpp -endif diff --git a/src/modules/sdlog2/sdlog2_version.h b/src/lib/version/version.h similarity index 95% rename from src/modules/sdlog2/sdlog2_version.h rename to src/lib/version/version.h index c6a9ba6387..af733aaf06 100644 --- a/src/modules/sdlog2/sdlog2_version.h +++ b/src/lib/version/version.h @@ -33,15 +33,15 @@ ****************************************************************************/ /** - * @file sdlog2_version.h + * @file version.h * * Tools for system version detection. * * @author Anton Babushkin */ -#ifndef SDLOG2_VERSION_H_ -#define SDLOG2_VERSION_H_ +#ifndef VERSION_H_ +#define VERSION_H_ /* GIT_VERSION is defined at build time via a Makefile call to the @@ -59,4 +59,4 @@ #define HW_ARCH "PX4FMU_V2" #endif -#endif /* SDLOG2_VERSION_H_ */ +#endif /* VERSION_H_ */ diff --git a/src/mainpage.dox b/src/mainpage.dox new file mode 100644 index 0000000000..7ca410341e --- /dev/null +++ b/src/mainpage.dox @@ -0,0 +1,9 @@ +/** +\mainpage PX4 Autopilot Flight Control Stack and Middleware + +This software repository offers a middleware for micro aerial vehicles capable of running efficiently on a 168 MHz Cortex M4F processor and a state of the art flight control stack supporting multicopter and fixed wing aircraft. It can be easily used for experimental air (and ground) vehicles as well, as the application on a spherical blimp shows. + +http://pixhawk.org + + +*/ \ No newline at end of file diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp index ecca04dd7f..83145ac72d 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -53,21 +53,6 @@ static const int8_t ret_error = -1; // error occurred KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : SuperBlock(parent, name), - // ekf matrices - F(9, 9), - G(9, 6), - P(9, 9), - P0(9, 9), - V(6, 6), - // attitude measurement ekf matrices - HAtt(4, 9), - RAtt(4, 4), - // position measurement ekf matrices - HPos(6, 9), - RPos(6, 6), - // attitude representations - C_nb(), - q(), // subscriptions _sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz _gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 100), // limit to 10 Hz @@ -112,8 +97,17 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : { using namespace math; + F.zero(); + G.zero(); + V.zero(); + HAtt.zero(); + RAtt.zero(); + HPos.zero(); + RPos.zero(); + // initial state covariance matrix - P0 = Matrix::identity(9) * 0.01f; + P0.identity(); + P0 *= 0.01f; P = P0; // initial state @@ -138,7 +132,7 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : _sensors.magnetometer_ga[2]); // initialize dcm - C_nb = Dcm(q); + C_nb = q.to_dcm(); // HPos is constant HPos(0, 3) = 1.0f; @@ -228,8 +222,8 @@ void KalmanNav::update() if (correctAtt() == ret_ok) _attitudeInitCounter++; if (_attitudeInitCounter > 100) { - warnx("initialized EKF attitude\n"); - warnx("phi: %8.4f, theta: %8.4f, psi: %8.4f\n", + warnx("initialized EKF attitude"); + warnx("phi: %8.4f, theta: %8.4f, psi: %8.4f", double(phi), double(theta), double(psi)); _attitudeInitialized = true; } @@ -259,8 +253,8 @@ void KalmanNav::update() // lat/lon and not have init map_projection_init(lat0, lon0); _positionInitialized = true; - warnx("initialized EKF state with GPS\n"); - warnx("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n", + warnx("initialized EKF state with GPS"); + warnx("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f", double(vN), double(vE), double(vD), lat, lon, double(alt)); } @@ -321,13 +315,12 @@ void KalmanNav::updatePublications() _pos.timestamp = _pubTimeStamp; _pos.time_gps_usec = _gps.timestamp_position; _pos.valid = true; - _pos.lat = getLatDegE7(); - _pos.lon = getLonDegE7(); + _pos.lat = lat * M_RAD_TO_DEG; + _pos.lon = lon * M_RAD_TO_DEG; _pos.alt = float(alt); - _pos.relative_alt = float(alt); // TODO, make relative - _pos.vx = vN; - _pos.vy = vE; - _pos.vz = vD; + _pos.vel_n = vN; + _pos.vel_e = vE; + _pos.vel_d = vD; _pos.yaw = psi; // local position publication @@ -404,28 +397,28 @@ int KalmanNav::predictState(float dt) // attitude prediction if (_attitudeInitialized) { - Vector3 w(_sensors.gyro_rad_s); + Vector<3> w(_sensors.gyro_rad_s); // attitude q = q + q.derivative(w) * dt; // renormalize quaternion if needed - if (fabsf(q.norm() - 1.0f) > 1e-4f) { - q = q.unit(); + if (fabsf(q.length() - 1.0f) > 1e-4f) { + q.normalize(); } // C_nb update - C_nb = Dcm(q); + C_nb = q.to_dcm(); // euler update - EulerAngles euler(C_nb); - phi = euler.getPhi(); - theta = euler.getTheta(); - psi = euler.getPsi(); + Vector<3> euler = C_nb.to_euler(); + phi = euler.data[0]; + theta = euler.data[1]; + psi = euler.data[2]; // specific acceleration in nav frame - Vector3 accelB(_sensors.accelerometer_m_s2); - Vector3 accelN = C_nb * accelB; + Vector<3> accelB(_sensors.accelerometer_m_s2); + Vector<3> accelN = C_nb * accelB; fN = accelN(0); fE = accelN(1); fD = accelN(2); @@ -549,10 +542,10 @@ int KalmanNav::predictStateCovariance(float dt) G(5, 4) = C_nb(2, 1); G(5, 5) = C_nb(2, 2); - // continuous predictioon equations - // for discrte time EKF + // continuous prediction equations + // for discrete time EKF // http://en.wikipedia.org/wiki/Extended_Kalman_filter - P = P + (F * P + P * F.transpose() + G * V * G.transpose()) * dt; + P = P + (F * P + P * F.transposed() + G * V * G.transposed()) * dt; return ret_ok; } @@ -577,13 +570,14 @@ int KalmanNav::correctAtt() // compensate roll and pitch, but not yaw // XXX take the vectors out of the C_nb matrix to avoid singularities - math::Dcm C_rp(math::EulerAngles(phi, theta, 0.0f));//C_nb.transpose(); + math::Matrix<3,3> C_rp; + C_rp.from_euler(phi, theta, 0.0f);//C_nb.transposed(); // mag measurement - Vector3 magBody(_sensors.magnetometer_ga); + Vector<3> magBody(_sensors.magnetometer_ga); // transform to earth frame - Vector3 magNav = C_rp * magBody; + Vector<3> magNav = C_rp * magBody; // calculate error between estimate and measurement // apply declination correction for true heading as well. @@ -592,12 +586,12 @@ int KalmanNav::correctAtt() if (yMag < -M_PI_F) yMag += 2*M_PI_F; // accel measurement - Vector3 zAccel(_sensors.accelerometer_m_s2); - float accelMag = zAccel.norm(); - zAccel = zAccel.unit(); + Vector<3> zAccel(_sensors.accelerometer_m_s2); + float accelMag = zAccel.length(); + zAccel.normalize(); // ignore accel correction when accel mag not close to g - Matrix RAttAdjust = RAtt; + Matrix<4,4> RAttAdjust = RAtt; bool ignoreAccel = fabsf(accelMag - _g.get()) > 1.1f; @@ -611,14 +605,10 @@ int KalmanNav::correctAtt() } // accel predicted measurement - Vector3 zAccelHat = (C_nb.transpose() * Vector3(0, 0, -_g.get())).unit(); + Vector<3> zAccelHat = (C_nb.transposed() * Vector<3>(0, 0, -_g.get())).normalized(); // calculate residual - Vector y(4); - y(0) = yMag; - y(1) = zAccel(0) - zAccelHat(0); - y(2) = zAccel(1) - zAccelHat(1); - y(3) = zAccel(2) - zAccelHat(2); + Vector<4> y(yMag, zAccel(0) - zAccelHat(0), zAccel(1) - zAccelHat(1), zAccel(2) - zAccelHat(2)); // HMag HAtt(0, 2) = 1; @@ -632,17 +622,17 @@ int KalmanNav::correctAtt() // compute correction // http://en.wikipedia.org/wiki/Extended_Kalman_filter - Matrix S = HAtt * P * HAtt.transpose() + RAttAdjust; // residual covariance - Matrix K = P * HAtt.transpose() * S.inverse(); - Vector xCorrect = K * y; + Matrix<4, 4> S = HAtt * P * HAtt.transposed() + RAttAdjust; // residual covariance + Matrix<9, 4> K = P * HAtt.transposed() * S.inversed(); + Vector<9> xCorrect = K * y; // check correciton is sane - for (size_t i = 0; i < xCorrect.getRows(); i++) { + for (size_t i = 0; i < xCorrect.get_size(); i++) { float val = xCorrect(i); if (isnan(val) || isinf(val)) { // abort correction and return - warnx("numerical failure in att correction\n"); + warnx("numerical failure in att correction"); // reset P matrix to P0 P = P0; return ret_error; @@ -669,7 +659,7 @@ int KalmanNav::correctAtt() P = P - K * HAtt * P; // fault detection - float beta = y.dot(S.inverse() * y); + float beta = y * (S.inversed() * y); if (beta > _faultAtt.get()) { warnx("fault in attitude: beta = %8.4f", (double)beta); @@ -678,7 +668,7 @@ int KalmanNav::correctAtt() // update quaternions from euler // angle correction - q = Quaternion(EulerAngles(phi, theta, psi)); + q.from_euler(phi, theta, psi); return ret_ok; } @@ -688,7 +678,7 @@ int KalmanNav::correctPos() using namespace math; // residual - Vector y(6); + Vector<6> y; y(0) = _gps.vel_n_m_s - vN; y(1) = _gps.vel_e_m_s - vE; y(2) = double(_gps.lat) - double(lat) * 1.0e7 * M_RAD_TO_DEG; @@ -698,17 +688,17 @@ int KalmanNav::correctPos() // compute correction // http://en.wikipedia.org/wiki/Extended_Kalman_filter - Matrix S = HPos * P * HPos.transpose() + RPos; // residual covariance - Matrix K = P * HPos.transpose() * S.inverse(); - Vector xCorrect = K * y; + Matrix<6,6> S = HPos * P * HPos.transposed() + RPos; // residual covariance + Matrix<9,6> K = P * HPos.transposed() * S.inversed(); + Vector<9> xCorrect = K * y; // check correction is sane - for (size_t i = 0; i < xCorrect.getRows(); i++) { + for (size_t i = 0; i < xCorrect.get_size(); i++) { float val = xCorrect(i); if (!isfinite(val)) { // abort correction and return - warnx("numerical failure in gps correction\n"); + warnx("numerical failure in gps correction"); // fallback to GPS vN = _gps.vel_n_m_s; vE = _gps.vel_e_m_s; @@ -735,7 +725,7 @@ int KalmanNav::correctPos() P = P - K * HPos * P; // fault detetcion - float beta = y.dot(S.inverse() * y); + float beta = y * (S.inversed() * y); static int counter = 0; if (beta > _faultPos.get() && (counter % 10 == 0)) { diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp index a69bde1a65..46ee4b6c8e 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp @@ -125,17 +125,17 @@ public: virtual void updateParams(); protected: // kalman filter - math::Matrix F; /**< Jacobian(f,x), where dx/dt = f(x,u) */ - math::Matrix G; /**< noise shaping matrix for gyro/accel */ - math::Matrix P; /**< state covariance matrix */ - math::Matrix P0; /**< initial state covariance matrix */ - math::Matrix V; /**< gyro/ accel noise matrix */ - math::Matrix HAtt; /**< attitude measurement matrix */ - math::Matrix RAtt; /**< attitude measurement noise matrix */ - math::Matrix HPos; /**< position measurement jacobian matrix */ - math::Matrix RPos; /**< position measurement noise matrix */ + math::Matrix<9,9> F; /**< Jacobian(f,x), where dx/dt = f(x,u) */ + math::Matrix<9,6> G; /**< noise shaping matrix for gyro/accel */ + math::Matrix<9,9> P; /**< state covariance matrix */ + math::Matrix<9,9> P0; /**< initial state covariance matrix */ + math::Matrix<6,6> V; /**< gyro/ accel noise matrix */ + math::Matrix<4,9> HAtt; /**< attitude measurement matrix */ + math::Matrix<4,4> RAtt; /**< attitude measurement noise matrix */ + math::Matrix<6,9> HPos; /**< position measurement jacobian matrix */ + math::Matrix<6,6> RPos; /**< position measurement noise matrix */ // attitude - math::Dcm C_nb; /**< direction cosine matrix from body to nav frame */ + math::Matrix<3,3> C_nb; /**< direction cosine matrix from body to nav frame */ math::Quaternion q; /**< quaternion from body to nav frame */ // subscriptions control::UOrbSubscription _sensors; /**< sensors sub. */ diff --git a/src/modules/att_pos_estimator_ekf/kalman_main.cpp b/src/modules/att_pos_estimator_ekf/kalman_main.cpp index 372b2d162c..3d20d4d2d6 100644 --- a/src/modules/att_pos_estimator_ekf/kalman_main.cpp +++ b/src/modules/att_pos_estimator_ekf/kalman_main.cpp @@ -107,7 +107,7 @@ int att_pos_estimator_ekf_main(int argc, char *argv[]) daemon_task = task_spawn_cmd("att_pos_estimator_ekf", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 30, - 4096, + 8192, kalman_demo_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index a70a14fe4b..66ec20b954 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -58,9 +58,13 @@ #include #include #include +#include +#include #include #include +#include + #include #include #include @@ -214,6 +218,10 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds struct sensor_combined_s raw; memset(&raw, 0, sizeof(raw)); + struct vehicle_gps_position_s gps; + memset(&gps, 0, sizeof(gps)); + struct vehicle_global_position_s global_pos; + memset(&global_pos, 0, sizeof(global_pos)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); struct vehicle_control_mode_s control_mode; @@ -221,12 +229,32 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds uint64_t last_data = 0; uint64_t last_measurement = 0; + uint64_t last_vel_t = 0; + + /* current velocity */ + math::Vector<3> vel; + vel.zero(); + /* previous velocity */ + math::Vector<3> vel_prev; + vel_prev.zero(); + /* actual acceleration (by GPS velocity) in body frame */ + math::Vector<3> acc; + acc.zero(); + /* rotation matrix */ + math::Matrix<3, 3> R; + R.identity(); /* subscribe to raw data */ int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); /* rate-limit raw data updates to 333 Hz (sensors app publishes at 200, so this is just paranoid) */ orb_set_interval(sub_raw, 3); + /* subscribe to GPS */ + int sub_gps = orb_subscribe(ORB_ID(vehicle_gps_position)); + + /* subscribe to GPS */ + int sub_global_pos = orb_subscribe(ORB_ID(vehicle_global_position)); + /* subscribe to param changes */ int sub_params = orb_subscribe(ORB_ID(parameter_update)); @@ -265,6 +293,10 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f }; unsigned offset_count = 0; + /* rotation matrix for magnetic declination */ + math::Matrix<3, 3> R_decl; + R_decl.identity(); + /* register the perf counter */ perf_counter_t ekf_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_ekf"); @@ -299,6 +331,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* update parameters */ parameters_update(&ekf_param_handles, &ekf_params); + + /* update mag declination rotation matrix */ + R_decl.from_euler(0.0f, 0.0f, ekf_params.mag_decl); } /* only run filter if sensor values changed */ @@ -307,6 +342,18 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* get latest measurements */ orb_copy(ORB_ID(sensor_combined), sub_raw, &raw); + bool gps_updated; + orb_check(sub_gps, &gps_updated); + if (gps_updated) { + orb_copy(ORB_ID(vehicle_gps_position), sub_gps, &gps); + } + + bool global_pos_updated; + orb_check(sub_global_pos, &global_pos_updated); + if (global_pos_updated) { + orb_copy(ORB_ID(vehicle_global_position), sub_global_pos, &global_pos); + } + if (!initialized) { // XXX disabling init for now initialized = true; @@ -352,9 +399,50 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds sensor_last_timestamp[1] = raw.timestamp; } - z_k[3] = raw.accelerometer_m_s2[0]; - z_k[4] = raw.accelerometer_m_s2[1]; - z_k[5] = raw.accelerometer_m_s2[2]; + hrt_abstime vel_t = 0; + bool vel_valid = false; + if (ekf_params.acc_comp == 1 && gps.fix_type >= 3 && gps.eph_m < 10.0f && gps.vel_ned_valid && hrt_absolute_time() < gps.timestamp_velocity + 500000) { + vel_valid = true; + if (gps_updated) { + vel_t = gps.timestamp_velocity; + vel(0) = gps.vel_n_m_s; + vel(1) = gps.vel_e_m_s; + vel(2) = gps.vel_d_m_s; + } + + } else if (ekf_params.acc_comp == 2 && global_pos.valid && hrt_absolute_time() < global_pos.timestamp + 500000) { + vel_valid = true; + if (global_pos_updated) { + vel_t = global_pos.timestamp; + vel(0) = global_pos.vel_n; + vel(1) = global_pos.vel_e; + vel(2) = global_pos.vel_d; + } + } + + if (vel_valid) { + /* velocity is valid */ + if (vel_t != 0) { + /* velocity updated */ + if (last_vel_t != 0 && vel_t != last_vel_t) { + float vel_dt = (vel_t - last_vel_t) / 1000000.0f; + /* calculate acceleration in body frame */ + acc = R.transposed() * ((vel - vel_prev) / vel_dt); + } + last_vel_t = vel_t; + vel_prev = vel; + } + + } else { + /* velocity is valid, reset acceleration */ + acc.zero(); + vel_prev.zero(); + last_vel_t = 0; + } + + z_k[3] = raw.accelerometer_m_s2[0] - acc(0); + z_k[4] = raw.accelerometer_m_s2[1] - acc(1); + z_k[5] = raw.accelerometer_m_s2[2] - acc(2); /* update magnetometer measurements */ if (sensor_last_count[2] != raw.magnetometer_counter) { @@ -425,7 +513,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds continue; } - if (last_data > 0 && raw.timestamp - last_data > 12000) + if (last_data > 0 && raw.timestamp - last_data > 30000) printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data); last_data = raw.timestamp; @@ -433,10 +521,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* send out */ att.timestamp = raw.timestamp; - // XXX Apply the same transformation to the rotation matrix - att.roll = euler[0] - ekf_params.roll_off; - att.pitch = euler[1] - ekf_params.pitch_off; - att.yaw = euler[2] - ekf_params.yaw_off; + att.roll = euler[0]; + att.pitch = euler[1]; + att.yaw = euler[2] + ekf_params.mag_decl; att.rollspeed = x_aposteriori[0]; att.pitchspeed = x_aposteriori[1]; @@ -445,12 +532,20 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds att.pitchacc = x_aposteriori[4]; att.yawacc = x_aposteriori[5]; - //att.yawspeed =z_k[2] ; + att.g_comp[0] = raw.accelerometer_m_s2[0] - acc(0); + att.g_comp[1] = raw.accelerometer_m_s2[1] - acc(1); + att.g_comp[2] = raw.accelerometer_m_s2[2] - acc(2); + /* copy offsets */ memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets)); + /* magnetic declination */ + + math::Matrix<3, 3> R_body = (&Rot_matrix[0]); + R = R_decl * R_body; + /* copy rotation matrix */ - memcpy(&att.R, Rot_matrix, sizeof(Rot_matrix)); + memcpy(&att.R[0][0], &R.data[0][0], sizeof(att.R)); att.R_valid = true; if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) { diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c index 3cfddf28ed..44f47b47ca 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -65,6 +65,11 @@ PARAM_DEFINE_FLOAT(ATT_ROLL_OFF3, 0.0f); PARAM_DEFINE_FLOAT(ATT_PITCH_OFF3, 0.0f); PARAM_DEFINE_FLOAT(ATT_YAW_OFF3, 0.0f); +/* magnetic declination, in degrees */ +PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f); + +PARAM_DEFINE_INT32(ATT_ACC_COMP, 0); + int parameters_init(struct attitude_estimator_ekf_param_handles *h) { /* PID parameters */ @@ -83,6 +88,10 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h) h->pitch_off = param_find("ATT_PITCH_OFF3"); h->yaw_off = param_find("ATT_YAW_OFF3"); + h->mag_decl = param_find("ATT_MAG_DECL"); + + h->acc_comp = param_find("ATT_ACC_COMP"); + return OK; } @@ -103,5 +112,9 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru param_get(h->pitch_off, &(p->pitch_off)); param_get(h->yaw_off, &(p->yaw_off)); + param_get(h->mag_decl, &(p->mag_decl)); + + param_get(h->acc_comp, &(p->acc_comp)); + return OK; } diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h index 09817d58e6..74a1416091 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h @@ -47,12 +47,16 @@ struct attitude_estimator_ekf_params { float roll_off; float pitch_off; float yaw_off; + float mag_decl; + int acc_comp; }; struct attitude_estimator_ekf_param_handles { param_t r0, r1, r2, r3; param_t q0, q1, q2, q3, q4; param_t roll_off, pitch_off, yaw_off; + param_t mag_decl; + param_t acc_comp; }; /** diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 5eeca5a1a3..36b75dd58a 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -194,15 +194,13 @@ int do_accel_calibration(int mavlink_fd) int32_t board_rotation_int; param_get(board_rotation_h, &(board_rotation_int)); enum Rotation board_rotation_id = (enum Rotation)board_rotation_int; - math::Matrix board_rotation(3, 3); + math::Matrix<3,3> board_rotation; get_rot_matrix(board_rotation_id, &board_rotation); - math::Matrix board_rotation_t = board_rotation.transpose(); - math::Vector3 accel_offs_vec; - accel_offs_vec.set(&accel_offs[0]); - math::Vector3 accel_offs_rotated = board_rotation_t * accel_offs_vec; - math::Matrix accel_T_mat(3, 3); - accel_T_mat.set(&accel_T[0][0]); - math::Matrix accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation; + math::Matrix<3,3> board_rotation_t = board_rotation.transposed(); + math::Vector<3> accel_offs_vec(&accel_offs[0]); + math::Vector<3> accel_offs_rotated = board_rotation_t * accel_offs_vec; + math::Matrix<3,3> accel_T_mat(&accel_T[0][0]); + math::Matrix<3,3> accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation; accel_scale.x_offset = accel_offs_rotated(0); accel_scale.x_scale = accel_T_rotated(0, 0); diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 1e53181217..f579fb52ad 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -106,14 +106,9 @@ static const int ERROR = -1; extern struct system_load_s system_load; -#define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f -#define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f - /* Decouple update interval and hysteris counters, all depends on intervals */ #define COMMANDER_MONITORING_INTERVAL 50000 #define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f)) -#define LOW_VOLTAGE_BATTERY_COUNTER_LIMIT (LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) -#define CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT (CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) #define MAVLINK_OPEN_INTERVAL 50000 @@ -204,9 +199,9 @@ void control_status_leds(vehicle_status_s *status, const actuator_armed_s *actua void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed); -void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status); +void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status); -transition_result_t check_main_state_machine(struct vehicle_status_s *current_status); +transition_result_t set_main_state_rc(struct vehicle_status_s *status); void print_reject_mode(const char *msg); @@ -376,6 +371,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe mavlink_log_info(mavlink_fd, "[cmd] HIL: FAILED resetting armed state"); } } + if (hil_ret == OK) ret = true; @@ -410,6 +406,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe arming_res = TRANSITION_NOT_CHANGED; } } + if (arming_res == TRANSITION_CHANGED) ret = true; @@ -452,6 +449,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } } } + if (main_res == TRANSITION_CHANGED) ret = true; @@ -491,8 +489,9 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe break; case VEHICLE_CMD_OVERRIDE_GOTO: { - // TODO listen vehicle_command topic directly from navigator (?) + // TODO listen vehicle_command topic directly from navigator (?) unsigned int mav_goto = cmd->param1; + if (mav_goto == 0) { // MAV_GOTO_DO_HOLD status->set_nav_state = NAV_STATE_LOITER; status->set_nav_state_timestamp = hrt_absolute_time(); @@ -513,11 +512,11 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } break; - /* Flight termination */ + /* Flight termination */ case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command if (armed->armed && cmd->param3 > 0.5) { //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO - transition_result_t flighttermination_res = flighttermination_state_transition(status, FLIGHTTERMINATION_STATE_ON); + transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION); result = VEHICLE_CMD_RESULT_ACCEPTED; ret = true; @@ -566,7 +565,6 @@ int commander_thread_main(int argc, char *argv[]) { /* not yet initialized */ commander_initialized = false; - bool home_position_set = false; bool battery_tune_played = false; bool arm_tune_played = false; @@ -580,6 +578,27 @@ int commander_thread_main(int argc, char *argv[]) /* welcome user */ warnx("starting"); + char *main_states_str[MAIN_STATE_MAX]; + main_states_str[0] = "MANUAL"; + main_states_str[1] = "SEATBELT"; + main_states_str[2] = "EASY"; + main_states_str[3] = "AUTO"; + + char *arming_states_str[ARMING_STATE_MAX]; + arming_states_str[0] = "INIT"; + arming_states_str[1] = "STANDBY"; + arming_states_str[2] = "ARMED"; + arming_states_str[3] = "ARMED_ERROR"; + arming_states_str[4] = "STANDBY_ERROR"; + arming_states_str[5] = "REBOOT"; + arming_states_str[6] = "IN_AIR_RESTORE"; + + char *failsafe_states_str[FAILSAFE_STATE_MAX]; + failsafe_states_str[0] = "NORMAL"; + failsafe_states_str[1] = "RTL"; + failsafe_states_str[2] = "LAND"; + failsafe_states_str[3] = "TERMINATION"; + /* pthread for slow low prio thread */ pthread_t commander_low_prio_thread; @@ -609,6 +628,7 @@ int commander_thread_main(int argc, char *argv[]) status.set_nav_state_timestamp = 0; status.arming_state = ARMING_STATE_INIT; status.hil_state = HIL_STATE_OFF; + status.failsafe_state = FAILSAFE_STATE_NORMAL; /* neither manual nor offboard control commands have been received */ status.offboard_control_signal_found_once = false; @@ -666,8 +686,6 @@ int commander_thread_main(int argc, char *argv[]) /* Start monitoring loop */ unsigned counter = 0; - unsigned low_voltage_counter = 0; - unsigned critical_voltage_counter = 0; unsigned stick_off_counter = 0; unsigned stick_on_counter = 0; @@ -745,7 +763,6 @@ int commander_thread_main(int argc, char *argv[]) int battery_sub = orb_subscribe(ORB_ID(battery_status)); struct battery_status_s battery; memset(&battery, 0, sizeof(battery)); - battery.voltage_v = 0.0f; /* Subscribe to subsystem info topic */ int subsys_sub = orb_subscribe(ORB_ID(subsystem_info)); @@ -889,13 +906,12 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(battery_status), battery_sub, &battery); - // warnx("bat v: %2.2f", battery.voltage_v); - - /* only consider battery voltage if system has been running 2s and battery voltage is higher than 4V */ - if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_v > 4.0f) { - status.battery_voltage = battery.voltage_v; + /* only consider battery voltage if system has been running 2s and battery voltage is valid */ + if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) { + status.battery_voltage = battery.voltage_filtered_v; + status.battery_current = battery.current_a; status.condition_battery_voltage_valid = true; - status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage); + status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_filtered_v, battery.discharged_mah); } } @@ -948,46 +964,29 @@ int commander_thread_main(int argc, char *argv[]) //on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0); } - // XXX remove later - //warnx("bat remaining: %2.2f", status.battery_remaining); - /* if battery voltage is getting lower, warn using buzzer, etc. */ if (status.condition_battery_voltage_valid && status.battery_remaining < 0.25f && !low_battery_voltage_actions_done) { - //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS - if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) { - low_battery_voltage_actions_done = true; - mavlink_log_critical(mavlink_fd, "#audio: WARNING: LOW BATTERY"); - status.battery_warning = VEHICLE_BATTERY_WARNING_LOW; - status_changed = true; - battery_tune_played = false; - } - - low_voltage_counter++; + low_battery_voltage_actions_done = true; + mavlink_log_critical(mavlink_fd, "#audio: WARNING: LOW BATTERY"); + status.battery_warning = VEHICLE_BATTERY_WARNING_LOW; + status_changed = true; + battery_tune_played = false; } else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.1f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) { /* critical battery voltage, this is rather an emergency, change state machine */ - if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) { - critical_battery_voltage_actions_done = true; - mavlink_log_critical(mavlink_fd, "#audio: EMERGENCY: CRITICAL BATTERY"); - status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL; - battery_tune_played = false; + critical_battery_voltage_actions_done = true; + mavlink_log_critical(mavlink_fd, "#audio: EMERGENCY: CRITICAL BATTERY"); + status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL; + battery_tune_played = false; - if (armed.armed) { - arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); + if (armed.armed) { + arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); - } else { - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed); - } - - status_changed = true; + } else { + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed); } - critical_voltage_counter++; - - } else { - - low_voltage_counter = 0; - critical_voltage_counter = 0; + status_changed = true; } /* End battery voltage check */ @@ -1029,19 +1028,18 @@ int commander_thread_main(int argc, char *argv[]) * position to the current position. */ - if (!home_position_set && gps_position.fix_type >= 3 && - (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) && // XXX note that vdop is 0 for mtk + if (!status.condition_home_position_valid && gps_position.fix_type >= 3 && + (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) && (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed && global_position.valid) { + /* copy position data to uORB home message, store it locally as well */ + home.lat = global_position.lat; + home.lon = global_position.lon; + home.alt = global_position.alt; - - home.lat = (double)global_position.lat / 1e7d; - home.lon = (double)global_position.lon / 1e7d; - home.altitude = (float)global_position.alt; - - warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.altitude); - mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.altitude); + warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.alt); + mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.alt); /* announce new home position */ if (home_pub > 0) { @@ -1052,129 +1050,163 @@ int commander_thread_main(int argc, char *argv[]) } /* mark home position as set */ - home_position_set = true; + status.condition_home_position_valid = true; tune_positive(); } } - /* ignore RC signals if in offboard control mode */ - if (!status.offboard_control_signal_found_once && sp_man.timestamp != 0) { - /* start RC input check */ - if (hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) { - /* handle the case where RC signal was regained */ - if (!status.rc_signal_found_once) { - status.rc_signal_found_once = true; - mavlink_log_critical(mavlink_fd, "#audio: detected RC signal first time"); + /* start RC input check */ + if (sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) { + /* handle the case where RC signal was regained */ + if (!status.rc_signal_found_once) { + status.rc_signal_found_once = true; + mavlink_log_critical(mavlink_fd, "#audio: detected RC signal first time"); + status_changed = true; + + } else { + if (status.rc_signal_lost) { + mavlink_log_critical(mavlink_fd, "#audio: RC signal regained"); status_changed = true; - - } else { - if (status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "#audio: RC signal regained"); - status_changed = true; - } } + } - status.rc_signal_lost = false; + status.rc_signal_lost = false; - transition_result_t res; // store all transitions results here + transition_result_t res; // store all transitions results here - /* arm/disarm by RC */ - res = TRANSITION_NOT_CHANGED; + /* arm/disarm by RC */ + res = TRANSITION_NOT_CHANGED; - /* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSISTED mode and landed) -> disarm - * do it only for rotary wings */ - if (status.is_rotary_wing && - (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) && - (status.main_state == MAIN_STATE_MANUAL || status.condition_landed) && - sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + /* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSISTED mode and landed) -> disarm + * do it only for rotary wings */ + if (status.is_rotary_wing && + (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) && + (status.main_state == MAIN_STATE_MANUAL || status.condition_landed) && + sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { - if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ - arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - res = arming_state_transition(&status, &safety, new_arming_state, &armed); - stick_off_counter = 0; - - } else { - stick_off_counter++; - } - - } else { + if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { + /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ + arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); + res = arming_state_transition(&status, &safety, new_arming_state, &armed); stick_off_counter = 0; - } - - /* check if left stick is in lower right position and we're in MANUAL mode -> arm */ - if (status.arming_state == ARMING_STATE_STANDBY && - sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { - if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - if (safety.safety_switch_available && !safety.safety_off) { - print_reject_arm("NOT ARMING: Press safety switch first."); - - } else if (status.main_state != MAIN_STATE_MANUAL) { - print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); - - } else { - res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); - } - - stick_on_counter = 0; - - } else { - stick_on_counter++; - } } else { - stick_on_counter = 0; - } - - if (res == TRANSITION_CHANGED) { - if (status.arming_state == ARMING_STATE_ARMED) { - mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC"); - - } else { - mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC"); - } - - } else if (res == TRANSITION_DENIED) { - warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); - mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); - } - - /* fill current_status according to mode switches */ - check_mode_switches(&sp_man, &status); - - /* evaluate the main state machine */ - res = check_main_state_machine(&status); - - if (res == TRANSITION_CHANGED) { - //mavlink_log_info(mavlink_fd, "[cmd] main state: %d", status.main_state); - tune_positive(); - - } else if (res == TRANSITION_DENIED) { - /* DENIED here indicates bug in the commander */ - warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); - mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); + stick_off_counter++; } } else { - if (!status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST"); - status.rc_signal_lost = true; - status_changed = true; + stick_off_counter = 0; + } + + /* check if left stick is in lower right position and we're in MANUAL mode -> arm */ + if (status.arming_state == ARMING_STATE_STANDBY && + sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { + if (safety.safety_switch_available && !safety.safety_off) { + print_reject_arm("NOT ARMING: Press safety switch first."); + + } else if (status.main_state != MAIN_STATE_MANUAL) { + print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); + + } else { + res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); + } + + stick_on_counter = 0; + + } else { + stick_on_counter++; + } + + } else { + stick_on_counter = 0; + } + + if (res == TRANSITION_CHANGED) { + if (status.arming_state == ARMING_STATE_ARMED) { + mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC"); + + } else { + mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC"); + } + + } else if (res == TRANSITION_DENIED) { + /* DENIED here indicates bug in the commander */ + mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied"); + } + + if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { + /* recover from failsafe */ + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); + } + + /* fill status according to mode switches */ + check_mode_switches(&sp_man, &status); + + /* evaluate the main state machine according to mode switches */ + res = set_main_state_rc(&status); + + if (res == TRANSITION_CHANGED) { + tune_positive(); + + } else if (res == TRANSITION_DENIED) { + /* DENIED here indicates bug in the commander */ + mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied"); + } + + } else { + if (!status.rc_signal_lost) { + mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST"); + status.rc_signal_lost = true; + status_changed = true; + } + + if (armed.armed) { + if (status.main_state == MAIN_STATE_AUTO) { + /* check if AUTO mode still allowed */ + transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO); + + if (res == TRANSITION_DENIED) { + /* AUTO mode denied, don't try RTL, switch to failsafe state LAND */ + res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); + + if (res == TRANSITION_DENIED) { + /* LAND not allowed, set TERMINATION state */ + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); + } + } + + } else { + /* failsafe for manual modes */ + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL); + + if (res == TRANSITION_DENIED) { + /* RTL not allowed (no global position estimate), try LAND */ + res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); + + if (res == TRANSITION_DENIED) { + /* LAND not allowed, set TERMINATION state */ + res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); + } + } + } + + } else { + if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { + /* reset failsafe when disarmed */ + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); } } } - /* Flight termination in manual mode if assisted switch is on easy position //xxx hack! */ - if (armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) { - transition_result_t flighttermination_res = flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_ON); - if (flighttermination_res == TRANSITION_CHANGED) { + // TODO remove this hack + /* flight termination in manual mode if assisted switch is on easy position */ + if (!status.is_rotary_wing && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) { + if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) { tune_positive(); } - } else { - flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_OFF); } - /* handle commands last, as the system needs to be updated to handle them */ orb_check(cmd_sub, &updated); @@ -1190,13 +1222,24 @@ int commander_thread_main(int argc, char *argv[]) /* check which state machines for changes, clear "changed" flag */ bool arming_state_changed = check_arming_state_changed(); bool main_state_changed = check_main_state_changed(); - bool flighttermination_state_changed = check_flighttermination_state_changed(); + bool failsafe_state_changed = check_failsafe_state_changed(); hrt_abstime t1 = hrt_absolute_time(); - if (arming_state_changed || main_state_changed) { - mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d", status.arming_state, status.main_state); + /* print new state */ + if (arming_state_changed) { status_changed = true; + mavlink_log_info(mavlink_fd, "[cmd] arming state: %s", arming_states_str[status.arming_state]); + } + + if (main_state_changed) { + status_changed = true; + mavlink_log_info(mavlink_fd, "[cmd] main state: %s", main_states_str[status.main_state]); + } + + if (failsafe_state_changed) { + status_changed = true; + mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %s", failsafe_states_str[status.failsafe_state]); } /* publish states (armed, control mode, vehicle status) at least with 5 Hz */ @@ -1375,72 +1418,72 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a } void -check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status) +check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status) { /* main mode switch */ if (!isfinite(sp_man->mode_switch)) { warnx("mode sw not finite"); - current_status->mode_switch = MODE_SWITCH_MANUAL; + status->mode_switch = MODE_SWITCH_MANUAL; } else if (sp_man->mode_switch > STICK_ON_OFF_LIMIT) { - current_status->mode_switch = MODE_SWITCH_AUTO; + status->mode_switch = MODE_SWITCH_AUTO; } else if (sp_man->mode_switch < -STICK_ON_OFF_LIMIT) { - current_status->mode_switch = MODE_SWITCH_MANUAL; + status->mode_switch = MODE_SWITCH_MANUAL; } else { - current_status->mode_switch = MODE_SWITCH_ASSISTED; + status->mode_switch = MODE_SWITCH_ASSISTED; } /* return switch */ if (!isfinite(sp_man->return_switch)) { - current_status->return_switch = RETURN_SWITCH_NONE; + status->return_switch = RETURN_SWITCH_NONE; } else if (sp_man->return_switch > STICK_ON_OFF_LIMIT) { - current_status->return_switch = RETURN_SWITCH_RETURN; + status->return_switch = RETURN_SWITCH_RETURN; } else { - current_status->return_switch = RETURN_SWITCH_NORMAL; + status->return_switch = RETURN_SWITCH_NORMAL; } /* assisted switch */ if (!isfinite(sp_man->assisted_switch)) { - current_status->assisted_switch = ASSISTED_SWITCH_SEATBELT; + status->assisted_switch = ASSISTED_SWITCH_SEATBELT; } else if (sp_man->assisted_switch > STICK_ON_OFF_LIMIT) { - current_status->assisted_switch = ASSISTED_SWITCH_EASY; + status->assisted_switch = ASSISTED_SWITCH_EASY; } else { - current_status->assisted_switch = ASSISTED_SWITCH_SEATBELT; + status->assisted_switch = ASSISTED_SWITCH_SEATBELT; } /* mission switch */ if (!isfinite(sp_man->mission_switch)) { - current_status->mission_switch = MISSION_SWITCH_NONE; + status->mission_switch = MISSION_SWITCH_NONE; } else if (sp_man->mission_switch > STICK_ON_OFF_LIMIT) { - current_status->mission_switch = MISSION_SWITCH_LOITER; + status->mission_switch = MISSION_SWITCH_LOITER; } else { - current_status->mission_switch = MISSION_SWITCH_MISSION; + status->mission_switch = MISSION_SWITCH_MISSION; } } transition_result_t -check_main_state_machine(struct vehicle_status_s *current_status) +set_main_state_rc(struct vehicle_status_s *status) { /* evaluate the main state machine */ transition_result_t res = TRANSITION_DENIED; - switch (current_status->mode_switch) { + switch (status->mode_switch) { case MODE_SWITCH_MANUAL: - res = main_state_transition(current_status, MAIN_STATE_MANUAL); + res = main_state_transition(status, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; case MODE_SWITCH_ASSISTED: - if (current_status->assisted_switch == ASSISTED_SWITCH_EASY) { - res = main_state_transition(current_status, MAIN_STATE_EASY); + if (status->assisted_switch == ASSISTED_SWITCH_EASY) { + res = main_state_transition(status, MAIN_STATE_EASY); if (res != TRANSITION_DENIED) break; // changed successfully or already in this state @@ -1449,34 +1492,34 @@ check_main_state_machine(struct vehicle_status_s *current_status) print_reject_mode("EASY"); } - res = main_state_transition(current_status, MAIN_STATE_SEATBELT); + res = main_state_transition(status, MAIN_STATE_SEATBELT); if (res != TRANSITION_DENIED) break; // changed successfully or already in this mode - if (current_status->assisted_switch != ASSISTED_SWITCH_EASY) // don't print both messages + if (status->assisted_switch != ASSISTED_SWITCH_EASY) // don't print both messages print_reject_mode("SEATBELT"); // else fallback to MANUAL - res = main_state_transition(current_status, MAIN_STATE_MANUAL); + res = main_state_transition(status, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; case MODE_SWITCH_AUTO: - res = main_state_transition(current_status, MAIN_STATE_AUTO); + res = main_state_transition(status, MAIN_STATE_AUTO); if (res != TRANSITION_DENIED) break; // changed successfully or already in this state // else fallback to SEATBELT (EASY likely will not work too) print_reject_mode("AUTO"); - res = main_state_transition(current_status, MAIN_STATE_SEATBELT); + res = main_state_transition(status, MAIN_STATE_SEATBELT); if (res != TRANSITION_DENIED) break; // changed successfully or already in this state // else fallback to MANUAL - res = main_state_transition(current_status, MAIN_STATE_MANUAL); + res = main_state_transition(status, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 565b4b66ab..21a1c4c2c7 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -44,6 +44,7 @@ #include #include #include +#include #include #include @@ -251,36 +252,47 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern) ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern); } -float battery_remaining_estimate_voltage(float voltage) +float battery_remaining_estimate_voltage(float voltage, float discharged) { float ret = 0; - static param_t bat_volt_empty; - static param_t bat_volt_full; - static param_t bat_n_cells; + static param_t bat_v_empty_h; + static param_t bat_v_full_h; + static param_t bat_n_cells_h; + static param_t bat_capacity_h; + static float bat_v_empty = 3.2f; + static float bat_v_full = 4.0f; + static int bat_n_cells = 3; + static float bat_capacity = -1.0f; static bool initialized = false; static unsigned int counter = 0; - static float ncells = 3; - // XXX change cells to int (and param to INT32) if (!initialized) { - bat_volt_empty = param_find("BAT_V_EMPTY"); - bat_volt_full = param_find("BAT_V_FULL"); - bat_n_cells = param_find("BAT_N_CELLS"); + bat_v_empty_h = param_find("BAT_V_EMPTY"); + bat_v_full_h = param_find("BAT_V_FULL"); + bat_n_cells_h = param_find("BAT_N_CELLS"); + bat_capacity_h = param_find("BAT_CAPACITY"); initialized = true; } - static float chemistry_voltage_empty = 3.2f; - static float chemistry_voltage_full = 4.05f; - if (counter % 100 == 0) { - param_get(bat_volt_empty, &chemistry_voltage_empty); - param_get(bat_volt_full, &chemistry_voltage_full); - param_get(bat_n_cells, &ncells); + param_get(bat_v_empty_h, &bat_v_empty); + param_get(bat_v_full_h, &bat_v_full); + param_get(bat_n_cells_h, &bat_n_cells); + param_get(bat_capacity_h, &bat_capacity); } counter++; - ret = (voltage - ncells * chemistry_voltage_empty) / (ncells * (chemistry_voltage_full - chemistry_voltage_empty)); + /* remaining charge estimate based on voltage */ + float remaining_voltage = (voltage - bat_n_cells * bat_v_empty) / (bat_n_cells * (bat_v_full - bat_v_empty)); + + if (bat_capacity > 0.0f) { + /* if battery capacity is known, use discharged current for estimate, but don't show more than voltage estimate */ + ret = fminf(remaining_voltage, 1.0f - discharged / bat_capacity); + } else { + /* else use voltage */ + ret = remaining_voltage; + } /* limit to sane values */ ret = (ret < 0.0f) ? 0.0f : ret; diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index e9514446c1..d0393f45a9 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -75,12 +75,13 @@ void rgbled_set_mode(rgbled_mode_t mode); void rgbled_set_pattern(rgbled_pattern_t *pattern); /** - * Provides a coarse estimate of remaining battery power. + * Estimate remaining battery charge. * - * The estimate is very basic and based on decharging voltage curves. + * Use integral of current if battery capacity known (BAT_CAPACITY parameter set), + * else use simple estimate based on voltage. * * @return the estimated remaining capacity in 0..1 */ -float battery_remaining_estimate_voltage(float voltage); +float battery_remaining_estimate_voltage(float voltage, float discharged); #endif /* COMMANDER_HELPER_H_ */ diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 691d3efcb0..d3155f7bf5 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -48,6 +48,7 @@ PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); -PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f); -PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f); -PARAM_DEFINE_FLOAT(BAT_N_CELLS, 3); +PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f); +PARAM_DEFINE_FLOAT(BAT_V_FULL, 3.9f); +PARAM_DEFINE_INT32(BAT_N_CELLS, 3); +PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 731e0e3ffc..c7256583a3 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -63,11 +63,11 @@ static const int ERROR = -1; static bool arming_state_changed = true; static bool main_state_changed = true; -static bool flighttermination_state_changed = true; +static bool failsafe_state_changed = true; transition_result_t arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety, - arming_state_t new_arming_state, struct actuator_armed_s *armed) + arming_state_t new_arming_state, struct actuator_armed_s *armed) { /* * Perform an atomic state update @@ -85,6 +85,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * /* enforce lockdown in HIL */ if (status->hil_state == HIL_STATE_ON) { armed->lockdown = true; + } else { armed->lockdown = false; } @@ -219,55 +220,54 @@ check_arming_state_changed() } transition_result_t -main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state) +main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state) { transition_result_t ret = TRANSITION_DENIED; - /* only check transition if the new state is actually different from the current one */ - if (new_main_state == current_state->main_state) { - ret = TRANSITION_NOT_CHANGED; + /* transition may be denied even if requested the same state because conditions may be changed */ + switch (new_main_state) { + case MAIN_STATE_MANUAL: + ret = TRANSITION_CHANGED; + break; - } else { + case MAIN_STATE_SEATBELT: - switch (new_main_state) { - case MAIN_STATE_MANUAL: + /* need at minimum altitude estimate */ + if (!status->is_rotary_wing || + (status->condition_local_altitude_valid || + status->condition_global_position_valid)) { ret = TRANSITION_CHANGED; - break; - - case MAIN_STATE_SEATBELT: - - /* need at minimum altitude estimate */ - if (!current_state->is_rotary_wing || - (current_state->condition_local_altitude_valid || - current_state->condition_global_position_valid)) { - ret = TRANSITION_CHANGED; - } - - break; - - case MAIN_STATE_EASY: - - /* need at minimum local position estimate */ - if (current_state->condition_local_position_valid || - current_state->condition_global_position_valid) { - ret = TRANSITION_CHANGED; - } - - break; - - case MAIN_STATE_AUTO: - - /* need global position estimate */ - if (current_state->condition_global_position_valid) { - ret = TRANSITION_CHANGED; - } - - break; } - if (ret == TRANSITION_CHANGED) { - current_state->main_state = new_main_state; + break; + + case MAIN_STATE_EASY: + + /* need at minimum local position estimate */ + if (status->condition_local_position_valid || + status->condition_global_position_valid) { + ret = TRANSITION_CHANGED; + } + + break; + + case MAIN_STATE_AUTO: + + /* need global position estimate */ + if (status->condition_global_position_valid) { + ret = TRANSITION_CHANGED; + } + + break; + } + + if (ret == TRANSITION_CHANGED) { + if (status->main_state != new_main_state) { + status->main_state = new_main_state; main_state_changed = true; + + } else { + ret = TRANSITION_NOT_CHANGED; } } @@ -287,10 +287,10 @@ check_main_state_changed() } bool -check_flighttermination_state_changed() +check_failsafe_state_changed() { - if (flighttermination_state_changed) { - flighttermination_state_changed = false; + if (failsafe_state_changed) { + failsafe_state_changed = false; return true; } else { @@ -361,41 +361,63 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s /** -* Transition from one flightermination state to another +* Transition from one failsafe state to another */ -transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state) +transition_result_t failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state) { transition_result_t ret = TRANSITION_DENIED; - /* only check transition if the new state is actually different from the current one */ - if (new_flighttermination_state == status->flighttermination_state) { + /* transition may be denied even if requested the same state because conditions may be changed */ + if (status->failsafe_state == FAILSAFE_STATE_TERMINATION) { + /* transitions from TERMINATION to other states not allowed */ + if (new_failsafe_state == FAILSAFE_STATE_TERMINATION) { ret = TRANSITION_NOT_CHANGED; - - } else { - - switch (new_flighttermination_state) { - case FLIGHTTERMINATION_STATE_ON: - ret = TRANSITION_CHANGED; - status->flighttermination_state = FLIGHTTERMINATION_STATE_ON; - warnx("state machine helper: change to FLIGHTTERMINATION_STATE_ON"); - break; - case FLIGHTTERMINATION_STATE_OFF: - ret = TRANSITION_CHANGED; - status->flighttermination_state = FLIGHTTERMINATION_STATE_OFF; - break; - - default: - break; - } - - if (ret == TRANSITION_CHANGED) { - flighttermination_state_changed = true; - // TODO - //control_mode->flag_control_flighttermination_enabled = status->flighttermination_state == FLIGHTTERMINATION_STATE_ON; - } } - return ret; + } else { + switch (new_failsafe_state) { + case FAILSAFE_STATE_NORMAL: + /* always allowed (except from TERMINATION state) */ + ret = TRANSITION_CHANGED; + break; + + case FAILSAFE_STATE_RTL: + /* global position and home position required for RTL */ + if (status->condition_global_position_valid && status->condition_home_position_valid) { + ret = TRANSITION_CHANGED; + } + + break; + + case FAILSAFE_STATE_LAND: + /* at least relative altitude estimate required for landing */ + if (status->condition_local_altitude_valid || status->condition_global_position_valid) { + ret = TRANSITION_CHANGED; + } + + break; + + case FAILSAFE_STATE_TERMINATION: + /* always allowed */ + ret = TRANSITION_CHANGED; + break; + + default: + break; + } + + if (ret == TRANSITION_CHANGED) { + if (status->failsafe_state != new_failsafe_state) { + status->failsafe_state = new_failsafe_state; + failsafe_state_changed = true; + + } else { + ret = TRANSITION_NOT_CHANGED; + } + } + } + + return ret; } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index e569fb4f37..f04879ff9e 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -67,11 +67,11 @@ transition_result_t main_state_transition(struct vehicle_status_s *current_state bool check_main_state_changed(); -transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state); +transition_result_t failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state); bool check_navigation_state_changed(); -bool check_flighttermination_state_changed(); +bool check_failsafe_state_changed(); void set_navigation_state_changed(); diff --git a/src/modules/controllib/uorb/blocks.cpp b/src/modules/controllib/uorb/blocks.cpp index e213ac17fc..e8fecef0d9 100644 --- a/src/modules/controllib/uorb/blocks.cpp +++ b/src/modules/controllib/uorb/blocks.cpp @@ -54,8 +54,8 @@ BlockWaypointGuidance::~BlockWaypointGuidance() {}; void BlockWaypointGuidance::update(vehicle_global_position_s &pos, vehicle_attitude_s &att, - mission_item_s &missionCmd, - mission_item_s &lastMissionCmd) + position_setpoint_s &missionCmd, + position_setpoint_s &lastMissionCmd) { // heading to waypoint @@ -86,7 +86,7 @@ BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const c _attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20), _ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20), _pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20), - _missionCmd(&getSubscriptions(), ORB_ID(mission_item_triplet), 20), + _missionCmd(&getSubscriptions(), ORB_ID(position_setpoint_triplet), 20), _manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20), _status(&getSubscriptions(), ORB_ID(vehicle_status), 20), _param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz diff --git a/src/modules/controllib/uorb/blocks.hpp b/src/modules/controllib/uorb/blocks.hpp index 8cc0d77d47..7c80c4b2ba 100644 --- a/src/modules/controllib/uorb/blocks.hpp +++ b/src/modules/controllib/uorb/blocks.hpp @@ -43,7 +43,7 @@ #include #include #include -#include +#include #include #include #include @@ -82,8 +82,8 @@ public: virtual ~BlockWaypointGuidance(); void update(vehicle_global_position_s &pos, vehicle_attitude_s &att, - mission_item_s &missionCmd, - mission_item_s &lastMissionCmd); + position_setpoint_s &missionCmd, + position_setpoint_s &lastMissionCmd); float getPsiCmd() { return _psiCmd; } }; @@ -98,7 +98,7 @@ protected: UOrbSubscription _attCmd; UOrbSubscription _ratesCmd; UOrbSubscription _pos; - UOrbSubscription _missionCmd; + UOrbSubscription _missionCmd; UOrbSubscription _manual; UOrbSubscription _status; UOrbSubscription _param_update; diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp index 108e9896d6..f7c0b61488 100644 --- a/src/modules/fixedwing_backside/fixedwing.cpp +++ b/src/modules/fixedwing_backside/fixedwing.cpp @@ -174,9 +174,9 @@ void BlockMultiModeBacksideAutopilot::update() // of control we will limit the velocity feedback between // the min/max velocity float v = _vLimit.update(sqrtf( - _pos.vx * _pos.vx + + _pos.vel_n * _pos.vel_n + _pos.vy * _pos.vy + - _pos.vz * _pos.vz)); + _pos.vel_d * _pos.vel_d)); // limit velocity command between min/max velocity float vCmd = _vLimit.update(_vCmd.get()); @@ -236,9 +236,9 @@ void BlockMultiModeBacksideAutopilot::update() // for the purpose of control we will limit the velocity feedback between // the min/max velocity float v = _vLimit.update(sqrtf( - _pos.vx * _pos.vx + + _pos.vel_n * _pos.vel_n + _pos.vy * _pos.vy + - _pos.vz * _pos.vz)); + _pos.vel_d * _pos.vel_d)); // pitch channel -> rate of climb // TODO, might want to put a gain on this, otherwise commanding diff --git a/src/modules/fixedwing_backside/fixedwing.hpp b/src/modules/fixedwing_backside/fixedwing.hpp index b4dbc36b09..e1c85c2618 100644 --- a/src/modules/fixedwing_backside/fixedwing.hpp +++ b/src/modules/fixedwing_backside/fixedwing.hpp @@ -264,7 +264,7 @@ private: BlockParamFloat _crMax; struct pollfd _attPoll; - mission_item_triplet_s _lastMissionCmd; + position_setpoint_triplet_s _lastMissionCmd; enum {CH_AIL, CH_ELV, CH_RDR, CH_THR}; uint64_t _timeStamp; public: diff --git a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c index 73df3fb9e0..888dd09423 100644 --- a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c +++ b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c @@ -299,7 +299,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint); start_pos = global_pos; //for now using the current position as the startpoint (= approx. last waypoint because the setpoint switch occurs at the waypoint) global_sp_updated_set_once = true; - psi_track = get_bearing_to_next_waypoint((double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d, + psi_track = get_bearing_to_next_waypoint(global_pos.lat, global_pos.lon, (double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d); printf("next wp direction: %0.4f\n", (double)psi_track); diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index c94180d681..17b1028f9a 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -619,7 +619,7 @@ FixedwingAttitudeControl::task_main() } /* Simple handling of failsafe: deploy parachute if failsafe is on */ - if (_vcontrol_mode.flag_control_flighttermination_enabled) { + if (_vcontrol_mode.flag_control_termination_enabled) { _actuators_airframe.control[1] = 1.0f; // warnx("_actuators_airframe.control[1] = 1.0f;"); } else { @@ -637,7 +637,7 @@ FixedwingAttitudeControl::task_main() /* if airspeed is smaller than min, the sensor is not giving good readings */ if (!_airspeed_valid || - (_airspeed.indicated_airspeed_m_s < 0.1f * _parameters.airspeed_min) || + (_airspeed.indicated_airspeed_m_s < 0.5f * _parameters.airspeed_min) || !isfinite(_airspeed.indicated_airspeed_m_s)) { airspeed = _parameters.airspeed_trim; @@ -704,9 +704,9 @@ FixedwingAttitudeControl::task_main() float speed_body_v = 0.0f; float speed_body_w = 0.0f; if(_att.R_valid) { - speed_body_u = _att.R[0][0] * _global_pos.vx + _att.R[1][0] * _global_pos.vy + _att.R[2][0] * _global_pos.vz; - speed_body_v = _att.R[0][1] * _global_pos.vx + _att.R[1][1] * _global_pos.vy + _att.R[2][1] * _global_pos.vz; - speed_body_w = _att.R[0][2] * _global_pos.vx + _att.R[1][2] * _global_pos.vy + _att.R[2][2] * _global_pos.vz; + speed_body_u = _att.R[0][0] * _global_pos.vel_n + _att.R[1][0] * _global_pos.vel_e + _att.R[2][0] * _global_pos.vel_d; + speed_body_v = _att.R[0][1] * _global_pos.vel_n + _att.R[1][1] * _global_pos.vel_e + _att.R[2][1] * _global_pos.vel_d; + speed_body_w = _att.R[0][2] * _global_pos.vel_n + _att.R[1][2] * _global_pos.vel_e + _att.R[2][2] * _global_pos.vel_d; } else { warnx("Did not get a valid R\n"); } diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 04caf0bbc3..a62b53221c 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -68,7 +68,7 @@ #include #include #include -#include +#include #include #include #include @@ -126,7 +126,7 @@ private: int _control_task; /**< task handle for sensor task */ int _global_pos_sub; - int _mission_item_triplet_sub; + int _pos_sp_triplet_sub; int _att_sub; /**< vehicle attitude subscription */ int _attitude_sub; /**< raw rc channels data subscription */ int _airspeed_sub; /**< airspeed subscription */ @@ -145,7 +145,7 @@ private: struct airspeed_s _airspeed; /**< airspeed */ struct vehicle_control_mode_s _control_mode; /**< vehicle status */ struct vehicle_global_position_s _global_pos; /**< global vehicle position */ - struct mission_item_triplet_s _mission_item_triplet; /**< triplet of mission items */ + struct position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of mission items */ struct sensor_combined_s _sensor_combined; /**< for body frame accelerations */ perf_counter_t _loop_perf; /**< loop performance counter */ @@ -175,7 +175,6 @@ private: /* takeoff/launch states */ bool launch_detected; bool usePreTakeoffThrust; - bool launch_detection_message_sent; /* Landingslope object */ Landingslope landingslope; @@ -193,7 +192,7 @@ private: uint64_t _airspeed_last_valid; ///< last time airspeed was valid. Used to detect sensor failures float _groundspeed_undershoot; ///< ground speed error to min. speed in m/s bool _global_pos_valid; ///< global position is valid - math::Dcm _R_nb; ///< current attitude + math::Matrix<3, 3> _R_nb; ///< current attitude ECL_L1_Pos_Controller _l1_control; TECS _tecs; @@ -332,11 +331,11 @@ private: /** * Control position. */ - bool control_position(const math::Vector2f &global_pos, const math::Vector2f &ground_speed, - const struct mission_item_triplet_s &_mission_item_triplet); + bool control_position(const math::Vector<2> &global_pos, const math::Vector<2> &ground_speed, + const struct position_setpoint_triplet_s &_pos_sp_triplet); float calculate_target_airspeed(float airspeed_demand); - void calculate_gndspeed_undershoot(const math::Vector2f ¤t_position, const math::Vector2f &ground_speed, const struct mission_item_triplet_s &mission_item_triplet); + void calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet); /** * Shim for calling task_main from task_create. @@ -368,7 +367,7 @@ FixedwingPositionControl::FixedwingPositionControl() : /* subscriptions */ _global_pos_sub(-1), - _mission_item_triplet_sub(-1), + _pos_sp_triplet_sub(-1), _att_sub(-1), _airspeed_sub(-1), _control_mode_sub(-1), @@ -397,8 +396,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _mavlink_fd(-1), launchDetector(), launch_detected(false), - usePreTakeoffThrust(false), - launch_detection_message_sent(false) + usePreTakeoffThrust(false) { /* safely initialize structs */ vehicle_attitude_s _att = {0}; @@ -408,7 +406,7 @@ FixedwingPositionControl::FixedwingPositionControl() : airspeed_s _airspeed = {0}; vehicle_control_mode_s _control_mode = {0}; vehicle_global_position_s _global_pos = {0}; - mission_item_triplet_s _mission_item_triplet = {0}; + position_setpoint_triplet_s _pos_sp_triplet = {0}; sensor_combined_s _sensor_combined = {0}; @@ -655,11 +653,11 @@ void FixedwingPositionControl::vehicle_setpoint_poll() { /* check if there is a new setpoint */ - bool mission_item_triplet_updated; - orb_check(_mission_item_triplet_sub, &mission_item_triplet_updated); + bool pos_sp_triplet_updated; + orb_check(_pos_sp_triplet_sub, &pos_sp_triplet_updated); - if (mission_item_triplet_updated) { - orb_copy(ORB_ID(mission_item_triplet), _mission_item_triplet_sub, &_mission_item_triplet); + if (pos_sp_triplet_updated) { + orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); _setpoint_valid = true; } } @@ -702,25 +700,25 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand) } void -FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector2f ¤t_position, const math::Vector2f &ground_speed, const struct mission_item_triplet_s &mission_item_triplet) +FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet) { if (_global_pos_valid) { /* rotate ground speed vector with current attitude */ - math::Vector2f yaw_vector(_R_nb(0, 0), _R_nb(1, 0)); + math::Vector<2> yaw_vector(_R_nb(0, 0), _R_nb(1, 0)); yaw_vector.normalize(); float ground_speed_body = yaw_vector * ground_speed; /* The minimum desired ground speed is the minimum airspeed projected on to the ground using the altitude and horizontal difference between the waypoints if available*/ float distance = 0.0f; float delta_altitude = 0.0f; - if (mission_item_triplet.previous_valid) { - distance = get_distance_to_next_waypoint(mission_item_triplet.previous.lat, mission_item_triplet.previous.lon, mission_item_triplet.current.lat, mission_item_triplet.current.lon); - delta_altitude = mission_item_triplet.current.altitude - mission_item_triplet.previous.altitude; + if (pos_sp_triplet.previous.valid) { + distance = get_distance_to_next_waypoint(pos_sp_triplet.previous.lat, pos_sp_triplet.previous.lon, pos_sp_triplet.current.lat, pos_sp_triplet.current.lon); + delta_altitude = pos_sp_triplet.current.alt - pos_sp_triplet.previous.alt; } else { - distance = get_distance_to_next_waypoint(current_position.getX(), current_position.getY(), mission_item_triplet.current.lat, mission_item_triplet.current.lon); - delta_altitude = mission_item_triplet.current.altitude - _global_pos.alt; + distance = get_distance_to_next_waypoint(current_position(0), current_position(1), pos_sp_triplet.current.lat, pos_sp_triplet.current.lon); + delta_altitude = pos_sp_triplet.current.alt - _global_pos.alt; } float ground_speed_desired = _parameters.airspeed_min * cosf(atan2f(delta_altitude, distance)); @@ -752,12 +750,12 @@ void FixedwingPositionControl::navigation_capabilities_publish() } bool -FixedwingPositionControl::control_position(const math::Vector2f ¤t_position, const math::Vector2f &ground_speed, - const struct mission_item_triplet_s &mission_item_triplet) +FixedwingPositionControl::control_position(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, + const struct position_setpoint_triplet_s &pos_sp_triplet) { bool setpoint = true; - calculate_gndspeed_undershoot(current_position, ground_speed, mission_item_triplet); + calculate_gndspeed_undershoot(current_position, ground_speed, pos_sp_triplet); float eas2tas = 1.0f; // XXX calculate actual number based on current measurements @@ -765,11 +763,11 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio float baro_altitude = _global_pos.alt; /* filter speed and altitude for controller */ - math::Vector3 accel_body(_sensor_combined.accelerometer_m_s2[0], _sensor_combined.accelerometer_m_s2[1], _sensor_combined.accelerometer_m_s2[2]); - math::Vector3 accel_earth = _R_nb * accel_body; + math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2); + math::Vector<3> accel_earth = _R_nb * accel_body; _tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth); - float altitude_error = _mission_item_triplet.current.altitude - _global_pos.alt; + float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt; /* no throttle limit as default */ float throttle_max = 1.0f; @@ -787,70 +785,68 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio _tecs.set_speed_weight(_parameters.speed_weight); /* current waypoint (the one currently heading for) */ - math::Vector2f next_wp(mission_item_triplet.current.lat, mission_item_triplet.current.lon); + math::Vector<2> next_wp(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon); /* current waypoint (the one currently heading for) */ - math::Vector2f curr_wp(mission_item_triplet.current.lat, mission_item_triplet.current.lon); + math::Vector<2> curr_wp(pos_sp_triplet.current.lat, pos_sp_triplet.current.lon); /* previous waypoint */ - math::Vector2f prev_wp; + math::Vector<2> prev_wp; - if (mission_item_triplet.previous_valid) { - prev_wp.setX(mission_item_triplet.previous.lat); - prev_wp.setY(mission_item_triplet.previous.lon); + if (pos_sp_triplet.previous.valid) { + prev_wp(0) = pos_sp_triplet.previous.lat; + prev_wp(1) = pos_sp_triplet.previous.lon; } else { /* * No valid previous waypoint, go for the current wp. * This is automatically handled by the L1 library. */ - prev_wp.setX(mission_item_triplet.current.lat); - prev_wp.setY(mission_item_triplet.current.lon); + prev_wp(0) = pos_sp_triplet.current.lat; + prev_wp(1) = pos_sp_triplet.current.lon; } - if (mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT || mission_item_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { + if (pos_sp_triplet.current.type == SETPOINT_TYPE_NORMAL) { /* waypoint is a plain navigation waypoint */ _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), _airspeed.indicated_airspeed_m_s, eas2tas, false, math::radians(_parameters.pitch_limit_min), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); - } else if (mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) { + } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER) { /* waypoint is a loiter waypoint */ - _l1_control.navigate_loiter(curr_wp, current_position, mission_item_triplet.current.loiter_radius, - mission_item_triplet.current.loiter_direction, ground_speed); + _l1_control.navigate_loiter(curr_wp, current_position, pos_sp_triplet.current.loiter_radius, + pos_sp_triplet.current.loiter_direction, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), _airspeed.indicated_airspeed_m_s, eas2tas, false, math::radians(_parameters.pitch_limit_min), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); - } else if (mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) { + } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { /* Horizontal landing control */ /* switch to heading hold for the last meters, continue heading hold after */ - float wp_distance = get_distance_to_next_waypoint(current_position.getX(), current_position.getY(), curr_wp.getX(), curr_wp.getY()); + float wp_distance = get_distance_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); //warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO"); if (wp_distance < _parameters.land_heading_hold_horizontal_distance || land_noreturn_horizontal) { /* heading hold, along the line connecting this and the last waypoint */ if (!land_noreturn_horizontal) {//set target_bearing in first occurrence - if (mission_item_triplet.previous_valid) { - target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), curr_wp.getX(), curr_wp.getY()); + if (pos_sp_triplet.previous.valid) { + target_bearing = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); } else { target_bearing = _att.yaw; } @@ -881,23 +877,23 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio // /* do not go down too early */ // if (wp_distance > 50.0f) { -// altitude_error = (_global_triplet.current.altitude + 25.0f) - _global_pos.alt; +// altitude_error = (_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 - float flare_pitch_angle_rad = -math::radians(5.0f);//math::radians(mission_item_triplet.current.param1) + float flare_pitch_angle_rad = -math::radians(5.0f);//math::radians(pos_sp_triplet.current.param1) float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f; float airspeed_land = 1.3f * _parameters.airspeed_min; float airspeed_approach = 1.3f * _parameters.airspeed_min; - float L_wp_distance = get_distance_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), curr_wp.getX(), curr_wp.getY()) * _parameters.land_slope_length; - float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _mission_item_triplet.current.altitude);//getLandingSlopeAbsoluteAltitude(L_wp_distance, _mission_item_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); - float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitude(wp_distance, _mission_item_triplet.current.altitude);//getLandingSlopeAbsoluteAltitude(wp_distance, _mission_item_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); + float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)) * _parameters.land_slope_length; + float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt);//getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt, landing_slope_angle_rad, horizontal_slope_displacement); + float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitude(wp_distance, _pos_sp_triplet.current.alt);//getLandingSlopeAbsoluteAltitude(wp_distance, _pos_sp_triplet.current.alt, landing_slope_angle_rad, horizontal_slope_displacement); - if ( (_global_pos.alt < _mission_item_triplet.current.altitude + landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out + if ( (_global_pos.alt < _pos_sp_triplet.current.alt + landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out /* land with minimal speed */ @@ -916,12 +912,12 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } - float flare_curve_alt = landingslope.getFlareCurveAltitude(wp_distance, _mission_item_triplet.current.altitude); + float flare_curve_alt = landingslope.getFlareCurveAltitude(wp_distance, _pos_sp_triplet.current.alt); /* avoid climbout */ if (flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical || land_stayonground) { - flare_curve_alt = mission_item_triplet.current.altitude; + flare_curve_alt = pos_sp_triplet.current.alt; land_stayonground = true; } @@ -979,16 +975,17 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); } - } else if (mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { + } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF) { /* Perform launch detection */ // warnx("Launch detection running"); if(!launch_detected) { //do not do further checks once a launch was detected if (launchDetector.launchDetectionEnabled()) { -// warnx("Launch detection enabled"); - if(!launch_detection_message_sent) { + static hrt_abstime last_sent = 0; + if(hrt_absolute_time() - last_sent > 4e6) { +// warnx("Launch detection running"); mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running"); - launch_detection_message_sent = true; + last_sent = hrt_absolute_time(); } launchDetector.update(_sensor_combined.accelerometer_m_s2[0]); if (launchDetector.getLaunchDetected()) { @@ -1012,9 +1009,9 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio if (altitude_error > 15.0f) { /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(1.3f * _parameters.airspeed_min), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(1.3f * _parameters.airspeed_min), _airspeed.indicated_airspeed_m_s, eas2tas, - true, math::max(math::radians(mission_item_triplet.current.pitch_min), math::radians(10.0f)), + true, math::max(math::radians(pos_sp_triplet.current.pitch_min), math::radians(10.0f)), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); @@ -1023,7 +1020,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } else { - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), _airspeed.indicated_airspeed_m_s, eas2tas, false, math::radians(_parameters.pitch_limit_min), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, @@ -1037,15 +1034,15 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio // warnx("nav bearing: %8.4f bearing err: %8.4f target bearing: %8.4f", (double)_l1_control.nav_bearing(), // (double)_l1_control.bearing_error(), (double)_l1_control.target_bearing()); - // warnx("prev wp: %8.4f/%8.4f, next wp: %8.4f/%8.4f prev:%s", (double)prev_wp.getX(), (double)prev_wp.getY(), - // (double)next_wp.getX(), (double)next_wp.getY(), (mission_item_triplet.previous_valid) ? "valid" : "invalid"); + // warnx("prev wp: %8.4f/%8.4f, next wp: %8.4f/%8.4f prev:%s", (double)prev_wp(0), (double)prev_wp(1), + // (double)next_wp(0), (double)next_wp(1), (pos_sp_triplet.previous_valid) ? "valid" : "invalid"); // XXX at this point we always want no loiter hold if a // mission is active _loiter_hold = false; /* reset land state */ - if (mission_item_triplet.current.nav_cmd != NAV_CMD_LAND) { + if (pos_sp_triplet.current.type != SETPOINT_TYPE_LAND) { land_noreturn_horizontal = false; land_noreturn_vertical = false; land_stayonground = false; @@ -1054,10 +1051,9 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } /* reset takeoff/launch state */ - if (mission_item_triplet.current.nav_cmd != NAV_CMD_TAKEOFF) { + if (pos_sp_triplet.current.type != SETPOINT_TYPE_TAKEOFF) { launch_detected = false; usePreTakeoffThrust = false; - launch_detection_message_sent = false; } if (was_circle_mode && !_l1_control.circle_mode()) { @@ -1178,7 +1174,7 @@ FixedwingPositionControl::task_main() * do subscriptions */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - _mission_item_triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet)); + _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); @@ -1266,14 +1262,14 @@ FixedwingPositionControl::task_main() vehicle_airspeed_poll(); // vehicle_baro_poll(); - math::Vector2f ground_speed(_global_pos.vx, _global_pos.vy); - math::Vector2f current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f); + math::Vector<2> ground_speed(_global_pos.vel_n, _global_pos.vel_e); + math::Vector<2> current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f); /* * Attempt to control position, on success (= sensors present and not in manual mode), * publish setpoint. */ - if (control_position(current_position, ground_speed, _mission_item_triplet)) { + if (control_position(current_position, ground_speed, _pos_sp_triplet)) { _att_sp.timestamp = hrt_absolute_time(); /* lazily publish the setpoint only once available */ @@ -1287,7 +1283,7 @@ FixedwingPositionControl::task_main() } /* XXX check if radius makes sense here */ - float turn_distance = _l1_control.switch_distance(_mission_item_triplet.current.acceptance_radius); + float turn_distance = _l1_control.switch_distance(100.0f); /* lazily publish navigation capabilities */ if (turn_distance != _nav_capabilities.turn_distance && turn_distance > 0) { 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 9f46b51706..7954d75c20 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 @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier + * 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 @@ -49,21 +48,75 @@ * */ +/** + * L1 period + * + * This is the L1 distance and defines the tracking + * point ahead of the aircraft its following. + * A value of 25 meters works for most aircraft. Shorten + * slowly during tuning until response is sharp without oscillation. + * + * @min 1.0 + * @max 100.0 + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_L1_PERIOD, 25.0f); - +/** + * L1 damping + * + * Damping factor for L1 control. + * + * @min 0.6 + * @max 0.9 + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_L1_DAMPING, 0.75f); - +/** + * Default Loiter Radius + * + * This radius is used when no other loiter radius is set. + * + * @min 10.0 + * @max 100.0 + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_LOITER_R, 50.0f); - +/** + * Cruise throttle + * + * This is the throttle setting required to achieve the desired cruise speed. Most airframes have a value of 0.5-0.7. + * + * @min 0.0 + * @max 1.0 + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_THR_CRUISE, 0.7f); - +/** + * Negative pitch limit + * + * The minimum negative pitch the controller will output. + * + * @unit degrees + * @min -60.0 + * @max 0.0 + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_P_LIM_MIN, -45.0f); - +/** + * Positive pitch limit + * + * The maximum positive pitch the controller will output. + * + * @unit degrees + * @min 0.0 + * @max 60.0 + * @group L1 Control + */ PARAM_DEFINE_FLOAT(FW_P_LIM_MAX, 45.0f); diff --git a/src/modules/mavlink/mavlink_orb_listener.cpp b/src/modules/mavlink/mavlink_orb_listener.cpp index e2675dfa71..44bf77bb0d 100644 --- a/src/modules/mavlink/mavlink_orb_listener.cpp +++ b/src/modules/mavlink/mavlink_orb_listener.cpp @@ -59,7 +59,6 @@ #include "mavlink_orb_listener.h" #include "mavlink_main.h" -//#include "util.h" void *uorb_receive_thread(void *arg); @@ -200,10 +199,10 @@ MavlinkOrbListener::l_vehicle_attitude(const struct listener *l) hrt_abstime t = hrt_absolute_time(); if (t >= last_sent_vfr + 100000) { last_sent_vfr = t; - float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy); + float groundspeed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e); uint16_t heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F; float throttle = armed.armed ? actuators_0.control[3] * 100.0f : 0.0f; - mavlink_msg_vfr_hud_send(_mavlink->get_mavlink_chan(), airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vz); + mavlink_msg_vfr_hud_send(_mavlink->get_mavlink_chan(), airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vel_d); } /* send quaternion values if it exists */ @@ -333,13 +332,13 @@ MavlinkOrbListener::l_global_position(const struct listener *l) mavlink_msg_global_position_int_send(_mavlink->get_mavlink_chan(), global_pos.timestamp / 1000, - global_pos.lat, - global_pos.lon, + global_pos.lat * 1e7, + global_pos.lon * 1e7, global_pos.alt * 1000.0f, - global_pos.relative_alt * 1000.0f, - global_pos.vx * 100.0f, - global_pos.vy * 100.0f, - global_pos.vz * 100.0f, + (global_pos.alt - home.alt) * 1000.0f, + global_pos.vel_n * 100.0f, + global_pos.vel_e * 100.0f, + global_pos.vel_d * 100.0f, _wrap_2pi(global_pos.yaw) * M_RAD_TO_DEG_F * 100.0f); } @@ -363,23 +362,18 @@ MavlinkOrbListener::l_local_position(const struct listener *l) void MavlinkOrbListener::l_global_position_setpoint(const struct listener *l) { - struct mission_item_triplet_s triplet; - orb_copy(ORB_ID(mission_item_triplet), mavlink_subs.triplet_sub, &triplet); + struct position_setpoint_triplet_s triplet; + orb_copy(ORB_ID(position_setpoint_triplet), mavlink_subs.triplet_sub, &triplet); - uint8_t coordinate_frame = MAV_FRAME_GLOBAL; - - if (!triplet.current_valid) + if (!triplet.current.valid) return; - if (triplet.current.altitude_is_relative) - coordinate_frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; - if (gcs_link) mavlink_msg_global_position_setpoint_int_send(_mavlink->get_mavlink_chan(), - coordinate_frame, + MAV_FRAME_GLOBAL, (int32_t)(triplet.current.lat * 1e7d), (int32_t)(triplet.current.lon * 1e7d), - (int32_t)(triplet.current.altitude * 1e3f), + (int32_t)(triplet.current.alt * 1e3f), (int16_t)(triplet.current.yaw * M_RAD_TO_DEG_F * 1e2f)); } @@ -615,11 +609,9 @@ MavlinkOrbListener::l_optical_flow(const struct listener *l) void MavlinkOrbListener::l_home(const struct listener *l) { - struct home_position_s home; - orb_copy(ORB_ID(home_position), mavlink_subs.home_sub, &home); - mavlink_msg_gps_global_origin_send(_mavlink->get_mavlink_chan(), (int32_t)(home.lat*1e7d), (int32_t)(home.lon*1e7d), (int32_t)(home.altitude)*1e3f); + mavlink_msg_gps_global_origin_send(_mavlink->get_mavlink_chan(), (int32_t)(home.lat*1e7d), (int32_t)(home.lon*1e7d), (int32_t)(home.alt)*1e3f); } void @@ -757,7 +749,7 @@ MavlinkOrbListener::uorb_receive_start(void) orb_set_interval(mavlink_subs.local_pos_sub, 1000); /* 1Hz active updates */ /* --- GLOBAL SETPOINT VALUE --- */ - mavlink_subs.triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet)); + mavlink_subs.triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); orb_set_interval(mavlink_subs.triplet_sub, 2000); /* 0.5 Hz updates */ /* --- LOCAL SETPOINT VALUE --- */ diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index ab4074558a..4f763c3c68 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -555,6 +555,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) hil_gps.alt = gps.alt; hil_gps.eph_m = (float)gps.eph * 1e-2f; // from cm to m hil_gps.epv_m = (float)gps.epv * 1e-2f; // from cm to m + hil_gps.timestamp_variance = gps.time_usec; hil_gps.s_variance_m_s = 5.0f; hil_gps.p_variance_m = hil_gps.eph_m * hil_gps.eph_m; hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s @@ -566,6 +567,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) if (heading_rad > M_PI_F) heading_rad -= 2.0f * M_PI_F; + hil_gps.timestamp_velocity = gps.time_usec; hil_gps.vel_n_m_s = gps.vn * 1e-2f; // from cm to m hil_gps.vel_e_m_s = gps.ve * 1e-2f; // from cm to m hil_gps.vel_d_m_s = gps.vd * 1e-2f; // from cm to m @@ -613,9 +615,9 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) hil_global_pos.lat = hil_state.lat; hil_global_pos.lon = hil_state.lon; hil_global_pos.alt = hil_state.alt / 1000.0f; - hil_global_pos.vx = hil_state.vx / 100.0f; - hil_global_pos.vy = hil_state.vy / 100.0f; - hil_global_pos.vz = hil_state.vz / 100.0f; + hil_global_pos.vel_n = hil_state.vx / 100.0f; + hil_global_pos.vel_e = hil_state.vy / 100.0f; + hil_global_pos.vel_d = hil_state.vz / 100.0f; } else { pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); @@ -659,8 +661,8 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) /* Calculate Rotation Matrix */ math::Quaternion q(hil_state.attitude_quaternion); - math::Dcm C_nb(q); - math::EulerAngles euler(C_nb); + math::Matrix<3,3> C_nb = q.to_dcm(); + math::Vector<3> euler = C_nb.to_euler(); /* set rotation matrix */ for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) @@ -675,9 +677,9 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) hil_attitude.q[3] = q(3); hil_attitude.q_valid = true; - hil_attitude.roll = euler.getPhi(); - hil_attitude.pitch = euler.getTheta(); - hil_attitude.yaw = euler.getPsi(); + hil_attitude.roll = euler(0); + hil_attitude.pitch = euler(1); + hil_attitude.yaw = euler(2); hil_attitude.rollspeed = hil_state.rollspeed; hil_attitude.pitchspeed = hil_state.pitchspeed; hil_attitude.yawspeed = hil_state.yawspeed; diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 556b6f8ad6..ea57714d2d 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -53,9 +53,8 @@ #include #include #include -#include +#include #include -#include #include #include #include diff --git a/src/modules/mavlink_onboard/orb_topics.h b/src/modules/mavlink_onboard/orb_topics.h index 86bfa26f25..bbc9f6e66a 100644 --- a/src/modules/mavlink_onboard/orb_topics.h +++ b/src/modules/mavlink_onboard/orb_topics.h @@ -50,7 +50,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp new file mode 100644 index 0000000000..245ac024b8 --- /dev/null +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -0,0 +1,779 @@ +/**************************************************************************** + * + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * Author: @author Tobias Naegeli + * @author Lorenz Meier + * @author Anton Babushkin + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mc_att_control_main.c + * Multicopter attitude controller. + * + * The controller has two loops: P loop for angular error and PD loop for angular rate error. + * Desired rotation calculated keeping in mind that yaw response is normally slower than roll/pitch. + * For small deviations controller rotates copter to have shortest path of thrust vector and independently rotates around yaw, + * so actual rotation axis is not constant. For large deviations controller rotates copter around fixed axis. + * These two approaches fused seamlessly with weight depending on angular error. + * When thrust vector directed near-horizontally (e.g. roll ~= PI/2) yaw setpoint ignored because of singularity. + * Controller doesn't use Euler angles for work, they generated only for more human-friendly control and logging. + * If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * Multicopter attitude control app start / stop handling function + * + * @ingroup apps + */ +extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]); + +#define MIN_TAKEOFF_THROTTLE 0.3f +#define YAW_DEADZONE 0.01f +#define RATES_I_LIMIT 0.5f + +class MulticopterAttitudeControl +{ +public: + /** + * Constructor + */ + MulticopterAttitudeControl(); + + /** + * Destructor, also kills the sensors task. + */ + ~MulticopterAttitudeControl(); + + /** + * Start the sensors task. + * + * @return OK on success. + */ + int start(); + +private: + + bool _task_should_exit; /**< if true, sensor task should exit */ + int _control_task; /**< task handle for sensor task */ + + int _att_sub; /**< vehicle attitude subscription */ + int _att_sp_sub; /**< vehicle attitude setpoint */ + int _control_mode_sub; /**< vehicle control mode subscription */ + int _params_sub; /**< notification of parameter updates */ + int _manual_sub; /**< notification of manual control updates */ + int _arming_sub; /**< arming status of outputs */ + + orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ + orb_advert_t _rates_sp_pub; /**< rate setpoint publication */ + orb_advert_t _actuators_0_pub; /**< actuator control group 0 setpoint */ + + struct vehicle_attitude_s _att; /**< vehicle attitude */ + struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ + struct manual_control_setpoint_s _manual; /**< r/c channel data */ + struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ + struct actuator_controls_s _actuators; /**< actuator control inputs */ + struct actuator_armed_s _arming; /**< actuator arming status */ + struct vehicle_rates_setpoint_s _rates_sp; /**< vehicle rates setpoint */ + + perf_counter_t _loop_perf; /**< loop performance counter */ + + math::Vector<3> _rates_prev; /**< angular rates on previous step */ + + struct { + param_t att_p; + param_t yaw_p; + param_t att_rate_p; + param_t att_rate_i; + param_t att_rate_d; + param_t yaw_rate_p; + param_t yaw_rate_i; + param_t yaw_rate_d; + } _params_handles; /**< handles for interesting parameters */ + + struct { + math::Vector<3> p; /**< P gain for angular error */ + math::Vector<3> rate_p; /**< P gain for angular rate error */ + math::Vector<3> rate_i; /**< I gain for angular rate error */ + math::Vector<3> rate_d; /**< D gain for angular rate error */ + } _params; + + /** + * Update our local parameter cache. + */ + int parameters_update(); + + /** + * Update control outputs + */ + void control_update(); + + /** + * Check for changes in vehicle control mode. + */ + void vehicle_control_mode_poll(); + + /** + * Check for changes in manual inputs. + */ + void vehicle_manual_poll(); + + /** + * Check for set triplet updates. + */ + void vehicle_setpoint_poll(); + + /** + * Check for arming status updates. + */ + void arming_status_poll(); + + /** + * Shim for calling task_main from task_create. + */ + static void task_main_trampoline(int argc, char *argv[]); + + /** + * Main sensor collection task. + */ + void task_main() __attribute__((noreturn)); +}; + +namespace att_control +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +MulticopterAttitudeControl *g_control; +} + +MulticopterAttitudeControl::MulticopterAttitudeControl() : + + _task_should_exit(false), + _control_task(-1), + +/* subscriptions */ + _att_sub(-1), + _att_sp_sub(-1), + _control_mode_sub(-1), + _params_sub(-1), + _manual_sub(-1), + _arming_sub(-1), + +/* publications */ + _att_sp_pub(-1), + _rates_sp_pub(-1), + _actuators_0_pub(-1), + +/* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")) + +{ + memset(&_att, 0, sizeof(_att)); + memset(&_att_sp, 0, sizeof(_att_sp)); + memset(&_manual, 0, sizeof(_manual)); + memset(&_control_mode, 0, sizeof(_control_mode)); + memset(&_arming, 0, sizeof(_arming)); + + _params.p.zero(); + _params.rate_p.zero(); + _params.rate_i.zero(); + _params.rate_d.zero(); + + _rates_prev.zero(); + + _params_handles.att_p = param_find("MC_ATT_P"); + _params_handles.yaw_p = param_find("MC_YAW_P"); + _params_handles.att_rate_p = param_find("MC_ATTRATE_P"); + _params_handles.att_rate_i = param_find("MC_ATTRATE_I"); + _params_handles.att_rate_d = param_find("MC_ATTRATE_D"); + _params_handles.yaw_rate_p = param_find("MC_YAWRATE_P"); + _params_handles.yaw_rate_i = param_find("MC_YAWRATE_I"); + _params_handles.yaw_rate_d = param_find("MC_YAWRATE_D"); + + /* fetch initial parameter values */ + parameters_update(); +} + +MulticopterAttitudeControl::~MulticopterAttitudeControl() +{ + if (_control_task != -1) { + /* task wakes up every 100ms or so at the longest */ + _task_should_exit = true; + + /* wait for a second for the task to quit at our request */ + unsigned i = 0; + + do { + /* wait 20ms */ + usleep(20000); + + /* if we have given up, kill it */ + if (++i > 50) { + task_delete(_control_task); + break; + } + } while (_control_task != -1); + } + + att_control::g_control = nullptr; +} + +int +MulticopterAttitudeControl::parameters_update() +{ + float v; + + param_get(_params_handles.att_p, &v); + _params.p(0) = v; + _params.p(1) = v; + param_get(_params_handles.yaw_p, &v); + _params.p(2) = v; + + param_get(_params_handles.att_rate_p, &v); + _params.rate_p(0) = v; + _params.rate_p(1) = v; + param_get(_params_handles.yaw_rate_p, &v); + _params.rate_p(2) = v; + + param_get(_params_handles.att_rate_i, &v); + _params.rate_i(0) = v; + _params.rate_i(1) = v; + param_get(_params_handles.yaw_rate_i, &v); + _params.rate_i(2) = v; + + param_get(_params_handles.att_rate_d, &v); + _params.rate_d(0) = v; + _params.rate_d(1) = v; + param_get(_params_handles.yaw_rate_d, &v); + _params.rate_d(2) = v; + + return OK; +} + +void +MulticopterAttitudeControl::vehicle_control_mode_poll() +{ + bool control_mode_updated; + + /* Check HIL state if vehicle status has changed */ + orb_check(_control_mode_sub, &control_mode_updated); + + if (control_mode_updated) { + + orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); + } +} + +void +MulticopterAttitudeControl::vehicle_manual_poll() +{ + bool manual_updated; + + /* get pilots inputs */ + orb_check(_manual_sub, &manual_updated); + + if (manual_updated) { + + orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual); + } +} + +void +MulticopterAttitudeControl::vehicle_setpoint_poll() +{ + /* check if there is a new setpoint */ + bool att_sp_updated; + orb_check(_att_sp_sub, &att_sp_updated); + + if (att_sp_updated) { + orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp); + } +} + +void +MulticopterAttitudeControl::arming_status_poll() +{ + /* check if there is a new setpoint */ + bool arming_updated; + orb_check(_arming_sub, &arming_updated); + + if (arming_updated) { + orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming); + } +} + +void +MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[]) +{ + att_control::g_control->task_main(); +} + +void +MulticopterAttitudeControl::task_main() +{ + /* inform about start */ + warnx("started"); + fflush(stdout); + + /* + * do subscriptions + */ + _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + _params_sub = orb_subscribe(ORB_ID(parameter_update)); + _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + _arming_sub = orb_subscribe(ORB_ID(actuator_armed)); + + /* rate limit attitude updates to 100Hz */ + orb_set_interval(_att_sub, 10); + + parameters_update(); + + /* initialize values of critical structs until first regular update */ + _arming.armed = false; + + /* get an initial update for all sensor and status data */ + vehicle_setpoint_poll(); + vehicle_control_mode_poll(); + vehicle_manual_poll(); + arming_status_poll(); + + /* setpoint rotation matrix */ + math::Matrix<3, 3> R_sp; + R_sp.identity(); + + /* rotation matrix for current state */ + math::Matrix<3, 3> R; + R.identity(); + + /* current angular rates */ + math::Vector<3> rates; + rates.zero(); + + /* angular rates integral error */ + math::Vector<3> rates_int; + rates_int.zero(); + + /* identity matrix */ + math::Matrix<3, 3> I; + I.identity(); + + math::Quaternion q; + + bool reset_yaw_sp = true; + + /* wakeup source(s) */ + struct pollfd fds[2]; + + /* Setup of loop */ + fds[0].fd = _params_sub; + fds[0].events = POLLIN; + fds[1].fd = _att_sub; + fds[1].events = POLLIN; + + while (!_task_should_exit) { + + /* wait for up to 500ms for data */ + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + + /* timed out - periodic check for _task_should_exit, etc. */ + if (pret == 0) + continue; + + /* this is undesirable but not much we can do - might want to flag unhappy status */ + if (pret < 0) { + warn("poll error %d, %d", pret, errno); + continue; + } + + perf_begin(_loop_perf); + + /* only update parameters if they changed */ + if (fds[0].revents & POLLIN) { + /* copy the topic to clear updated flag */ + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), _params_sub, &update); + + parameters_update(); + } + + /* only run controller if attitude changed */ + if (fds[1].revents & POLLIN) { + static uint64_t last_run = 0; + float dt = (hrt_absolute_time() - last_run) / 1000000.0f; + last_run = hrt_absolute_time(); + + /* guard against too large dt's */ + if (dt > 0.02f) + dt = 0.02f; + + /* copy attitude topic */ + orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); + + vehicle_setpoint_poll(); + vehicle_control_mode_poll(); + arming_status_poll(); + vehicle_manual_poll(); + + float yaw_sp_move_rate = 0.0f; + bool publish_att_sp = false; + + /* define which input is the dominating control input */ + if (_control_mode.flag_control_manual_enabled) { + /* manual input */ + if (!_control_mode.flag_control_climb_rate_enabled) { + /* pass throttle directly if not in altitude control mode */ + _att_sp.thrust = _manual.throttle; + } + + if (!_arming.armed) { + /* reset yaw setpoint when disarmed */ + reset_yaw_sp = true; + } + + if (_control_mode.flag_control_attitude_enabled) { + /* control attitude, update attitude setpoint depending on mode */ + + if (_att_sp.thrust < 0.1f) { + // TODO + //if (_status.condition_landed) { + /* reset yaw setpoint if on ground */ + // reset_yaw_sp = true; + //} + } else { + if (_manual.yaw < -YAW_DEADZONE || YAW_DEADZONE < _manual.yaw) { + /* move yaw setpoint */ + yaw_sp_move_rate = _manual.yaw; + _att_sp.yaw_body = _wrap_pi(_att_sp.yaw_body + yaw_sp_move_rate * dt); + _att_sp.R_valid = false; + publish_att_sp = true; + } + } + + /* reset yaw setpint to current position if needed */ + if (reset_yaw_sp) { + reset_yaw_sp = false; + _att_sp.yaw_body = _att.yaw; + _att_sp.R_valid = false; + publish_att_sp = true; + } + + if (!_control_mode.flag_control_velocity_enabled) { + /* update attitude setpoint if not in position control mode */ + _att_sp.roll_body = _manual.roll; + _att_sp.pitch_body = _manual.pitch; + _att_sp.R_valid = false; + publish_att_sp = true; + } + + } else { + /* manual rate inputs (ACRO) */ + // TODO + /* reset yaw setpoint after ACRO */ + reset_yaw_sp = true; + } + + } else { + /* reset yaw setpoint after non-manual control */ + reset_yaw_sp = true; + } + + if (_att_sp.R_valid) { + /* rotation matrix in _att_sp is valid, use it */ + R_sp.set(&_att_sp.R_body[0][0]); + + } else { + /* rotation matrix in _att_sp is not valid, use euler angles instead */ + R_sp.from_euler(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body); + + /* copy rotation matrix back to setpoint struct */ + memcpy(&_att_sp.R_body[0][0], &R_sp.data[0][0], sizeof(_att_sp.R_body)); + _att_sp.R_valid = true; + } + + if (publish_att_sp) { + /* publish the attitude setpoint */ + _att_sp.timestamp = hrt_absolute_time(); + + if (_att_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); + + } else { + _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); + } + } + + /* rotation matrix for current state */ + R.set(_att.R); + + /* current body angular rates */ + rates(0) = _att.rollspeed; + rates(1) = _att.pitchspeed; + rates(2) = _att.yawspeed; + + /* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */ + math::Vector<3> R_z(R(0, 2), R(1, 2), R(2, 2)); + math::Vector<3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2)); + + /* axis and sin(angle) of desired rotation */ + math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z); + + /* calculate angle error */ + float e_R_z_sin = e_R.length(); + float e_R_z_cos = R_z * R_sp_z; + + /* calculate weight for yaw control */ + float yaw_w = R_sp(2, 2) * R_sp(2, 2); + + /* calculate rotation matrix after roll/pitch only rotation */ + math::Matrix<3, 3> R_rp; + + if (e_R_z_sin > 0.0f) { + /* get axis-angle representation */ + float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos); + math::Vector<3> e_R_z_axis = e_R / e_R_z_sin; + + e_R = e_R_z_axis * e_R_z_angle; + + /* cross product matrix for e_R_axis */ + math::Matrix<3, 3> e_R_cp; + e_R_cp.zero(); + e_R_cp(0, 1) = -e_R_z_axis(2); + e_R_cp(0, 2) = e_R_z_axis(1); + e_R_cp(1, 0) = e_R_z_axis(2); + e_R_cp(1, 2) = -e_R_z_axis(0); + e_R_cp(2, 0) = -e_R_z_axis(1); + e_R_cp(2, 1) = e_R_z_axis(0); + + /* rotation matrix for roll/pitch only rotation */ + R_rp = R * (I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos)); + + } else { + /* zero roll/pitch rotation */ + R_rp = R; + } + + /* R_rp and R_sp has the same Z axis, calculate yaw error */ + math::Vector<3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0)); + math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0)); + e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w; + + if (e_R_z_cos < 0.0f) { + /* for large thrust vector rotations use another rotation method: + * calculate angle and axis for R -> R_sp rotation directly */ + q.from_dcm(R.transposed() * R_sp); + math::Vector<3> e_R_d = q.imag(); + e_R_d.normalize(); + e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0)); + + /* use fusion of Z axis based rotation and direct rotation */ + float direct_w = e_R_z_cos * e_R_z_cos * yaw_w; + e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w; + } + + /* angular rates setpoint*/ + math::Vector<3> rates_sp = _params.p.emult(e_R); + + /* feed forward yaw setpoint rate */ + rates_sp(2) += yaw_sp_move_rate * yaw_w; + + /* reset integral if disarmed */ + // TODO add LANDED flag here + if (!_arming.armed) { + rates_int.zero(); + } + + /* rate controller */ + math::Vector<3> rates_err = rates_sp - rates; + math::Vector<3> control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / fmaxf(dt, 0.003f) + rates_int; + _rates_prev = rates; + + /* update integral */ + for (int i = 0; i < 3; i++) { + float rate_i = rates_int(i) + _params.rate_i(i) * rates_err(i) * dt; + + if (isfinite(rate_i)) { + if (rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && control(i) > -RATES_I_LIMIT && control(i) < RATES_I_LIMIT) { + rates_int(i) = rate_i; + } + } + } + + /* publish the attitude rates setpoint */ + _rates_sp.roll = rates_sp(0); + _rates_sp.pitch = rates_sp(1); + _rates_sp.yaw = rates_sp(2); + _rates_sp.thrust = _att_sp.thrust; + _rates_sp.timestamp = hrt_absolute_time(); + + if (_rates_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_rates_setpoint), _rates_sp_pub, &_rates_sp); + + } else { + _rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_rates_sp); + } + + /* publish the attitude controls */ + if (_control_mode.flag_control_rates_enabled) { + _actuators.control[0] = (isfinite(control(0))) ? control(0) : 0.0f; + _actuators.control[1] = (isfinite(control(1))) ? control(1) : 0.0f; + _actuators.control[2] = (isfinite(control(2))) ? control(2) : 0.0f; + _actuators.control[3] = (isfinite(_rates_sp.thrust)) ? _rates_sp.thrust : 0.0f; + _actuators.timestamp = hrt_absolute_time(); + + } else { + /* controller disabled, publish zero attitude controls */ + _actuators.control[0] = 0.0f; + _actuators.control[1] = 0.0f; + _actuators.control[2] = 0.0f; + _actuators.control[3] = 0.0f; + _actuators.timestamp = hrt_absolute_time(); + } + + if (_actuators_0_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); + + } else { + /* advertise and publish */ + _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); + } + } + + perf_end(_loop_perf); + } + + warnx("exit"); + + _control_task = -1; + _exit(0); +} + +int +MulticopterAttitudeControl::start() +{ + ASSERT(_control_task == -1); + + /* start the task */ + _control_task = task_spawn_cmd("mc_att_control", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2048, + (main_t)&MulticopterAttitudeControl::task_main_trampoline, + nullptr); + + if (_control_task < 0) { + warn("task start failed"); + return -errno; + } + + return OK; +} + +int mc_att_control_main(int argc, char *argv[]) +{ + if (argc < 1) + errx(1, "usage: mc_att_control {start|stop|status}"); + + if (!strcmp(argv[1], "start")) { + + if (att_control::g_control != nullptr) + errx(1, "already running"); + + att_control::g_control = new MulticopterAttitudeControl; + + if (att_control::g_control == nullptr) + errx(1, "alloc failed"); + + if (OK != att_control::g_control->start()) { + delete att_control::g_control; + att_control::g_control = nullptr; + err(1, "start failed"); + } + + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + if (att_control::g_control == nullptr) + errx(1, "not running"); + + delete att_control::g_control; + att_control::g_control = nullptr; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (att_control::g_control) { + errx(0, "running"); + + } else { + errx(1, "not running"); + } + } + + warnx("unrecognized command"); + return 1; +} diff --git a/src/lib/mathlib/math/arm/Vector.cpp b/src/modules/mc_att_control/mc_att_control_params.c similarity index 68% rename from src/lib/mathlib/math/arm/Vector.cpp rename to src/modules/mc_att_control/mc_att_control_params.c index 7ea6496bb1..a170365eec 100644 --- a/src/lib/mathlib/math/arm/Vector.cpp +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -1,6 +1,9 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * Author: @author Tobias Naegeli + * @author Lorenz Meier + * @author Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,9 +35,19 @@ ****************************************************************************/ /** - * @file Vector.cpp - * - * math vector + * @file mc_att_control_params.c + * Parameters for multicopter attitude controller. */ -#include "Vector.hpp" +#include + +PARAM_DEFINE_FLOAT(MC_ATT_P, 6.0f); +PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_YAW_P, 2.0f); +PARAM_DEFINE_FLOAT(MC_YAW_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.1f); +PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.002f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); diff --git a/src/modules/multirotor_pos_control/module.mk b/src/modules/mc_att_control/module.mk similarity index 87% rename from src/modules/multirotor_pos_control/module.mk rename to src/modules/mc_att_control/module.mk index bc4b48fb40..64b876f69b 100644 --- a/src/modules/multirotor_pos_control/module.mk +++ b/src/modules/mc_att_control/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -32,11 +32,10 @@ ############################################################################ # -# Build multirotor position control +# Multirotor attitude controller (vector based, no Euler singularities) # -MODULE_COMMAND = multirotor_pos_control +MODULE_COMMAND = mc_att_control -SRCS = multirotor_pos_control.c \ - multirotor_pos_control_params.c \ - thrust_pid.c +SRCS = mc_att_control_main.cpp \ + mc_att_control_params.c diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp new file mode 100644 index 0000000000..d3e39e3a02 --- /dev/null +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -0,0 +1,1031 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Anton Babushkin + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mc_pos_control_main.cpp + * Multicopter position controller. + * + * The controller has two loops: P loop for position error and PID loop for velocity error. + * Output of velocity controller is thrust vector that splitted to thrust direction + * (i.e. rotation matrix for multicopter orientation) and thrust module (i.e. multicopter thrust itself). + * Controller doesn't use Euler angles for work, they generated only for more human-friendly control and logging. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define TILT_COS_MAX 0.7f +#define SIGMA 0.000001f + +/** + * Multicopter position control app start / stop handling function + * + * @ingroup apps + */ +extern "C" __EXPORT int mc_pos_control_main(int argc, char *argv[]); + +class MulticopterPositionControl +{ +public: + /** + * Constructor + */ + MulticopterPositionControl(); + + /** + * Destructor, also kills task. + */ + ~MulticopterPositionControl(); + + /** + * Start task. + * + * @return OK on success. + */ + int start(); + +private: + + bool _task_should_exit; /**< if true, task should exit */ + int _control_task; /**< task handle for task */ + + int _att_sub; /**< vehicle attitude subscription */ + int _att_sp_sub; /**< vehicle attitude setpoint */ + int _control_mode_sub; /**< vehicle control mode subscription */ + int _params_sub; /**< notification of parameter updates */ + int _manual_sub; /**< notification of manual control updates */ + int _arming_sub; /**< arming status of outputs */ + int _local_pos_sub; /**< vehicle local position */ + int _pos_sp_triplet_sub; /**< position setpoint triplet */ + + orb_advert_t _local_pos_sp_pub; /**< local position setpoint publication */ + orb_advert_t _att_sp_pub; /**< attitude setpoint publication */ + orb_advert_t _global_vel_sp_pub; /**< vehicle global velocity setpoint */ + + struct vehicle_attitude_s _att; /**< vehicle attitude */ + struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ + struct manual_control_setpoint_s _manual; /**< r/c channel data */ + struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ + struct actuator_armed_s _arming; /**< actuator arming status */ + struct vehicle_local_position_s _local_pos; /**< vehicle local position */ + struct vehicle_local_position_setpoint_s _local_pos_sp; /**< vehicle local position */ + struct position_setpoint_triplet_s _pos_sp_triplet; /**< vehicle global position setpoint triplet */ + struct vehicle_global_velocity_setpoint_s _global_vel_sp; /**< vehicle global velocity setpoint */ + + struct { + param_t thr_min; + param_t thr_max; + param_t z_p; + param_t z_vel_p; + param_t z_vel_i; + param_t z_vel_d; + param_t z_vel_max; + param_t z_ff; + param_t xy_p; + param_t xy_vel_p; + param_t xy_vel_i; + param_t xy_vel_d; + param_t xy_vel_max; + param_t xy_ff; + param_t tilt_max; + param_t land_speed; + param_t land_tilt_max; + + param_t rc_scale_pitch; + param_t rc_scale_roll; + param_t rc_scale_yaw; + } _params_handles; /**< handles for interesting parameters */ + + struct { + float thr_min; + float thr_max; + float tilt_max; + float land_speed; + float land_tilt_max; + + float rc_scale_pitch; + float rc_scale_roll; + float rc_scale_yaw; + + math::Vector<3> pos_p; + math::Vector<3> vel_p; + math::Vector<3> vel_i; + math::Vector<3> vel_d; + math::Vector<3> vel_ff; + math::Vector<3> vel_max; + math::Vector<3> sp_offs_max; + } _params; + + math::Vector<3> _pos; + math::Vector<3> _vel; + math::Vector<3> _pos_sp; + math::Vector<3> _vel_sp; + math::Vector<3> _vel_prev; /**< velocity on previous step */ + + /** + * Update our local parameter cache. + */ + int parameters_update(bool force); + + /** + * Update control outputs + */ + void control_update(); + + /** + * Check for changes in subscribed topics. + */ + void poll_subscriptions(); + + static float scale_control(float ctl, float end, float dz); + + /** + * Shim for calling task_main from task_create. + */ + static void task_main_trampoline(int argc, char *argv[]); + + /** + * Main sensor collection task. + */ + void task_main() __attribute__((noreturn)); +}; + +namespace pos_control +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +MulticopterPositionControl *g_control; +} + +MulticopterPositionControl::MulticopterPositionControl() : + + _task_should_exit(false), + _control_task(-1), + +/* subscriptions */ + _att_sub(-1), + _att_sp_sub(-1), + _control_mode_sub(-1), + _params_sub(-1), + _manual_sub(-1), + _arming_sub(-1), + _local_pos_sub(-1), + _pos_sp_triplet_sub(-1), + +/* publications */ + _local_pos_sp_pub(-1), + _att_sp_pub(-1), + _global_vel_sp_pub(-1) +{ + memset(&_att, 0, sizeof(_att)); + memset(&_att_sp, 0, sizeof(_att_sp)); + memset(&_manual, 0, sizeof(_manual)); + memset(&_control_mode, 0, sizeof(_control_mode)); + memset(&_arming, 0, sizeof(_arming)); + memset(&_local_pos, 0, sizeof(_local_pos)); + memset(&_local_pos_sp, 0, sizeof(_local_pos_sp)); + memset(&_pos_sp_triplet, 0, sizeof(_pos_sp_triplet)); + memset(&_global_vel_sp, 0, sizeof(_global_vel_sp)); + + _params.pos_p.zero(); + _params.vel_p.zero(); + _params.vel_i.zero(); + _params.vel_d.zero(); + _params.vel_max.zero(); + _params.vel_ff.zero(); + _params.sp_offs_max.zero(); + + _pos.zero(); + _vel.zero(); + _pos_sp.zero(); + _vel_sp.zero(); + _vel_prev.zero(); + + _params_handles.thr_min = param_find("MPC_THR_MIN"); + _params_handles.thr_max = param_find("MPC_THR_MAX"); + _params_handles.z_p = param_find("MPC_Z_P"); + _params_handles.z_vel_p = param_find("MPC_Z_VEL_P"); + _params_handles.z_vel_i = param_find("MPC_Z_VEL_I"); + _params_handles.z_vel_d = param_find("MPC_Z_VEL_D"); + _params_handles.z_vel_max = param_find("MPC_Z_VEL_MAX"); + _params_handles.z_ff = param_find("MPC_Z_FF"); + _params_handles.xy_p = param_find("MPC_XY_P"); + _params_handles.xy_vel_p = param_find("MPC_XY_VEL_P"); + _params_handles.xy_vel_i = param_find("MPC_XY_VEL_I"); + _params_handles.xy_vel_d = param_find("MPC_XY_VEL_D"); + _params_handles.xy_vel_max = param_find("MPC_XY_VEL_MAX"); + _params_handles.xy_ff = param_find("MPC_XY_FF"); + _params_handles.tilt_max = param_find("MPC_TILT_MAX"); + _params_handles.land_speed = param_find("MPC_LAND_SPEED"); + _params_handles.land_tilt_max = param_find("MPC_LAND_TILT"); + _params_handles.rc_scale_pitch = param_find("RC_SCALE_PITCH"); + _params_handles.rc_scale_roll = param_find("RC_SCALE_ROLL"); + _params_handles.rc_scale_yaw = param_find("RC_SCALE_YAW"); + + /* fetch initial parameter values */ + parameters_update(true); +} + +MulticopterPositionControl::~MulticopterPositionControl() +{ + if (_control_task != -1) { + /* task wakes up every 100ms or so at the longest */ + _task_should_exit = true; + + /* wait for a second for the task to quit at our request */ + unsigned i = 0; + + do { + /* wait 20ms */ + usleep(20000); + + /* if we have given up, kill it */ + if (++i > 50) { + task_delete(_control_task); + break; + } + } while (_control_task != -1); + } + + pos_control::g_control = nullptr; +} + +int +MulticopterPositionControl::parameters_update(bool force) +{ + bool updated; + struct parameter_update_s param_upd; + + orb_check(_params_sub, &updated); + + if (updated) + orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_upd); + + if (updated || force) { + param_get(_params_handles.thr_min, &_params.thr_min); + param_get(_params_handles.thr_max, &_params.thr_max); + param_get(_params_handles.tilt_max, &_params.tilt_max); + param_get(_params_handles.land_speed, &_params.land_speed); + param_get(_params_handles.land_tilt_max, &_params.land_tilt_max); + param_get(_params_handles.rc_scale_pitch, &_params.rc_scale_pitch); + param_get(_params_handles.rc_scale_roll, &_params.rc_scale_roll); + param_get(_params_handles.rc_scale_yaw, &_params.rc_scale_yaw); + + float v; + param_get(_params_handles.xy_p, &v); + _params.pos_p(0) = v; + _params.pos_p(1) = v; + param_get(_params_handles.z_p, &v); + _params.pos_p(2) = v; + param_get(_params_handles.xy_vel_p, &v); + _params.vel_p(0) = v; + _params.vel_p(1) = v; + param_get(_params_handles.z_vel_p, &v); + _params.vel_p(2) = v; + param_get(_params_handles.xy_vel_i, &v); + _params.vel_i(0) = v; + _params.vel_i(1) = v; + param_get(_params_handles.z_vel_i, &v); + _params.vel_i(2) = v; + param_get(_params_handles.xy_vel_d, &v); + _params.vel_d(0) = v; + _params.vel_d(1) = v; + param_get(_params_handles.z_vel_d, &v); + _params.vel_d(2) = v; + param_get(_params_handles.xy_vel_max, &v); + _params.vel_max(0) = v; + _params.vel_max(1) = v; + param_get(_params_handles.z_vel_max, &v); + _params.vel_max(2) = v; + param_get(_params_handles.xy_ff, &v); + _params.vel_ff(0) = v; + _params.vel_ff(1) = v; + param_get(_params_handles.z_ff, &v); + _params.vel_ff(2) = v; + + _params.sp_offs_max = _params.vel_max.edivide(_params.pos_p) * 2.0f; + } + + return OK; +} + +void +MulticopterPositionControl::poll_subscriptions() +{ + bool updated; + + orb_check(_att_sub, &updated); + + if (updated) + orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); + + orb_check(_att_sp_sub, &updated); + + if (updated) + orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp); + + orb_check(_control_mode_sub, &updated); + + if (updated) + orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); + + orb_check(_manual_sub, &updated); + + if (updated) + orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual); + + orb_check(_arming_sub, &updated); + + if (updated) + orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming); + + orb_check(_pos_sp_triplet_sub, &updated); + + if (updated) + orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); +} + +float +MulticopterPositionControl::scale_control(float ctl, float end, float dz) +{ + if (ctl > dz) { + return (ctl - dz) / (end - dz); + + } else if (ctl < -dz) { + return (ctl + dz) / (end - dz); + + } else { + return 0.0f; + } +} + +void +MulticopterPositionControl::task_main_trampoline(int argc, char *argv[]) +{ + pos_control::g_control->task_main(); +} + +void +MulticopterPositionControl::task_main() +{ + warnx("started"); + + static int mavlink_fd; + mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + mavlink_log_info(mavlink_fd, "[mpc] started"); + + /* + * do subscriptions + */ + _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + _params_sub = orb_subscribe(ORB_ID(parameter_update)); + _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + _arming_sub = orb_subscribe(ORB_ID(actuator_armed)); + _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); + _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); + + parameters_update(true); + + /* initialize values of critical structs until first regular update */ + _arming.armed = false; + + /* get an initial update for all sensor and status data */ + poll_subscriptions(); + + bool reset_sp_z = true; + bool reset_sp_xy = true; + bool reset_int_z = true; + bool reset_int_z_manual = false; + bool reset_int_xy = true; + bool was_armed = false; + + hrt_abstime t_prev = 0; + + const float alt_ctl_dz = 0.2f; + const float pos_ctl_dz = 0.05f; + + hrt_abstime ref_timestamp = 0; + int32_t ref_lat = 0.0f; + int32_t ref_lon = 0.0f; + float ref_alt = 0.0f; + + math::Vector<3> sp_move_rate; + sp_move_rate.zero(); + math::Vector<3> thrust_int; + thrust_int.zero(); + math::Matrix<3, 3> R; + R.identity(); + + /* wakeup source */ + struct pollfd fds[1]; + + /* Setup of loop */ + fds[0].fd = _local_pos_sub; + fds[0].events = POLLIN; + + while (!_task_should_exit) { + /* wait for up to 500ms for data */ + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500); + + /* timed out - periodic check for _task_should_exit */ + if (pret == 0) + continue; + + /* this is undesirable but not much we can do */ + if (pret < 0) { + warn("poll error %d, %d", pret, errno); + continue; + } + + orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos); + + poll_subscriptions(); + parameters_update(false); + + hrt_abstime t = hrt_absolute_time(); + float dt = t_prev != 0 ? (t - t_prev) * 0.000001f : 0.0f; + t_prev = t; + + if (_control_mode.flag_armed && !was_armed) { + /* reset setpoints and integrals on arming */ + reset_sp_z = true; + reset_sp_xy = true; + reset_int_z = true; + reset_int_xy = true; + } + + was_armed = _control_mode.flag_armed; + + if (_control_mode.flag_control_altitude_enabled || + _control_mode.flag_control_position_enabled || + _control_mode.flag_control_climb_rate_enabled || + _control_mode.flag_control_velocity_enabled) { + + _pos(0) = _local_pos.x; + _pos(1) = _local_pos.y; + _pos(2) = _local_pos.z; + _vel(0) = _local_pos.vx; + _vel(1) = _local_pos.vy; + _vel(2) = _local_pos.vz; + + sp_move_rate.zero(); + + if (_local_pos.ref_timestamp != ref_timestamp) { + /* initialize local projection with new reference */ + double lat_home = _local_pos.ref_lat * 1e-7; + double lon_home = _local_pos.ref_lon * 1e-7; + map_projection_init(lat_home, lon_home); + mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", (double)lat_home, (double)lon_home); + + if (_control_mode.flag_control_manual_enabled && ref_timestamp != 0) { + /* correct setpoint in manual mode to stay in the same point */ + float ref_change_x = 0.0f; + float ref_change_y = 0.0f; + map_projection_project(ref_lat, ref_lon, &ref_change_x, &ref_change_y); + _pos_sp(0) += ref_change_x; + _pos_sp(1) += ref_change_y; + _pos_sp(2) += _local_pos.ref_alt - ref_alt; + } + + ref_timestamp = _local_pos.ref_timestamp; + ref_lat = _local_pos.ref_lat; + ref_lon = _local_pos.ref_lon; + ref_alt = _local_pos.ref_alt; + } + + if (_control_mode.flag_control_manual_enabled) { + /* manual control */ + if (_control_mode.flag_control_altitude_enabled) { + /* reset setpoint Z to current altitude if needed */ + if (reset_sp_z) { + reset_sp_z = false; + _pos_sp(2) = _pos(2); + mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double) - _pos_sp(2)); + } + + /* move altitude setpoint with throttle stick */ + sp_move_rate(2) = -scale_control(_manual.throttle - 0.5f, 0.5f, alt_ctl_dz); + } + + if (_control_mode.flag_control_position_enabled) { + /* reset setpoint XY to current position if needed */ + if (reset_sp_xy) { + reset_sp_xy = false; + _pos_sp(0) = _pos(0); + _pos_sp(1) = _pos(1); + mavlink_log_info(mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1)); + } + + /* move position setpoint with roll/pitch stick */ + sp_move_rate(0) = scale_control(-_manual.pitch / _params.rc_scale_pitch, 1.0f, pos_ctl_dz); + sp_move_rate(1) = scale_control(_manual.roll / _params.rc_scale_roll, 1.0f, pos_ctl_dz); + } + + /* limit setpoint move rate */ + float sp_move_norm = sp_move_rate.length(); + + if (sp_move_norm > 1.0f) { + sp_move_rate /= sp_move_norm; + } + + /* scale to max speed and rotate around yaw */ + math::Matrix<3, 3> R_yaw_sp; + R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body); + sp_move_rate = R_yaw_sp * sp_move_rate.emult(_params.vel_max); + + /* move position setpoint */ + _pos_sp += sp_move_rate * dt; + + /* check if position setpoint is too far from actual position */ + math::Vector<3> pos_sp_offs; + pos_sp_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); + } + + } else { + /* AUTO */ + if (_pos_sp_triplet.current.valid) { + struct position_setpoint_s current_sp = _pos_sp_triplet.current; + + _pos_sp(2) = -(current_sp.alt - ref_alt); + + map_projection_project(current_sp.lat, current_sp.lon, &_pos_sp(0), &_pos_sp(1)); + + if (isfinite(current_sp.yaw)) { + _att_sp.yaw_body = current_sp.yaw; + } + + /* in case of interrupted mission don't go to waypoint but stay at current position */ + reset_sp_xy = true; + reset_sp_z = true; + + } else { + /* no waypoint, loiter, reset position setpoint if needed */ + if (reset_sp_xy) { + reset_sp_xy = false; + _pos_sp(0) = _pos(0); + _pos_sp(1) = _pos(1); + } + + if (reset_sp_z) { + reset_sp_z = false; + _pos_sp(2) = _pos(2); + } + } + } + + /* copy resulting setpoint to vehicle_local_position_setpoint topic for logging */ + _local_pos_sp.yaw = _att_sp.yaw_body; + _local_pos_sp.x = _pos_sp(0); + _local_pos_sp.y = _pos_sp(1); + _local_pos_sp.z = _pos_sp(2); + + /* publish local position setpoint */ + if (_local_pos_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &_local_pos_sp); + + } else { + _local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp); + } + + /* run position & altitude controllers, calculate velocity setpoint */ + math::Vector<3> pos_err = _pos_sp - _pos; + _vel_sp = pos_err.emult(_params.pos_p) + sp_move_rate.emult(_params.vel_ff); + + if (!_control_mode.flag_control_altitude_enabled) { + reset_sp_z = true; + _vel_sp(2) = 0.0f; + } + + if (!_control_mode.flag_control_position_enabled) { + reset_sp_xy = true; + _vel_sp(0) = 0.0f; + _vel_sp(1) = 0.0f; + } + + if (!_control_mode.flag_control_manual_enabled) { + /* use constant descend rate when landing, ignore altitude setpoint */ + if (_pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { + _vel_sp(2) = _params.land_speed; + } + + /* limit 3D speed only in AUTO mode */ + float vel_sp_norm = _vel_sp.edivide(_params.vel_max).length(); + + if (vel_sp_norm > 1.0f) { + _vel_sp /= vel_sp_norm; + } + } + + _global_vel_sp.vx = _vel_sp(0); + _global_vel_sp.vy = _vel_sp(1); + _global_vel_sp.vz = _vel_sp(2); + + /* publish velocity setpoint */ + if (_global_vel_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &_global_vel_sp); + + } else { + _global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &_global_vel_sp); + } + + if (_control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled) { + /* reset integrals if needed */ + if (_control_mode.flag_control_climb_rate_enabled) { + if (reset_int_z) { + reset_int_z = false; + float i = _params.thr_min; + + if (reset_int_z_manual) { + i = _manual.throttle; + + if (i < _params.thr_min) { + i = _params.thr_min; + + } else if (i > _params.thr_max) { + i = _params.thr_max; + } + } + + thrust_int(2) = -i; + mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i); + } + + } else { + reset_int_z = true; + } + + if (_control_mode.flag_control_velocity_enabled) { + if (reset_int_xy) { + reset_int_xy = false; + thrust_int(0) = 0.0f; + thrust_int(1) = 0.0f; + mavlink_log_info(mavlink_fd, "[mpc] reset xy vel integral"); + } + + } else { + reset_int_xy = true; + } + + /* velocity error */ + math::Vector<3> vel_err = _vel_sp - _vel; + + /* derivative of velocity error, not includes setpoint acceleration */ + math::Vector<3> vel_err_d = (sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt; + _vel_prev = _vel; + + /* thrust vector in NED frame */ + math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + vel_err_d.emult(_params.vel_d) + thrust_int; + + if (!_control_mode.flag_control_velocity_enabled) { + thrust_sp(0) = 0.0f; + thrust_sp(1) = 0.0f; + } + + if (!_control_mode.flag_control_climb_rate_enabled) { + thrust_sp(2) = 0.0f; + } + + /* limit thrust vector and check for saturation */ + bool saturation_xy = false; + bool saturation_z = false; + + /* limit min lift */ + float thr_min = _params.thr_min; + + if (!_control_mode.flag_control_velocity_enabled && thr_min < 0.0f) { + /* don't allow downside thrust direction in manual attitude mode */ + thr_min = 0.0f; + } + + if (-thrust_sp(2) < thr_min) { + thrust_sp(2) = -thr_min; + saturation_z = true; + } + + /* limit max tilt */ + float tilt = atan2f(math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(), -thrust_sp(2)); + float tilt_max = _params.tilt_max; + if (!_control_mode.flag_control_manual_enabled) { + if (_pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { + /* limit max tilt and min lift when landing */ + tilt_max = _params.land_tilt_max; + if (thr_min < 0.0f) + thr_min = 0.0f; + } + } + + if (_control_mode.flag_control_velocity_enabled) { + if (tilt > tilt_max && thr_min >= 0.0f) { + /* crop horizontal component */ + float k = tanf(tilt_max) / tanf(tilt); + thrust_sp(0) *= k; + thrust_sp(1) *= k; + saturation_xy = true; + } + } else { + /* thrust compensation for altitude only control mode */ + float att_comp; + + if (_att.R[2][2] > TILT_COS_MAX) { + att_comp = 1.0f / _att.R[2][2]; + } else if (_att.R[2][2] > 0.0f) { + att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _att.R[2][2] + 1.0f; + saturation_z = true; + } else { + att_comp = 1.0f; + saturation_z = true; + } + + thrust_sp(2) *= att_comp; + } + + /* limit max thrust */ + float thrust_abs = thrust_sp.length(); + + if (thrust_abs > _params.thr_max) { + if (thrust_sp(2) < 0.0f) { + if (-thrust_sp(2) > _params.thr_max) { + /* thrust Z component is too large, limit it */ + thrust_sp(0) = 0.0f; + thrust_sp(1) = 0.0f; + thrust_sp(2) = -_params.thr_max; + saturation_xy = true; + saturation_z = true; + + } else { + /* preserve thrust Z component and lower XY, keeping altitude is more important than position */ + float thrust_xy_max = sqrtf(_params.thr_max * _params.thr_max - thrust_sp(2) * thrust_sp(2)); + float thrust_xy_abs = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(); + float k = thrust_xy_max / thrust_xy_abs; + thrust_sp(0) *= k; + thrust_sp(1) *= k; + saturation_xy = true; + } + + } else { + /* Z component is negative, going down, simply limit thrust vector */ + float k = _params.thr_max / thrust_abs; + thrust_sp *= k; + saturation_xy = true; + saturation_z = true; + } + + thrust_abs = _params.thr_max; + } + + /* update integrals */ + if (_control_mode.flag_control_velocity_enabled && !saturation_xy) { + thrust_int(0) += vel_err(0) * _params.vel_i(0) * dt; + thrust_int(1) += vel_err(1) * _params.vel_i(1) * dt; + } + + if (_control_mode.flag_control_climb_rate_enabled && !saturation_z) { + thrust_int(2) += vel_err(2) * _params.vel_i(2) * dt; + + /* protection against flipping on ground when landing */ + if (thrust_int(2) > 0.0f) + thrust_int(2) = 0.0f; + } + + /* calculate attitude setpoint from thrust vector */ + if (_control_mode.flag_control_velocity_enabled) { + /* desired body_z axis = -normalize(thrust_vector) */ + math::Vector<3> body_x; + math::Vector<3> body_y; + math::Vector<3> body_z; + + if (thrust_abs > SIGMA) { + body_z = -thrust_sp / thrust_abs; + + } else { + /* no thrust, set Z axis to safe value */ + body_z.zero(); + body_z(2) = 1.0f; + } + + /* vector of desired yaw direction in XY plane, rotated by PI/2 */ + math::Vector<3> y_C(-sinf(_att_sp.yaw_body), cosf(_att_sp.yaw_body), 0.0f); + + if (fabsf(body_z(2)) > SIGMA) { + /* desired body_x axis, orthogonal to body_z */ + body_x = y_C % body_z; + + /* keep nose to front while inverted upside down */ + if (body_z(2) < 0.0f) { + body_x = -body_x; + } + + body_x.normalize(); + + } else { + /* desired thrust is in XY plane, set X downside to construct correct matrix, + * but yaw component will not be used actually */ + body_x.zero(); + body_x(2) = 1.0f; + } + + /* desired body_y axis */ + body_y = body_z % body_x; + + /* fill rotation matrix */ + for (int i = 0; i < 3; i++) { + R(i, 0) = body_x(i); + R(i, 1) = body_y(i); + R(i, 2) = body_z(i); + } + + /* copy rotation matrix to attitude setpoint topic */ + memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body)); + _att_sp.R_valid = true; + + /* calculate euler angles, for logging only, must not be used for control */ + math::Vector<3> euler = R.to_euler(); + _att_sp.roll_body = euler(0); + _att_sp.pitch_body = euler(1); + /* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */ + } + + _att_sp.thrust = thrust_abs; + + _att_sp.timestamp = hrt_absolute_time(); + + /* publish attitude setpoint */ + if (_att_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); + + } else { + _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); + } + + } else { + reset_int_z = true; + } + + } else { + /* position controller disabled, reset setpoints */ + reset_sp_z = true; + reset_sp_xy = true; + reset_int_z = true; + reset_int_xy = true; + } + + /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ + reset_int_z_manual = _control_mode.flag_armed && _control_mode.flag_control_manual_enabled && !_control_mode.flag_control_climb_rate_enabled; + } + + warnx("stopped"); + mavlink_log_info(mavlink_fd, "[mpc] stopped"); + + _control_task = -1; + _exit(0); +} + +int +MulticopterPositionControl::start() +{ + ASSERT(_control_task == -1); + + /* start the task */ + _control_task = task_spawn_cmd("mc_pos_control", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2048, + (main_t)&MulticopterPositionControl::task_main_trampoline, + nullptr); + + if (_control_task < 0) { + warn("task start failed"); + return -errno; + } + + return OK; +} + +int mc_pos_control_main(int argc, char *argv[]) +{ + if (argc < 1) + errx(1, "usage: mc_pos_control {start|stop|status}"); + + if (!strcmp(argv[1], "start")) { + + if (pos_control::g_control != nullptr) + errx(1, "already running"); + + pos_control::g_control = new MulticopterPositionControl; + + if (pos_control::g_control == nullptr) + errx(1, "alloc failed"); + + if (OK != pos_control::g_control->start()) { + delete pos_control::g_control; + pos_control::g_control = nullptr; + err(1, "start failed"); + } + + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + if (pos_control::g_control == nullptr) + errx(1, "not running"); + + delete pos_control::g_control; + pos_control::g_control = nullptr; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (pos_control::g_control) { + errx(0, "running"); + + } else { + errx(1, "not running"); + } + } + + warnx("unrecognized command"); + return 1; +} diff --git a/src/lib/mathlib/math/Vector2f.hpp b/src/modules/mc_pos_control/mc_pos_control_params.c similarity index 67% rename from src/lib/mathlib/math/Vector2f.hpp rename to src/modules/mc_pos_control/mc_pos_control_params.c index ecd62e81cb..9eb56545d7 100644 --- a/src/lib/mathlib/math/Vector2f.hpp +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -1,6 +1,7 @@ /**************************************************************************** * * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,48 +33,26 @@ ****************************************************************************/ /** - * @file Vector2f.hpp - * - * math 3 vector + * @file mc_pos_control_params.c + * Multicopter position controller parameters. */ -#pragma once - -#include "Vector.hpp" - -namespace math -{ - -class __EXPORT Vector2f : - public Vector -{ -public: - Vector2f(); - Vector2f(const Vector &right); - Vector2f(float x, float y); - Vector2f(const float *data); - virtual ~Vector2f(); - float cross(const Vector2f &b) const; - float operator %(const Vector2f &v) const; - float operator *(const Vector2f &v) const; - inline Vector2f operator*(const float &right) const { - return Vector::operator*(right); - } - - /** - * accessors - */ - void setX(float x) { (*this)(0) = x; } - void setY(float y) { (*this)(1) = y; } - const float &getX() const { return (*this)(0); } - const float &getY() const { return (*this)(1); } -}; - -class __EXPORT Vector2 : - public Vector2f -{ -}; - -int __EXPORT vector2fTest(); -} // math +#include +PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.0f); +PARAM_DEFINE_FLOAT(MPC_THR_MAX, 1.0f); +PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.1f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_I, 0.02f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 5.0f); +PARAM_DEFINE_FLOAT(MPC_Z_FF, 0.5f); +PARAM_DEFINE_FLOAT(MPC_XY_P, 1.0f); +PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.1f); +PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.02f); +PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.01f); +PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f); +PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f); +PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 1.0f); +PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 1.0f); +PARAM_DEFINE_FLOAT(MPC_LAND_TILT, 0.3f); diff --git a/src/modules/multirotor_att_control/module.mk b/src/modules/mc_pos_control/module.mk old mode 100755 new mode 100644 similarity index 86% rename from src/modules/multirotor_att_control/module.mk rename to src/modules/mc_pos_control/module.mk index 7569e1c7ee..0b566d7bd0 --- a/src/modules/multirotor_att_control/module.mk +++ b/src/modules/mc_pos_control/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# Copyright (c) 2013 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -32,11 +32,10 @@ ############################################################################ # -# Build multirotor attitude controller +# Build multicopter position controller # -MODULE_COMMAND = multirotor_att_control +MODULE_COMMAND = mc_pos_control -SRCS = multirotor_att_control_main.c \ - multirotor_attitude_control.c \ - multirotor_rate_control.c +SRCS = mc_pos_control_main.cpp \ + mc_pos_control_params.c diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c deleted file mode 100644 index 111e9197f6..0000000000 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ /dev/null @@ -1,465 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier - * Anton Babushkin - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file multirotor_att_control_main.c - * - * Implementation of multirotor attitude control main loop. - * - * @author Lorenz Meier - * @author Anton Babushkin - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include "multirotor_attitude_control.h" -#include "multirotor_rate_control.h" - -__EXPORT int multirotor_att_control_main(int argc, char *argv[]); - -static bool thread_should_exit; -static int mc_task; -static bool motor_test_mode = false; -static const float min_takeoff_throttle = 0.3f; -static const float yaw_deadzone = 0.01f; - -static int -mc_thread_main(int argc, char *argv[]) -{ - /* declare and safely initialize all structs */ - struct vehicle_attitude_s att; - memset(&att, 0, sizeof(att)); - struct vehicle_attitude_setpoint_s att_sp; - memset(&att_sp, 0, sizeof(att_sp)); - struct offboard_control_setpoint_s offboard_sp; - memset(&offboard_sp, 0, sizeof(offboard_sp)); - struct vehicle_control_mode_s control_mode; - memset(&control_mode, 0, sizeof(control_mode)); - struct manual_control_setpoint_s manual; - memset(&manual, 0, sizeof(manual)); - struct sensor_combined_s sensor; - memset(&sensor, 0, sizeof(sensor)); - struct vehicle_rates_setpoint_s rates_sp; - memset(&rates_sp, 0, sizeof(rates_sp)); - struct vehicle_status_s status; - memset(&status, 0, sizeof(status)); - struct actuator_controls_s actuators; - memset(&actuators, 0, sizeof(actuators)); - - /* subscribe */ - int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); - int vehicle_attitude_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - int offboard_control_setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); - int vehicle_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - int manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); - int vehicle_rates_setpoint_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); - int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); - - /* publish actuator controls */ - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { - actuators.control[i] = 0.0f; - } - - orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); - orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); - orb_advert_t rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); - - /* register the perf counter */ - perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_att_control_runtime"); - perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "multirotor_att_control_interval"); - perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "multirotor_att_control_err"); - - warnx("starting"); - - /* store last control mode to detect mode switches */ - bool control_yaw_position = true; - bool reset_yaw_sp = true; - - struct pollfd fds[1] = { - { .fd = vehicle_attitude_sub, .events = POLLIN }, - }; - - while (!thread_should_exit) { - - /* wait for a sensor update, check for exit condition every 500 ms */ - int ret = poll(fds, 1, 500); - - if (ret < 0) { - /* poll error, count it in perf */ - perf_count(mc_err_perf); - - } else if (ret > 0) { - /* only run controller if attitude changed */ - perf_begin(mc_loop_perf); - - /* attitude */ - orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); - - bool updated; - - /* parameters */ - orb_check(parameter_update_sub, &updated); - - if (updated) { - struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); - /* update parameters */ - } - - /* control mode */ - orb_check(vehicle_control_mode_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_control_mode), vehicle_control_mode_sub, &control_mode); - } - - /* manual control setpoint */ - orb_check(manual_control_setpoint_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(manual_control_setpoint), manual_control_setpoint_sub, &manual); - } - - /* attitude setpoint */ - orb_check(vehicle_attitude_setpoint_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_attitude_setpoint), vehicle_attitude_setpoint_sub, &att_sp); - } - - /* offboard control setpoint */ - orb_check(offboard_control_setpoint_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(offboard_control_setpoint), offboard_control_setpoint_sub, &offboard_sp); - } - - /* vehicle status */ - orb_check(vehicle_status_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &status); - } - - /* sensors */ - orb_check(sensor_combined_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); - } - - /* set flag to safe value */ - control_yaw_position = true; - - /* reset yaw setpoint if not armed */ - if (!control_mode.flag_armed) { - reset_yaw_sp = true; - } - - /* define which input is the dominating control input */ - if (control_mode.flag_control_offboard_enabled) { - /* offboard inputs */ - if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) { - rates_sp.roll = offboard_sp.p1; - rates_sp.pitch = offboard_sp.p2; - rates_sp.yaw = offboard_sp.p3; - rates_sp.thrust = offboard_sp.p4; - rates_sp.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); - - } else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) { - att_sp.roll_body = offboard_sp.p1; - att_sp.pitch_body = offboard_sp.p2; - att_sp.yaw_body = offboard_sp.p3; - att_sp.thrust = offboard_sp.p4; - att_sp.timestamp = hrt_absolute_time(); - /* publish the result to the vehicle actuators */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - } - - /* reset yaw setpoint after offboard control */ - reset_yaw_sp = true; - - } else if (control_mode.flag_control_manual_enabled) { - /* manual input */ - if (control_mode.flag_control_attitude_enabled) { - /* control attitude, update attitude setpoint depending on mode */ - if (att_sp.thrust < 0.1f) { - /* no thrust, don't try to control yaw */ - rates_sp.yaw = 0.0f; - control_yaw_position = false; - - if (status.condition_landed) { - /* reset yaw setpoint if on ground */ - reset_yaw_sp = true; - } - - } else { - /* only move yaw setpoint if manual input is != 0 */ - if (manual.yaw < -yaw_deadzone || yaw_deadzone < manual.yaw) { - /* control yaw rate */ - control_yaw_position = false; - rates_sp.yaw = manual.yaw; - reset_yaw_sp = true; // has no effect on control, just for beautiful log - - } else { - control_yaw_position = true; - } - } - - if (!control_mode.flag_control_velocity_enabled) { - /* update attitude setpoint if not in position control mode */ - att_sp.roll_body = manual.roll; - att_sp.pitch_body = manual.pitch; - - if (!control_mode.flag_control_climb_rate_enabled) { - /* pass throttle directly if not in altitude control mode */ - att_sp.thrust = manual.throttle; - } - } - - /* reset yaw setpint to current position if needed */ - if (reset_yaw_sp) { - att_sp.yaw_body = att.yaw; - reset_yaw_sp = false; - } - - if (motor_test_mode) { - printf("testmode"); - att_sp.roll_body = 0.0f; - att_sp.pitch_body = 0.0f; - att_sp.yaw_body = 0.0f; - att_sp.thrust = 0.1f; - } - - att_sp.timestamp = hrt_absolute_time(); - - /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - - } else { - /* manual rate inputs (ACRO), from RC control or joystick */ - if (control_mode.flag_control_rates_enabled) { - rates_sp.roll = manual.roll; - rates_sp.pitch = manual.pitch; - rates_sp.yaw = manual.yaw; - rates_sp.thrust = manual.throttle; - rates_sp.timestamp = hrt_absolute_time(); - } - - /* reset yaw setpoint after ACRO */ - reset_yaw_sp = true; - } - - } else { - if (!control_mode.flag_control_attitude_enabled) { - /* no control, try to stay on place */ - if (!control_mode.flag_control_velocity_enabled) { - /* no velocity control, reset attitude setpoint */ - att_sp.roll_body = 0.0f; - att_sp.pitch_body = 0.0f; - att_sp.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - } - } - - /* reset yaw setpoint after non-manual control */ - reset_yaw_sp = true; - } - - /* check if we should we reset integrals */ - bool reset_integral = !control_mode.flag_armed || att_sp.thrust < 0.1f; // TODO use landed status instead of throttle - - /* run attitude controller if needed */ - if (control_mode.flag_control_attitude_enabled) { - multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position, reset_integral); - orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); - } - - /* measure in what intervals the controller runs */ - perf_count(mc_interval_perf); - - /* run rates controller if needed */ - if (control_mode.flag_control_rates_enabled) { - /* get current rate setpoint */ - bool rates_sp_updated = false; - orb_check(vehicle_rates_setpoint_sub, &rates_sp_updated); - - if (rates_sp_updated) { - orb_copy(ORB_ID(vehicle_rates_setpoint), vehicle_rates_setpoint_sub, &rates_sp); - } - - /* apply controller */ - float rates[3]; - rates[0] = att.rollspeed; - rates[1] = att.pitchspeed; - rates[2] = att.yawspeed; - multirotor_control_rates(&rates_sp, rates, &actuators, reset_integral); - - } else { - /* rates controller disabled, set actuators to zero for safety */ - actuators.control[0] = 0.0f; - actuators.control[1] = 0.0f; - actuators.control[2] = 0.0f; - actuators.control[3] = 0.0f; - } - - /* fill in manual control values */ - actuators.control[4] = manual.flaps; - actuators.control[5] = manual.aux1; - actuators.control[6] = manual.aux2; - actuators.control[7] = manual.aux3; - - actuators.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); - - perf_end(mc_loop_perf); - } - } - - warnx("stopping, disarming motors"); - - /* kill all outputs */ - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) - actuators.control[i] = 0.0f; - - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); - - close(vehicle_attitude_sub); - close(vehicle_control_mode_sub); - close(manual_control_setpoint_sub); - close(actuator_pub); - close(att_sp_pub); - - perf_print_counter(mc_loop_perf); - perf_free(mc_loop_perf); - - fflush(stdout); - exit(0); -} - -static void -usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - - fprintf(stderr, "usage: multirotor_att_control [-m ] [-t] {start|status|stop}\n"); - fprintf(stderr, " is 'rates' or 'attitude'\n"); - fprintf(stderr, " -t enables motor test mode with 10%% thrust\n"); - exit(1); -} - -int multirotor_att_control_main(int argc, char *argv[]) -{ - int ch; - unsigned int optioncount = 0; - - while ((ch = getopt(argc, argv, "tm:")) != EOF) { - switch (ch) { - case 't': - motor_test_mode = true; - optioncount += 1; - break; - - case ':': - usage("missing parameter"); - break; - - default: - fprintf(stderr, "option: -%c\n", ch); - usage("unrecognized option"); - break; - } - } - - argc -= optioncount; - //argv += optioncount; - - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1 + optioncount], "start")) { - - thread_should_exit = false; - mc_task = task_spawn_cmd("multirotor_att_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 15, - 2048, - mc_thread_main, - NULL); - exit(0); - } - - if (!strcmp(argv[1 + optioncount], "stop")) { - thread_should_exit = true; - exit(0); - } - - usage("unrecognized command"); - exit(1); -} diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c deleted file mode 100644 index 8245aa5609..0000000000 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.c +++ /dev/null @@ -1,254 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes - * Laurens Mackay - * Tobias Naegeli - * Martin Rutschmann - * Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* - * @file multirotor_attitude_control.c - * - * Implementation of attitude controller for multirotors. - * - * @author Thomas Gubler - * @author Julian Oes - * @author Laurens Mackay - * @author Tobias Naegeli - * @author Martin Rutschmann - * @author Lorenz Meier - */ - -#include "multirotor_attitude_control.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 2.0f); -PARAM_DEFINE_FLOAT(MC_YAWPOS_I, 0.15f); -PARAM_DEFINE_FLOAT(MC_YAWPOS_D, 0.0f); -//PARAM_DEFINE_FLOAT(MC_YAWPOS_AWU, 1.0f); -//PARAM_DEFINE_FLOAT(MC_YAWPOS_LIM, 3.0f); - -PARAM_DEFINE_FLOAT(MC_ATT_P, 6.8f); -PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f); -PARAM_DEFINE_FLOAT(MC_ATT_D, 0.0f); -//PARAM_DEFINE_FLOAT(MC_ATT_AWU, 0.05f); -//PARAM_DEFINE_FLOAT(MC_ATT_LIM, 0.4f); - -//PARAM_DEFINE_FLOAT(MC_ATT_XOFF, 0.0f); -//PARAM_DEFINE_FLOAT(MC_ATT_YOFF, 0.0f); - -struct mc_att_control_params { - float yaw_p; - float yaw_i; - float yaw_d; - //float yaw_awu; - //float yaw_lim; - - float att_p; - float att_i; - float att_d; - //float att_awu; - //float att_lim; - - //float att_xoff; - //float att_yoff; -}; - -struct mc_att_control_param_handles { - param_t yaw_p; - param_t yaw_i; - param_t yaw_d; - //param_t yaw_awu; - //param_t yaw_lim; - - param_t att_p; - param_t att_i; - param_t att_d; - //param_t att_awu; - //param_t att_lim; - - //param_t att_xoff; - //param_t att_yoff; -}; - -/** - * Initialize all parameter handles and values - * - */ -static int parameters_init(struct mc_att_control_param_handles *h); - -/** - * Update all parameters - * - */ -static int parameters_update(const struct mc_att_control_param_handles *h, struct mc_att_control_params *p); - - -static int parameters_init(struct mc_att_control_param_handles *h) -{ - /* PID parameters */ - h->yaw_p = param_find("MC_YAWPOS_P"); - h->yaw_i = param_find("MC_YAWPOS_I"); - h->yaw_d = param_find("MC_YAWPOS_D"); - //h->yaw_awu = param_find("MC_YAWPOS_AWU"); - //h->yaw_lim = param_find("MC_YAWPOS_LIM"); - - h->att_p = param_find("MC_ATT_P"); - h->att_i = param_find("MC_ATT_I"); - h->att_d = param_find("MC_ATT_D"); - //h->att_awu = param_find("MC_ATT_AWU"); - //h->att_lim = param_find("MC_ATT_LIM"); - - //h->att_xoff = param_find("MC_ATT_XOFF"); - //h->att_yoff = param_find("MC_ATT_YOFF"); - - return OK; -} - -static int parameters_update(const struct mc_att_control_param_handles *h, struct mc_att_control_params *p) -{ - param_get(h->yaw_p, &(p->yaw_p)); - param_get(h->yaw_i, &(p->yaw_i)); - param_get(h->yaw_d, &(p->yaw_d)); - //param_get(h->yaw_awu, &(p->yaw_awu)); - //param_get(h->yaw_lim, &(p->yaw_lim)); - - param_get(h->att_p, &(p->att_p)); - param_get(h->att_i, &(p->att_i)); - param_get(h->att_d, &(p->att_d)); - //param_get(h->att_awu, &(p->att_awu)); - //param_get(h->att_lim, &(p->att_lim)); - - //param_get(h->att_xoff, &(p->att_xoff)); - //param_get(h->att_yoff, &(p->att_yoff)); - - return OK; -} - -void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, - const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position, bool reset_integral) -{ - static uint64_t last_run = 0; - static uint64_t last_input = 0; - float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; - last_run = hrt_absolute_time(); - - if (last_input != att_sp->timestamp) { - last_input = att_sp->timestamp; - } - - static int motor_skip_counter = 0; - - static PID_t pitch_controller; - static PID_t roll_controller; - - static struct mc_att_control_params p; - static struct mc_att_control_param_handles h; - - static bool initialized = false; - - static float yaw_error; - - /* initialize the pid controllers when the function is called for the first time */ - if (initialized == false) { - parameters_init(&h); - parameters_update(&h, &p); - - pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, PID_MODE_DERIVATIV_SET, 0.0f); - pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, PID_MODE_DERIVATIV_SET, 0.0f); - - initialized = true; - } - - /* load new parameters with lower rate */ - if (motor_skip_counter % 500 == 0) { - /* update parameters from storage */ - parameters_update(&h, &p); - - /* apply parameters */ - pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); - pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); - } - - /* reset integrals if needed */ - if (reset_integral) { - pid_reset_integral(&pitch_controller); - pid_reset_integral(&roll_controller); - //TODO pid_reset_integral(&yaw_controller); - } - - /* calculate current control outputs */ - - /* control pitch (forward) output */ - rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body , - att->pitch, att->pitchspeed, deltaT); - - /* control roll (left/right) output */ - rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body , - att->roll, att->rollspeed, deltaT); - - if (control_yaw_position) { - /* control yaw rate */ - // TODO use pid lib - - /* positive error: rotate to right, negative error, rotate to left (NED frame) */ - // yaw_error = _wrap_pi(att_sp->yaw_body - att->yaw); - - yaw_error = att_sp->yaw_body - att->yaw; - - if (yaw_error > M_PI_F) { - yaw_error -= M_TWOPI_F; - - } else if (yaw_error < -M_PI_F) { - yaw_error += M_TWOPI_F; - } - - rates_sp->yaw = p.yaw_p * (yaw_error) - (p.yaw_d * att->yawspeed); - } - - rates_sp->thrust = att_sp->thrust; - //need to update the timestamp now that we've touched rates_sp - rates_sp->timestamp = hrt_absolute_time(); - - motor_skip_counter++; -} diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.h b/src/modules/multirotor_att_control/multirotor_attitude_control.h deleted file mode 100644 index 431a435f7d..0000000000 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.h +++ /dev/null @@ -1,65 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes - * Laurens Mackay - * Tobias Naegeli - * Martin Rutschmann - * Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* - * @file multirotor_attitude_control.h - * - * Definition of attitude controller for multirotors. - * - * @author Thomas Gubler - * @author Julian Oes - * @author Laurens Mackay - * @author Tobias Naegeli - * @author Martin Rutschmann - * @author Lorenz Meier - */ - -#ifndef MULTIROTOR_ATTITUDE_CONTROL_H_ -#define MULTIROTOR_ATTITUDE_CONTROL_H_ - -#include -#include -#include -#include -#include - -void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, - const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position, bool reset_integral); - -#endif /* MULTIROTOR_ATTITUDE_CONTROL_H_ */ diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c deleted file mode 100644 index 86ac0e4ff0..0000000000 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ /dev/null @@ -1,196 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. - * Author: Tobias Naegeli - * Lorenz Meier - * Anton Babushkin - * Julian Oes - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file multirotor_rate_control.c - * - * Implementation of rate controller for multirotors. - * - * @author Tobias Naegeli - * @author Lorenz Meier - * @author Anton Babushkin - * @author Julian Oes - */ - -#include "multirotor_rate_control.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); -PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.005f); -PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.2f); - -PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.09f); -PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.002f); -PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f); - -struct mc_rate_control_params { - - float yawrate_p; - float yawrate_d; - float yawrate_i; - - float attrate_p; - float attrate_d; - float attrate_i; - - float rate_lim; -}; - -struct mc_rate_control_param_handles { - - param_t yawrate_p; - param_t yawrate_i; - param_t yawrate_d; - - param_t attrate_p; - param_t attrate_i; - param_t attrate_d; -}; - -/** - * Initialize all parameter handles and values - * - */ -static int parameters_init(struct mc_rate_control_param_handles *h); - -/** - * Update all parameters - * - */ -static int parameters_update(const struct mc_rate_control_param_handles *h, struct mc_rate_control_params *p); - - -static int parameters_init(struct mc_rate_control_param_handles *h) -{ - /* PID parameters */ - h->yawrate_p = param_find("MC_YAWRATE_P"); - h->yawrate_i = param_find("MC_YAWRATE_I"); - h->yawrate_d = param_find("MC_YAWRATE_D"); - - h->attrate_p = param_find("MC_ATTRATE_P"); - h->attrate_i = param_find("MC_ATTRATE_I"); - h->attrate_d = param_find("MC_ATTRATE_D"); - - return OK; -} - -static int parameters_update(const struct mc_rate_control_param_handles *h, struct mc_rate_control_params *p) -{ - param_get(h->yawrate_p, &(p->yawrate_p)); - param_get(h->yawrate_i, &(p->yawrate_i)); - param_get(h->yawrate_d, &(p->yawrate_d)); - - param_get(h->attrate_p, &(p->attrate_p)); - param_get(h->attrate_i, &(p->attrate_i)); - param_get(h->attrate_d, &(p->attrate_d)); - - return OK; -} - -void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, - const float rates[], struct actuator_controls_s *actuators, bool reset_integral) -{ - static uint64_t last_run = 0; - const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; - static uint64_t last_input = 0; - - if (last_input != rate_sp->timestamp) { - last_input = rate_sp->timestamp; - } - - last_run = hrt_absolute_time(); - - static int motor_skip_counter = 0; - - static PID_t pitch_rate_controller; - static PID_t roll_rate_controller; - static PID_t yaw_rate_controller; - - static struct mc_rate_control_params p; - static struct mc_rate_control_param_handles h; - - static bool initialized = false; - - /* initialize the pid controllers when the function is called for the first time */ - if (initialized == false) { - parameters_init(&h); - parameters_update(&h, &p); - initialized = true; - - pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f); - pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f); - pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f); - } - - /* load new parameters with lower rate */ - if (motor_skip_counter % 2500 == 0) { - /* update parameters from storage */ - parameters_update(&h, &p); - pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f); - pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f); - pid_set_parameters(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, 1.0f, 1.0f); - } - - /* reset integrals if needed */ - if (reset_integral) { - pid_reset_integral(&pitch_rate_controller); - pid_reset_integral(&roll_rate_controller); - pid_reset_integral(&yaw_rate_controller); - } - - /* run pitch, roll and yaw controllers */ - float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT); - float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT); - float yaw_control = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT); - - actuators->control[0] = roll_control; - actuators->control[1] = pitch_control; - actuators->control[2] = yaw_control; - actuators->control[3] = rate_sp->thrust; - - motor_skip_counter++; -} diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.h b/src/modules/multirotor_att_control/multirotor_rate_control.h deleted file mode 100644 index ca7794c59b..0000000000 --- a/src/modules/multirotor_att_control/multirotor_rate_control.h +++ /dev/null @@ -1,64 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes - * Laurens Mackay - * Tobias Naegeli - * Martin Rutschmann - * Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* - * @file multirotor_attitude_control.h - * - * Definition of rate controller for multirotors. - * - * @author Thomas Gubler - * @author Julian Oes - * @author Laurens Mackay - * @author Tobias Naegeli - * @author Martin Rutschmann - * @author Lorenz Meier - */ - -#ifndef MULTIROTOR_RATE_CONTROL_H_ -#define MULTIROTOR_RATE_CONTROL_H_ - -#include -#include -#include -#include - -void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, - const float rates[], struct actuator_controls_s *actuators, bool reset_integral); - -#endif /* MULTIROTOR_RATE_CONTROL_H_ */ diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c deleted file mode 100644 index 2ca6504208..0000000000 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ /dev/null @@ -1,553 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file multirotor_pos_control.c - * - * Multirotor position controller - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "multirotor_pos_control_params.h" -#include "thrust_pid.h" - - -static bool thread_should_exit = false; /**< Deamon exit flag */ -static bool thread_running = false; /**< Deamon status flag */ -static int deamon_task; /**< Handle of deamon task / thread */ - -__EXPORT int multirotor_pos_control_main(int argc, char *argv[]); - -/** - * Mainloop of position controller. - */ -static int multirotor_pos_control_thread_main(int argc, char *argv[]); - -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -static float scale_control(float ctl, float end, float dz); - -static float norm(float x, float y); - -static void usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - - fprintf(stderr, "usage: multirotor_pos_control {start|stop|status}\n\n"); - exit(1); -} - -/** - * The deamon app only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to task_spawn(). - */ -int multirotor_pos_control_main(int argc, char *argv[]) -{ - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - warnx("already running"); - /* this is not an error */ - exit(0); - } - - warnx("start"); - thread_should_exit = false; - deamon_task = task_spawn_cmd("multirotor_pos_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 60, - 4096, - multirotor_pos_control_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - warnx("stop"); - thread_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - warnx("app is running"); - - } else { - warnx("app not started"); - } - - exit(0); - } - - usage("unrecognized command"); - exit(1); -} - -static float scale_control(float ctl, float end, float dz) -{ - if (ctl > dz) { - return (ctl - dz) / (end - dz); - - } else if (ctl < -dz) { - return (ctl + dz) / (end - dz); - - } else { - return 0.0f; - } -} - -static float norm(float x, float y) -{ - return sqrtf(x * x + y * y); -} - -static int multirotor_pos_control_thread_main(int argc, char *argv[]) -{ - /* welcome user */ - warnx("started"); - static int mavlink_fd; - mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - mavlink_log_info(mavlink_fd, "[mpc] started"); - - /* structures */ - struct vehicle_control_mode_s control_mode; - memset(&control_mode, 0, sizeof(control_mode)); - struct vehicle_attitude_s att; - memset(&att, 0, sizeof(att)); - struct vehicle_attitude_setpoint_s att_sp; - memset(&att_sp, 0, sizeof(att_sp)); - struct manual_control_setpoint_s manual; - memset(&manual, 0, sizeof(manual)); - struct vehicle_local_position_s local_pos; - memset(&local_pos, 0, sizeof(local_pos)); - struct mission_item_triplet_s triplet; - memset(&triplet, 0, sizeof(triplet)); - struct vehicle_global_velocity_setpoint_s global_vel_sp; - memset(&global_vel_sp, 0, sizeof(global_vel_sp)); - struct vehicle_local_position_setpoint_s local_pos_sp; - memset(&local_pos_sp, 0, sizeof(local_pos_sp)); - - /* subscribe to attitude, motor setpoints and system state */ - int param_sub = orb_subscribe(ORB_ID(parameter_update)); - int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - int mission_triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet)); - - /* publish setpoint */ - orb_advert_t local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &local_pos_sp); - orb_advert_t global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &global_vel_sp); - orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); - - bool reset_mission_sp = false; - bool global_pos_sp_valid = false; - bool reset_man_sp_z = true; - bool reset_man_sp_xy = true; - bool reset_int_z = true; - bool reset_int_z_manual = false; - bool reset_int_xy = true; - bool was_armed = false; - bool reset_auto_sp_xy = true; - bool reset_auto_sp_z = true; - bool reset_takeoff_sp = true; - - hrt_abstime t_prev = 0; - const float alt_ctl_dz = 0.2f; - const float pos_ctl_dz = 0.05f; - - float ref_alt = 0.0f; - hrt_abstime ref_alt_t = 0; - uint64_t local_ref_timestamp = 0; - - PID_t xy_pos_pids[2]; - PID_t xy_vel_pids[2]; - PID_t z_pos_pid; - thrust_pid_t z_vel_pid; - - thread_running = true; - - struct multirotor_position_control_params params; - struct multirotor_position_control_param_handles params_h; - parameters_init(¶ms_h); - parameters_update(¶ms_h, ¶ms); - - - for (int i = 0; i < 2; i++) { - pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f); - pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.tilt_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); - } - - pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max, PID_MODE_DERIVATIV_SET, 0.02f); - thrust_pid_init(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); - - while (!thread_should_exit) { - - bool param_updated; - orb_check(param_sub, ¶m_updated); - - if (param_updated) { - /* clear updated flag */ - struct parameter_update_s ps; - orb_copy(ORB_ID(parameter_update), param_sub, &ps); - /* update params */ - parameters_update(¶ms_h, ¶ms); - - for (int i = 0; i < 2; i++) { - pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f); - /* use integral_limit_out = tilt_max / 2 */ - float i_limit; - - if (params.xy_vel_i > 0.0f) { - i_limit = params.tilt_max / params.xy_vel_i / 2.0f; - - } else { - i_limit = 0.0f; // not used - } - - pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, i_limit, params.tilt_max); - } - - pid_set_parameters(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max); - thrust_pid_set_parameters(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min); - } - - bool updated; - - orb_check(control_mode_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); - } - - orb_check(mission_triplet_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(mission_item_triplet), mission_triplet_sub, &triplet); - global_pos_sp_valid = triplet.current_valid; - reset_mission_sp = true; - } - - hrt_abstime t = hrt_absolute_time(); - float dt; - - if (t_prev != 0) { - dt = (t - t_prev) * 0.000001f; - - } else { - dt = 0.0f; - } - - if (control_mode.flag_armed && !was_armed) { - /* reset setpoints and integrals on arming */ - reset_man_sp_z = true; - reset_man_sp_xy = true; - reset_auto_sp_z = true; - reset_auto_sp_xy = true; - reset_takeoff_sp = true; - reset_int_z = true; - reset_int_xy = true; - } - - was_armed = control_mode.flag_armed; - - t_prev = t; - - if (control_mode.flag_control_altitude_enabled || control_mode.flag_control_velocity_enabled || control_mode.flag_control_position_enabled) { - orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); - orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); - orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp); - - float z_sp_offs_max = params.z_vel_max / params.z_p * 2.0f; - float xy_sp_offs_max = params.xy_vel_max / params.xy_p * 2.0f; - float sp_move_rate[3] = { 0.0f, 0.0f, 0.0f }; - - if (control_mode.flag_control_manual_enabled) { - /* manual control */ - /* check for reference point updates and correct setpoint */ - if (local_pos.ref_timestamp != ref_alt_t) { - if (ref_alt_t != 0) { - /* home alt changed, don't follow large ground level changes in manual flight */ - local_pos_sp.z += local_pos.ref_alt - ref_alt; - } - - ref_alt_t = local_pos.ref_timestamp; - ref_alt = local_pos.ref_alt; - // TODO also correct XY setpoint - } - - /* reset setpoints to current position if needed */ - if (control_mode.flag_control_altitude_enabled) { - if (reset_man_sp_z) { - reset_man_sp_z = false; - local_pos_sp.z = local_pos.z; - mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double) - local_pos_sp.z); - } - - /* move altitude setpoint with throttle stick */ - float z_sp_ctl = scale_control(manual.throttle - 0.5f, 0.5f, alt_ctl_dz); - - if (z_sp_ctl != 0.0f) { - sp_move_rate[2] = -z_sp_ctl * params.z_vel_max; - local_pos_sp.z += sp_move_rate[2] * dt; - - if (local_pos_sp.z > local_pos.z + z_sp_offs_max) { - local_pos_sp.z = local_pos.z + z_sp_offs_max; - - } else if (local_pos_sp.z < local_pos.z - z_sp_offs_max) { - local_pos_sp.z = local_pos.z - z_sp_offs_max; - } - } - } - - if (control_mode.flag_control_position_enabled) { - if (reset_man_sp_xy) { - reset_man_sp_xy = false; - local_pos_sp.x = local_pos.x; - local_pos_sp.y = local_pos.y; - pid_reset_integral(&xy_vel_pids[0]); - pid_reset_integral(&xy_vel_pids[1]); - mavlink_log_info(mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y); - } - - /* move position setpoint with roll/pitch stick */ - float pos_pitch_sp_ctl = scale_control(-manual.pitch / params.rc_scale_pitch, 1.0f, pos_ctl_dz); - float pos_roll_sp_ctl = scale_control(manual.roll / params.rc_scale_roll, 1.0f, pos_ctl_dz); - - if (pos_pitch_sp_ctl != 0.0f || pos_roll_sp_ctl != 0.0f) { - /* calculate direction and increment of control in NED frame */ - float xy_sp_ctl_dir = att.yaw + atan2f(pos_roll_sp_ctl, pos_pitch_sp_ctl); - float xy_sp_ctl_speed = norm(pos_pitch_sp_ctl, pos_roll_sp_ctl) * params.xy_vel_max; - sp_move_rate[0] = cosf(xy_sp_ctl_dir) * xy_sp_ctl_speed; - sp_move_rate[1] = sinf(xy_sp_ctl_dir) * xy_sp_ctl_speed; - local_pos_sp.x += sp_move_rate[0] * dt; - local_pos_sp.y += sp_move_rate[1] * dt; - /* limit maximum setpoint from position offset and preserve direction - * fail safe, should not happen in normal operation */ - float pos_vec_x = local_pos_sp.x - local_pos.x; - float pos_vec_y = local_pos_sp.y - local_pos.y; - float pos_vec_norm = norm(pos_vec_x, pos_vec_y) / xy_sp_offs_max; - - if (pos_vec_norm > 1.0f) { - local_pos_sp.x = local_pos.x + pos_vec_x / pos_vec_norm; - local_pos_sp.y = local_pos.y + pos_vec_y / pos_vec_norm; - } - } - } - - /* copy yaw setpoint to vehicle_local_position_setpoint topic */ - local_pos_sp.yaw = att_sp.yaw_body; - - /* local position setpoint is valid and can be used for auto loiter after position controlled mode */ - reset_auto_sp_xy = !control_mode.flag_control_position_enabled; - reset_auto_sp_z = !control_mode.flag_control_altitude_enabled; - reset_takeoff_sp = true; - - /* force reprojection of global setpoint after manual mode */ - reset_mission_sp = true; - } - /* AUTO not implemented */ - - /* publish local position setpoint */ - orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); - - /* run position & altitude controllers, calculate velocity setpoint */ - if (control_mode.flag_control_altitude_enabled) { - global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz - sp_move_rate[2], dt) + sp_move_rate[2]; - - } else { - reset_man_sp_z = true; - global_vel_sp.vz = 0.0f; - } - - if (control_mode.flag_control_position_enabled) { - /* calculate velocity set point in NED frame */ - global_vel_sp.vx = pid_calculate(&xy_pos_pids[0], local_pos_sp.x, local_pos.x, local_pos.vx - sp_move_rate[0], dt) + sp_move_rate[0]; - global_vel_sp.vy = pid_calculate(&xy_pos_pids[1], local_pos_sp.y, local_pos.y, local_pos.vy - sp_move_rate[1], dt) + sp_move_rate[1]; - - /* limit horizontal speed */ - float xy_vel_sp_norm = norm(global_vel_sp.vx, global_vel_sp.vy) / params.xy_vel_max; - - if (xy_vel_sp_norm > 1.0f) { - global_vel_sp.vx /= xy_vel_sp_norm; - global_vel_sp.vy /= xy_vel_sp_norm; - } - - } else { - reset_man_sp_xy = true; - global_vel_sp.vx = 0.0f; - global_vel_sp.vy = 0.0f; - } - - /* publish new velocity setpoint */ - orb_publish(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, &global_vel_sp); - // TODO subscribe to velocity setpoint if altitude/position control disabled - - if (control_mode.flag_control_climb_rate_enabled || control_mode.flag_control_velocity_enabled) { - /* run velocity controllers, calculate thrust vector with attitude-thrust compensation */ - float thrust_sp[3] = { 0.0f, 0.0f, 0.0f }; - - if (control_mode.flag_control_climb_rate_enabled) { - if (reset_int_z) { - reset_int_z = false; - float i = params.thr_min; - - if (reset_int_z_manual) { - i = manual.throttle; - - if (i < params.thr_min) { - i = params.thr_min; - - } else if (i > params.thr_max) { - i = params.thr_max; - } - } - - thrust_pid_set_integral(&z_vel_pid, -i); - mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i); - } - - thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt, att.R[2][2]); - att_sp.thrust = -thrust_sp[2]; - - } else { - /* reset thrust integral when altitude control enabled */ - reset_int_z = true; - } - - if (control_mode.flag_control_velocity_enabled) { - /* calculate thrust set point in NED frame */ - if (reset_int_xy) { - reset_int_xy = false; - pid_reset_integral(&xy_vel_pids[0]); - pid_reset_integral(&xy_vel_pids[1]); - mavlink_log_info(mavlink_fd, "[mpc] reset pos integral"); - } - - thrust_sp[0] = pid_calculate(&xy_vel_pids[0], global_vel_sp.vx, local_pos.vx, 0.0f, dt); - thrust_sp[1] = pid_calculate(&xy_vel_pids[1], global_vel_sp.vy, local_pos.vy, 0.0f, dt); - - /* thrust_vector now contains desired acceleration (but not in m/s^2) in NED frame */ - /* limit horizontal part of thrust */ - float thrust_xy_dir = atan2f(thrust_sp[1], thrust_sp[0]); - /* assuming that vertical component of thrust is g, - * horizontal component = g * tan(alpha) */ - float tilt = atanf(norm(thrust_sp[0], thrust_sp[1])); - - if (tilt > params.tilt_max) { - tilt = params.tilt_max; - } - - /* convert direction to body frame */ - thrust_xy_dir -= att.yaw; - /* calculate roll and pitch */ - att_sp.roll_body = sinf(thrust_xy_dir) * tilt; - att_sp.pitch_body = -cosf(thrust_xy_dir) * tilt / cosf(att_sp.roll_body); - - } else { - reset_int_xy = true; - } - - att_sp.timestamp = hrt_absolute_time(); - - /* publish new attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - } - - } else { - /* position controller disabled, reset setpoints */ - reset_man_sp_z = true; - reset_man_sp_xy = true; - reset_int_z = true; - reset_int_xy = true; - reset_mission_sp = true; - reset_auto_sp_xy = true; - reset_auto_sp_z = true; - } - - /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ - reset_int_z_manual = control_mode.flag_armed && control_mode.flag_control_manual_enabled && !control_mode.flag_control_climb_rate_enabled; - - /* run at approximately 50 Hz */ - usleep(20000); - } - - warnx("stopped"); - mavlink_log_info(mavlink_fd, "[mpc] stopped"); - - thread_running = false; - - fflush(stdout); - return 0; -} - diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c deleted file mode 100644 index b7041e4d57..0000000000 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c +++ /dev/null @@ -1,112 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* - * @file multirotor_pos_control_params.c - * - * Parameters for multirotor_pos_control - */ - -#include "multirotor_pos_control_params.h" - -/* controller parameters */ -PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.2f); -PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.8f); -PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f); -PARAM_DEFINE_FLOAT(MPC_Z_D, 0.0f); -PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.1f); -PARAM_DEFINE_FLOAT(MPC_Z_VEL_I, 0.0f); -PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f); -PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 3.0f); -PARAM_DEFINE_FLOAT(MPC_XY_P, 0.5f); -PARAM_DEFINE_FLOAT(MPC_XY_D, 0.0f); -PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.2f); -PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.0f); -PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.0f); -PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f); -PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 0.5f); - -int parameters_init(struct multirotor_position_control_param_handles *h) -{ - h->takeoff_alt = param_find("NAV_TAKEOFF_ALT"); - h->takeoff_gap = param_find("NAV_TAKEOFF_GAP"); - h->thr_min = param_find("MPC_THR_MIN"); - h->thr_max = param_find("MPC_THR_MAX"); - h->z_p = param_find("MPC_Z_P"); - h->z_d = param_find("MPC_Z_D"); - h->z_vel_p = param_find("MPC_Z_VEL_P"); - h->z_vel_i = param_find("MPC_Z_VEL_I"); - h->z_vel_d = param_find("MPC_Z_VEL_D"); - h->z_vel_max = param_find("MPC_Z_VEL_MAX"); - h->xy_p = param_find("MPC_XY_P"); - h->xy_d = param_find("MPC_XY_D"); - h->xy_vel_p = param_find("MPC_XY_VEL_P"); - h->xy_vel_i = param_find("MPC_XY_VEL_I"); - h->xy_vel_d = param_find("MPC_XY_VEL_D"); - h->xy_vel_max = param_find("MPC_XY_VEL_MAX"); - h->tilt_max = param_find("MPC_TILT_MAX"); - - h->rc_scale_pitch = param_find("RC_SCALE_PITCH"); - h->rc_scale_roll = param_find("RC_SCALE_ROLL"); - h->rc_scale_yaw = param_find("RC_SCALE_YAW"); - - return OK; -} - -int parameters_update(const struct multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p) -{ - param_get(h->takeoff_alt, &(p->takeoff_alt)); - param_get(h->takeoff_gap, &(p->takeoff_gap)); - param_get(h->thr_min, &(p->thr_min)); - param_get(h->thr_max, &(p->thr_max)); - param_get(h->z_p, &(p->z_p)); - param_get(h->z_d, &(p->z_d)); - param_get(h->z_vel_p, &(p->z_vel_p)); - param_get(h->z_vel_i, &(p->z_vel_i)); - param_get(h->z_vel_d, &(p->z_vel_d)); - param_get(h->z_vel_max, &(p->z_vel_max)); - param_get(h->xy_p, &(p->xy_p)); - param_get(h->xy_d, &(p->xy_d)); - param_get(h->xy_vel_p, &(p->xy_vel_p)); - param_get(h->xy_vel_i, &(p->xy_vel_i)); - param_get(h->xy_vel_d, &(p->xy_vel_d)); - param_get(h->xy_vel_max, &(p->xy_vel_max)); - param_get(h->tilt_max, &(p->tilt_max)); - - param_get(h->rc_scale_pitch, &(p->rc_scale_pitch)); - param_get(h->rc_scale_roll, &(p->rc_scale_roll)); - param_get(h->rc_scale_yaw, &(p->rc_scale_yaw)); - - return OK; -} diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h deleted file mode 100644 index fc658dadb8..0000000000 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h +++ /dev/null @@ -1,101 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/* - * @file multirotor_pos_control_params.h - * - * Parameters for multirotor_pos_control - */ - -#include - -struct multirotor_position_control_params { - float takeoff_alt; - float takeoff_gap; - float thr_min; - float thr_max; - float z_p; - float z_d; - float z_vel_p; - float z_vel_i; - float z_vel_d; - float z_vel_max; - float xy_p; - float xy_d; - float xy_vel_p; - float xy_vel_i; - float xy_vel_d; - float xy_vel_max; - float tilt_max; - - float rc_scale_pitch; - float rc_scale_roll; - float rc_scale_yaw; -}; - -struct multirotor_position_control_param_handles { - param_t takeoff_alt; - param_t takeoff_gap; - param_t thr_min; - param_t thr_max; - param_t z_p; - param_t z_d; - param_t z_vel_p; - param_t z_vel_i; - param_t z_vel_d; - param_t z_vel_max; - param_t xy_p; - param_t xy_d; - param_t xy_vel_p; - param_t xy_vel_i; - param_t xy_vel_d; - param_t xy_vel_max; - param_t tilt_max; - - param_t rc_scale_pitch; - param_t rc_scale_roll; - param_t rc_scale_yaw; -}; - -/** - * Initialize all parameter handles and values - * - */ -int parameters_init(struct multirotor_position_control_param_handles *h); - -/** - * Update all parameters - * - */ -int parameters_update(const struct multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p); diff --git a/src/modules/multirotor_pos_control/thrust_pid.c b/src/modules/multirotor_pos_control/thrust_pid.c deleted file mode 100644 index b985630aec..0000000000 --- a/src/modules/multirotor_pos_control/thrust_pid.c +++ /dev/null @@ -1,189 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file thrust_pid.c - * - * Implementation of thrust control PID. - * - * @author Anton Babushkin - */ - -#include "thrust_pid.h" -#include - -__EXPORT void thrust_pid_init(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max, uint8_t mode, float dt_min) -{ - pid->kp = kp; - pid->ki = ki; - pid->kd = kd; - pid->limit_min = limit_min; - pid->limit_max = limit_max; - pid->mode = mode; - pid->dt_min = dt_min; - pid->last_output = 0.0f; - pid->sp = 0.0f; - pid->error_previous = 0.0f; - pid->integral = 0.0f; -} - -__EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max) -{ - int ret = 0; - - if (isfinite(kp)) { - pid->kp = kp; - - } else { - ret = 1; - } - - if (isfinite(ki)) { - pid->ki = ki; - - } else { - ret = 1; - } - - if (isfinite(kd)) { - pid->kd = kd; - - } else { - ret = 1; - } - - if (isfinite(limit_min)) { - pid->limit_min = limit_min; - - } else { - ret = 1; - } - - if (isfinite(limit_max)) { - pid->limit_max = limit_max; - - } else { - ret = 1; - } - - return ret; -} - -__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt, float r22) -{ - /* Alternative integral component calculation - * - * start: - * error = setpoint - current_value - * integral = integral + (Ki * error * dt) - * derivative = (error - previous_error) / dt - * previous_error = error - * output = (Kp * error) + integral + (Kd * derivative) - * wait(dt) - * goto start - */ - - if (!isfinite(sp) || !isfinite(val) || !isfinite(dt)) { - return pid->last_output; - } - - float i, d; - pid->sp = sp; - - // Calculated current error value - float error = pid->sp - val; - - // Calculate or measured current error derivative - if (pid->mode == THRUST_PID_MODE_DERIVATIV_CALC) { - d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min); - pid->error_previous = error; - - } else if (pid->mode == THRUST_PID_MODE_DERIVATIV_CALC_NO_SP) { - d = (-val - pid->error_previous) / fmaxf(dt, pid->dt_min); - pid->error_previous = -val; - - } else { - d = 0.0f; - } - - if (!isfinite(d)) { - d = 0.0f; - } - - /* calculate the error integral */ - i = pid->integral + (pid->ki * error * dt); - - /* attitude-thrust compensation - * r22 is (2, 2) componet of rotation matrix for current attitude */ - float att_comp; - - if (r22 > 0.8f) - att_comp = 1.0f / r22; - else if (r22 > 0.0f) - att_comp = ((1.0f / 0.8f - 1.0f) / 0.8f) * r22 + 1.0f; - else - att_comp = 1.0f; - - /* calculate output */ - float output = ((error * pid->kp) + i + (d * pid->kd)) * att_comp; - - /* check for saturation */ - if (output < pid->limit_min || output > pid->limit_max) { - /* saturated, recalculate output with old integral */ - output = (error * pid->kp) + pid->integral + (d * pid->kd); - - } else { - if (isfinite(i)) { - pid->integral = i; - } - } - - if (isfinite(output)) { - if (output > pid->limit_max) { - output = pid->limit_max; - - } else if (output < pid->limit_min) { - output = pid->limit_min; - } - - pid->last_output = output; - } - - return pid->last_output; -} - -__EXPORT void thrust_pid_set_integral(thrust_pid_t *pid, float i) -{ - pid->integral = i; -} diff --git a/src/modules/multirotor_pos_control/thrust_pid.h b/src/modules/multirotor_pos_control/thrust_pid.h deleted file mode 100644 index 5e169c1ba5..0000000000 --- a/src/modules/multirotor_pos_control/thrust_pid.h +++ /dev/null @@ -1,76 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file thrust_pid.h - * - * Definition of thrust control PID interface. - * - * @author Anton Babushkin - */ - -#ifndef THRUST_PID_H_ -#define THRUST_PID_H_ - -#include - -__BEGIN_DECLS - -/* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error */ -#define THRUST_PID_MODE_DERIVATIV_CALC 0 -/* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value, setpoint derivative is ignored */ -#define THRUST_PID_MODE_DERIVATIV_CALC_NO_SP 1 - -typedef struct { - float kp; - float ki; - float kd; - float sp; - float integral; - float error_previous; - float last_output; - float limit_min; - float limit_max; - float dt_min; - uint8_t mode; -} thrust_pid_t; - -__EXPORT void thrust_pid_init(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max, uint8_t mode, float dt_min); -__EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max); -__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt, float r22); -__EXPORT void thrust_pid_set_integral(thrust_pid_t *pid, float i); - -__END_DECLS - -#endif /* THRUST_PID_H_ */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 63952be6ee..9c5e62be7a 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -62,7 +62,7 @@ #include #include #include -#include +#include #include #include #include @@ -150,7 +150,7 @@ private: int _onboard_mission_sub; /**< notification of onboard mission updates */ int _capabilities_sub; /**< notification of vehicle capabilities updates */ - orb_advert_t _triplet_pub; /**< publish position setpoint triplet */ + orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */ orb_advert_t _mission_result_pub; /**< publish mission result topic */ orb_advert_t _control_mode_pub; /**< publish vehicle control mode topic */ @@ -158,16 +158,18 @@ private: struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ struct vehicle_global_position_s _global_pos; /**< global vehicle position */ struct home_position_s _home_pos; /**< home position for RTL */ - struct mission_item_triplet_s _mission_item_triplet; /**< triplet of mission items */ + struct position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */ struct mission_result_s _mission_result; /**< mission result for commander/mavlink */ + struct mission_item_s _mission_item; /**< current mission item */ + bool _mission_item_valid; /**< current mission item valid */ perf_counter_t _loop_perf; /**< loop performance counter */ - + Geofence _geofence; bool _geofence_violation_warning_sent; bool _fence_valid; /**< flag if fence is valid */ - bool _inside_fence; /**< vehicle is inside fence */ + bool _inside_fence; /**< vehicle is inside fence */ struct navigation_capabilities_s _nav_caps; @@ -184,6 +186,8 @@ private: uint64_t _set_nav_state_timestamp; /**< timestamp of last handled navigation state request */ + char *nav_states_str[NAV_STATE_MAX]; + struct { float min_altitude; float acceptance_radius; @@ -192,6 +196,7 @@ private: float takeoff_alt; float land_alt; float rtl_alt; + float rtl_land_delay; } _parameters; /**< local copies of parameters */ struct { @@ -202,6 +207,7 @@ private: param_t takeoff_alt; param_t land_alt; param_t rtl_alt; + param_t rtl_land_delay; } _parameter_handles; /**< handles for parameters */ enum Event { @@ -210,6 +216,7 @@ private: EVENT_LOITER_REQUESTED, EVENT_MISSION_REQUESTED, EVENT_RTL_REQUESTED, + EVENT_LAND_REQUESTED, EVENT_MISSION_CHANGED, EVENT_HOME_POSITION_CHANGED, MAX_EVENT @@ -286,7 +293,7 @@ private: void start_loiter(); void start_mission(); void start_rtl(); - void finish_rtl(); + void start_land(); /** * Guards offboard mission @@ -311,7 +318,7 @@ private: /** * Move to next waypoint */ - void advance_mission(); + void set_mission_item(); /** * Switch to next RTL state @@ -319,35 +326,24 @@ private: void set_rtl_item(); /** - * Helper function to get a loiter item + * Set position setpoint for mission item */ - void get_loiter_item(mission_item_s *item); + void position_setpoint_from_mission_item(position_setpoint_s *sp, mission_item_s *item); /** * Helper function to get a takeoff item */ - void get_takeoff_item(mission_item_s *item); + void get_takeoff_setpoint(position_setpoint_s *pos_sp); /** * Publish a new mission item triplet for position controller */ - void publish_mission_item_triplet(); + void publish_position_setpoint_triplet(); /** * Publish vehicle_control_mode topic for controllers */ void publish_control_mode(); - - - /** - * Compare two mission items if they are equivalent - * Two mission items can be considered equivalent for the purpose of the navigator even if some fields differ. - * - * @return true if equivalent, false otherwise - */ - bool cmp_mission_item_equivalent(const struct mission_item_s a, const struct mission_item_s b); - - void add_home_pos_to_rtl(struct mission_item_s *new_mission_item); }; namespace navigator @@ -362,7 +358,7 @@ namespace navigator Navigator *g_navigator; } -Navigator::Navigator() : +Navigator::Navigator() : /* state machine transition table */ StateTable(&myTable[0][0], NAV_STATE_MAX, MAX_EVENT), @@ -381,7 +377,7 @@ Navigator::Navigator() : _capabilities_sub(-1), /* publications */ - _triplet_pub(-1), + _pos_sp_triplet_pub(-1), _mission_result_pub(-1), _control_mode_pub(-1), @@ -398,6 +394,7 @@ Navigator::Navigator() : _waypoint_yaw_reached(false), _time_first_inside_orbit(0), _set_nav_state_timestamp(0), + _mission_item_valid(false), _need_takeoff(true), _do_takeoff(false), _geofence_violation_warning_sent(false) @@ -409,15 +406,19 @@ Navigator::Navigator() : _parameter_handles.takeoff_alt = param_find("NAV_TAKEOFF_ALT"); _parameter_handles.land_alt = param_find("NAV_LAND_ALT"); _parameter_handles.rtl_alt = param_find("NAV_RTL_ALT"); + _parameter_handles.rtl_land_delay = param_find("NAV_RTL_LAND_T"); - _mission_item_triplet.previous_valid = false; - _mission_item_triplet.current_valid = false; - _mission_item_triplet.next_valid = false; - memset(&_mission_item_triplet.previous, 0, sizeof(struct mission_item_s)); - memset(&_mission_item_triplet.current, 0, sizeof(struct mission_item_s)); - memset(&_mission_item_triplet.next, 0, sizeof(struct mission_item_s)); - + memset(&_pos_sp_triplet, 0, sizeof(struct position_setpoint_triplet_s)); memset(&_mission_result, 0, sizeof(struct mission_result_s)); + memset(&_mission_item, 0, sizeof(struct mission_item_s)); + + memset(&nav_states_str, 0, sizeof(nav_states_str)); + nav_states_str[0] = "NONE"; + nav_states_str[1] = "READY"; + nav_states_str[2] = "LOITER"; + nav_states_str[3] = "MISSION"; + nav_states_str[4] = "RTL"; + nav_states_str[5] = "LAND"; /* Initialize state machine */ myState = NAV_STATE_NONE; @@ -463,6 +464,7 @@ Navigator::parameters_update() param_get(_parameter_handles.takeoff_alt, &(_parameters.takeoff_alt)); param_get(_parameter_handles.land_alt, &(_parameters.land_alt)); param_get(_parameter_handles.rtl_alt, &(_parameters.rtl_alt)); + param_get(_parameter_handles.rtl_land_delay, &(_parameters.rtl_land_delay)); _mission.set_onboard_mission_allowed((bool)_parameter_handles.onboard_mission_enabled); @@ -472,7 +474,6 @@ Navigator::parameters_update() void Navigator::global_position_update() { - /* load local copies */ orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); } @@ -493,16 +494,20 @@ void Navigator::offboard_mission_update(bool isrotaryWing) { struct mission_s offboard_mission; + if (orb_copy(ORB_ID(mission), _offboard_mission_sub, &offboard_mission) == OK) { /* Check mission feasibility, for now do not handle the return value, * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */ dm_item_t dm_current; + if (offboard_mission.dataman_id == 0) { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + } else { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; } + missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count, _geofence); _mission.set_offboard_dataman_id(offboard_mission.dataman_id); @@ -519,6 +524,7 @@ void Navigator::onboard_mission_update() { struct mission_s onboard_mission; + if (orb_copy(ORB_ID(mission), _onboard_mission_sub, &onboard_mission) == OK) { _mission.set_current_onboard_mission_index(onboard_mission.current_index); @@ -561,11 +567,13 @@ Navigator::task_main() * else clear geofence data in datamanager */ struct stat buffer; - if( stat (GEOFENCE_FILENAME, &buffer) == 0 ) { + + if (stat(GEOFENCE_FILENAME, &buffer) == 0) { warnx("Try to load geofence.txt"); _geofence.loadFromFile(GEOFENCE_FILENAME); + } else { - if (_geofence.clearDm() > 0 ) + if (_geofence.clearDm() > 0) warnx("Geofence cleared"); else warnx("Could not clear geofence"); @@ -581,7 +589,7 @@ Navigator::task_main() _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _home_pos_sub = orb_subscribe(ORB_ID(home_position)); - + /* copy all topics first time */ vehicle_status_update(); parameters_update(); @@ -647,87 +655,122 @@ Navigator::task_main() vehicle_status_update(); pub_control_mode = true; - /* Evaluate state machine from commander and set the navigator mode accordingly */ - if (_vstatus.main_state == MAIN_STATE_AUTO && - (_vstatus.arming_state == ARMING_STATE_ARMED || _vstatus.arming_state == ARMING_STATE_ARMED_ERROR)) { - bool stick_mode = false; - if (!_vstatus.rc_signal_lost) { - /* RC signal available, use control switches to set mode */ - /* RETURN switch, overrides MISSION switch */ - if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { - if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { - dispatch(EVENT_RTL_REQUESTED); - } - stick_mode = true; - } else { - /* MISSION switch */ - if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) { - dispatch(EVENT_LOITER_REQUESTED); - stick_mode = true; - } else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) { - /* switch to mission only if available */ - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); + /* evaluate state machine from commander and set the navigator mode accordingly */ + if (_vstatus.arming_state == ARMING_STATE_ARMED || _vstatus.arming_state == ARMING_STATE_ARMED_ERROR) { + if (_vstatus.failsafe_state == FAILSAFE_STATE_NORMAL) { + if (_vstatus.main_state == MAIN_STATE_AUTO) { + bool stick_mode = false; + + if (!_vstatus.rc_signal_lost) { + /* RC signal available, use control switches to set mode */ + /* RETURN switch, overrides MISSION switch */ + if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { + if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + dispatch(EVENT_RTL_REQUESTED); + } + + stick_mode = true; + } else { - dispatch(EVENT_LOITER_REQUESTED); + /* MISSION switch */ + if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) { + dispatch(EVENT_LOITER_REQUESTED); + stick_mode = true; + + } else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) { + /* switch to mission only if available */ + if (_mission.current_mission_available()) { + dispatch(EVENT_MISSION_REQUESTED); + + } else { + dispatch(EVENT_LOITER_REQUESTED); + } + + stick_mode = true; + } + + if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) { + /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ + dispatch(EVENT_LOITER_REQUESTED); + stick_mode = true; + } } - stick_mode = true; } - if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) { - /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ - dispatch(EVENT_LOITER_REQUESTED); - stick_mode = true; - } - } - } - if (!stick_mode) { - if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) { - /* commander requested new navigation mode, try to set it */ - _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; + if (!stick_mode) { + if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) { + /* commander requested new navigation mode, try to set it */ + _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; - switch (_vstatus.set_nav_state) { - case NAV_STATE_NONE: - /* nothing to do */ - break; + switch (_vstatus.set_nav_state) { + case NAV_STATE_NONE: + /* nothing to do */ + break; - case NAV_STATE_LOITER: - dispatch(EVENT_LOITER_REQUESTED); - break; + case NAV_STATE_LOITER: + dispatch(EVENT_LOITER_REQUESTED); + break; + + case NAV_STATE_MISSION: + if (_mission.current_mission_available()) { + dispatch(EVENT_MISSION_REQUESTED); + + } else { + dispatch(EVENT_LOITER_REQUESTED); + } + + break; + + case NAV_STATE_RTL: + if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + dispatch(EVENT_RTL_REQUESTED); + } + + break; + + default: + warnx("ERROR: Requested navigation state not supported"); + break; + } - case NAV_STATE_MISSION: - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); } else { - dispatch(EVENT_LOITER_REQUESTED); - } - break; + /* on first switch to AUTO try mission by default, if none is available fallback to loiter */ + if (myState == NAV_STATE_NONE) { + if (_mission.current_mission_available()) { + dispatch(EVENT_MISSION_REQUESTED); - case NAV_STATE_RTL: - if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { - dispatch(EVENT_RTL_REQUESTED); + } else { + dispatch(EVENT_LOITER_REQUESTED); + } + } } - break; - - default: - warnx("ERROR: Requested navigation state not supported"); - break; } } else { - /* on first switch to AUTO try mission by default, if none is available fallback to loiter */ - if (myState == NAV_STATE_NONE) { - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); - } else { - dispatch(EVENT_LOITER_REQUESTED); - } - } + /* not in AUTO mode */ + dispatch(EVENT_NONE_REQUESTED); } + + } else if (_vstatus.failsafe_state == FAILSAFE_STATE_RTL) { + /* RTL on failsafe */ + if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + + dispatch(EVENT_RTL_REQUESTED); + } + + } else if (_vstatus.failsafe_state == FAILSAFE_STATE_LAND) { + /* LAND on failsafe */ + if (myState != NAV_STATE_READY) { + dispatch(EVENT_LAND_REQUESTED); + } + + } else { + /* shouldn't act */ + dispatch(EVENT_NONE_REQUESTED); } } else { - /* not in AUTO */ + /* not armed */ dispatch(EVENT_NONE_REQUESTED); } } @@ -746,7 +789,7 @@ Navigator::task_main() /* offboard mission updated */ if (fds[4].revents & POLLIN) { offboard_mission_update(_vstatus.is_rotary_wing); - // XXX check if mission really changed + // XXX check if mission really changed dispatch(EVENT_MISSION_CHANGED); } @@ -767,6 +810,7 @@ Navigator::task_main() /* global position updated */ if (fds[1].revents & POLLIN) { global_position_update(); + /* only check if waypoint has been reached in MISSION and RTL modes */ if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL) { if (check_mission_item_reached()) { @@ -775,15 +819,15 @@ Navigator::task_main() } /* Check geofence violation */ - if(!_geofence.inside(&_global_pos)) { + if (!_geofence.inside(&_global_pos)) { //xxx: publish geofence violation here (or change local flag depending on which app handles the flight termination) /* Issue a warning about the geofence violation once */ - if (!_geofence_violation_warning_sent) - { + if (!_geofence_violation_warning_sent) { mavlink_log_critical(_mavlink_fd, "#audio: Geofence violation"); _geofence_violation_warning_sent = true; } + } else { /* Reset the _geofence_violation_warning_sent field */ _geofence_violation_warning_sent = false; @@ -792,7 +836,7 @@ Navigator::task_main() /* notify user about state changes */ if (myState != prevState) { - mavlink_log_info(_mavlink_fd, "[navigator] nav state %d -> %d", prevState, myState); + mavlink_log_info(_mavlink_fd, "[navigator] nav state: %s", nav_states_str[myState]); prevState = myState; pub_control_mode = true; } @@ -833,128 +877,153 @@ Navigator::start() } void -Navigator::status() +Navigator::status() { warnx("Global position is %svalid", _global_pos.valid ? "" : "in"); + if (_global_pos.valid) { - warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon / 1e7d, _global_pos.lat / 1e7d); + warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon, _global_pos.lat); warnx("Altitude %5.5f meters, altitude above home %5.5f meters", - (double)_global_pos.alt, (double)_global_pos.relative_alt); - warnx("Ground velocity in m/s, x %5.5f, y %5.5f, z %5.5f", - (double)_global_pos.vx, (double)_global_pos.vy, (double)_global_pos.vz); + (double)_global_pos.alt, (double)(_global_pos.alt - _home_pos.alt)); + warnx("Ground velocity in m/s, N %5.5f, E %5.5f, D %5.5f", + (double)_global_pos.vel_n, (double)_global_pos.vel_e, (double)_global_pos.vel_d); warnx("Compass heading in degrees %5.5f", (double)(_global_pos.yaw * M_RAD_TO_DEG_F)); } + if (_fence_valid) { warnx("Geofence is valid"); // warnx("Vertex longitude latitude"); // for (unsigned i = 0; i < _fence.count; i++) // warnx("%6u %9.5f %8.5f", i, (double)_fence.vertices[i].lon, (double)_fence.vertices[i].lat); + } else { warnx("Geofence not set"); } switch (myState) { - case NAV_STATE_NONE: - warnx("State: None"); - break; - case NAV_STATE_LOITER: - warnx("State: Loiter"); - break; - case NAV_STATE_MISSION: - warnx("State: Mission"); - break; - case NAV_STATE_RTL: - warnx("State: RTL"); - break; - default: - warnx("State: Unknown"); - break; + case NAV_STATE_NONE: + warnx("State: None"); + break; + + case NAV_STATE_LOITER: + warnx("State: Loiter"); + break; + + case NAV_STATE_MISSION: + warnx("State: Mission"); + break; + + case NAV_STATE_RTL: + warnx("State: RTL"); + break; + + default: + warnx("State: Unknown"); + break; } } StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { - { - /* STATE_NONE */ + { + /* NAV_STATE_NONE */ /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE}, /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, }, - { - /* STATE_READY */ - /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE}, + { + /* NAV_STATE_READY */ + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, /* EVENT_READY_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_READY}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_READY}, }, { - /* STATE_LOITER */ + /* NAV_STATE_LOITER */ /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, /* EVENT_READY_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER}, /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, }, - { - /* STATE_MISSION */ + { + /* NAV_STATE_MISSION */ /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {NO_ACTION, NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_MISSION}, }, - { - /* STATE_RTL */ + { + /* NAV_STATE_RTL */ /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL}, - /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, // TODO need to reset rtl_state + }, + { + /* NAV_STATE_LAND */ + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, + /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, + /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {NO_ACTION, NAV_STATE_LAND}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LAND}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LAND}, }, }; void Navigator::start_none() { - _mission_item_triplet.previous_valid = false; - _mission_item_triplet.current_valid = false; - _mission_item_triplet.next_valid = false; + _pos_sp_triplet.previous.valid = false; + _pos_sp_triplet.current.valid = false; + _pos_sp_triplet.next.valid = false; + _mission_item_valid = false; _reset_loiter_pos = true; _do_takeoff = false; _rtl_state = RTL_STATE_NONE; - publish_mission_item_triplet(); + publish_position_setpoint_triplet(); } void Navigator::start_ready() { - _mission_item_triplet.previous_valid = false; - _mission_item_triplet.current_valid = false; - _mission_item_triplet.next_valid = false; + _pos_sp_triplet.previous.valid = false; + _pos_sp_triplet.current.valid = false; + _pos_sp_triplet.next.valid = false; + _mission_item_valid = false; _reset_loiter_pos = true; _do_takeoff = false; - // TODO check if (_rtl_state != RTL_STATE_LAND) { + /* allow RTL if landed not at home */ _rtl_state = RTL_STATE_NONE; } - publish_mission_item_triplet(); + publish_position_setpoint_triplet(); } void @@ -963,33 +1032,41 @@ Navigator::start_loiter() _do_takeoff = false; /* set loiter position if needed */ - if (_reset_loiter_pos || !_mission_item_triplet.current_valid) { + if (_reset_loiter_pos || !_pos_sp_triplet.current.valid) { _reset_loiter_pos = false; - _mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d; - _mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d; - _mission_item_triplet.current.yaw = NAN; // NAN means to use current yaw + _pos_sp_triplet.current.lat = _global_pos.lat; + _pos_sp_triplet.current.lon = _global_pos.lon; + _pos_sp_triplet.current.yaw = NAN; // NAN means to use current yaw - _mission_item_triplet.current.altitude_is_relative = false; - float min_alt_amsl = _parameters.min_altitude + _home_pos.altitude; + float min_alt_amsl = _parameters.min_altitude + _home_pos.alt; /* use current altitude if above min altitude set by parameter */ if (_global_pos.alt < min_alt_amsl) { - _mission_item_triplet.current.altitude = min_alt_amsl; + _pos_sp_triplet.current.alt = min_alt_amsl; mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt)); + } else { - _mission_item_triplet.current.altitude = _global_pos.alt; + _pos_sp_triplet.current.alt = _global_pos.alt; mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude"); } + + _pos_sp_triplet.current.type = SETPOINT_TYPE_NORMAL; + + if (_rtl_state == RTL_STATE_LAND) { + /* if RTL landing was interrupted, avoid landing from MIN_ALT on next RTL */ + _rtl_state = RTL_STATE_DESCEND; + } } - _mission_item_triplet.previous_valid = false; - _mission_item_triplet.current_valid = true; - _mission_item_triplet.next_valid = false; + _pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius; + _pos_sp_triplet.current.loiter_direction = 1; + _pos_sp_triplet.previous.valid = false; + _pos_sp_triplet.current.valid = true; + _pos_sp_triplet.next.valid = false; + _mission_item_valid = false; - get_loiter_item(&_mission_item_triplet.current); - - publish_mission_item_triplet(); + publish_position_setpoint_triplet(); } void @@ -997,16 +1074,14 @@ Navigator::start_mission() { _need_takeoff = true; - mavlink_log_info(_mavlink_fd, "[navigator] mission started"); - advance_mission(); + set_mission_item(); } void -Navigator::advance_mission() +Navigator::set_mission_item() { /* copy current mission to previous item */ - memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); - _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; + memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); _reset_loiter_pos = true; _do_takeoff = false; @@ -1015,36 +1090,34 @@ Navigator::advance_mission() bool onboard; unsigned index; - ret = _mission.get_current_mission_item(&_mission_item_triplet.current, &onboard, &index); + ret = _mission.get_current_mission_item(&_mission_item, &onboard, &index); if (ret == OK) { - _mission_item_triplet.current_valid = true; - add_home_pos_to_rtl(&_mission_item_triplet.current); + _mission_item_valid = true; + position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); - if (_mission_item_triplet.current.nav_cmd != NAV_CMD_RETURN_TO_LAUNCH && - _mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT && - _mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_TURN_COUNT && - _mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_UNLIMITED) { + if (_mission_item.nav_cmd != NAV_CMD_RETURN_TO_LAUNCH && + _mission_item.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT && + _mission_item.nav_cmd != NAV_CMD_LOITER_TURN_COUNT && + _mission_item.nav_cmd != NAV_CMD_LOITER_UNLIMITED) { /* don't reset RTL state on RTL or LOITER items */ _rtl_state = RTL_STATE_NONE; } if (_vstatus.is_rotary_wing) { if (_need_takeoff && ( - _mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF || - _mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT || - _mission_item_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH || - _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED - )) { + _mission_item.nav_cmd == NAV_CMD_TAKEOFF || + _mission_item.nav_cmd == NAV_CMD_WAYPOINT || + _mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH || + _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + _mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED + )) { /* do special TAKEOFF handling for VTOL */ _need_takeoff = false; /* calculate desired takeoff altitude AMSL */ - float takeoff_alt_amsl = _mission_item_triplet.current.altitude; - if (_mission_item_triplet.current.altitude_is_relative) - takeoff_alt_amsl += _home_pos.altitude; + float takeoff_alt_amsl = _pos_sp_triplet.current.alt; if (_vstatus.condition_landed) { /* takeoff to at least NAV_TAKEOFF_ALT from ground if landed */ @@ -1052,210 +1125,305 @@ Navigator::advance_mission() } /* check if we really need takeoff */ - if (_vstatus.condition_landed || _global_pos.alt < takeoff_alt_amsl - _mission_item_triplet.current.acceptance_radius) { + if (_vstatus.condition_landed || _global_pos.alt < takeoff_alt_amsl - _mission_item.acceptance_radius) { /* force TAKEOFF if landed or waypoint altitude is more than current */ _do_takeoff = true; - /* move current mission item to next */ - memcpy(&_mission_item_triplet.next, &_mission_item_triplet.current, sizeof(mission_item_s)); - _mission_item_triplet.next_valid = true; + /* move current position setpoint to next */ + memcpy(&_pos_sp_triplet.next, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - /* set current item to TAKEOFF */ - get_takeoff_item(&_mission_item_triplet.current); + /* set current setpoint to takeoff */ - _mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d; - _mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d; - _mission_item_triplet.current.altitude = takeoff_alt_amsl; - _mission_item_triplet.current.altitude_is_relative = false; + _pos_sp_triplet.current.lat = _global_pos.lat; + _pos_sp_triplet.current.lon = _global_pos.lon; + _pos_sp_triplet.current.alt = takeoff_alt_amsl; + _pos_sp_triplet.current.yaw = NAN; + _pos_sp_triplet.current.type = SETPOINT_TYPE_TAKEOFF; } - } else if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) { + + } else if (_mission_item.nav_cmd == NAV_CMD_LAND) { /* will need takeoff after landing */ _need_takeoff = true; } } if (_do_takeoff) { - mavlink_log_info(_mavlink_fd, "[navigator] takeoff to %.1fm", _mission_item_triplet.current.altitude); + mavlink_log_info(_mavlink_fd, "[navigator] takeoff to %.1fm above home", _pos_sp_triplet.current.alt - _home_pos.alt); + } else { if (onboard) { mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", index); + } else { mavlink_log_info(_mavlink_fd, "[navigator] heading to offboard WP %d", index); } } + } else { /* since a mission is not advanced without WPs available, this is not supposed to happen */ - _mission_item_triplet.current_valid = false; + _mission_item_valid = false; + _pos_sp_triplet.current.valid = false; warnx("ERROR: current WP can't be set"); } if (!_do_takeoff) { - ret = _mission.get_next_mission_item(&_mission_item_triplet.next); + mission_item_s item_next; + ret = _mission.get_next_mission_item(&item_next); if (ret == OK) { - add_home_pos_to_rtl(&_mission_item_triplet.next); - _mission_item_triplet.next_valid = true; + position_setpoint_from_mission_item(&_pos_sp_triplet.next, &item_next); + } else { /* this will fail for the last WP */ - _mission_item_triplet.next_valid = false; + _pos_sp_triplet.next.valid = false; } } - publish_mission_item_triplet(); + publish_position_setpoint_triplet(); } void Navigator::start_rtl() { - _reset_loiter_pos = true; _do_takeoff = false; - if (_rtl_state == RTL_STATE_NONE) - _rtl_state = RTL_STATE_CLIMB; - mavlink_log_info(_mavlink_fd, "[navigator] RTL started"); + if (_rtl_state == RTL_STATE_NONE) { + if (_global_pos.alt < _home_pos.alt + _parameters.rtl_alt) { + _rtl_state = RTL_STATE_CLIMB; + + } else { + _rtl_state = RTL_STATE_RETURN; + + if (_reset_loiter_pos) { + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _global_pos.alt; + } + } + } + + _reset_loiter_pos = true; set_rtl_item(); } +void +Navigator::start_land() +{ + _do_takeoff = false; + _reset_loiter_pos = true; + + _pos_sp_triplet.previous.valid = false; + _pos_sp_triplet.next.valid = false; + + _pos_sp_triplet.current.valid = true; + _pos_sp_triplet.current.type = SETPOINT_TYPE_LAND; + _pos_sp_triplet.current.lat = _global_pos.lat; + _pos_sp_triplet.current.lon = _global_pos.lon; + _pos_sp_triplet.current.alt = _global_pos.alt; + _pos_sp_triplet.current.loiter_direction = 1; + _pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius; + _pos_sp_triplet.current.yaw = NAN; +} + void Navigator::set_rtl_item() { switch (_rtl_state) { case RTL_STATE_CLIMB: { - memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); - _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; + memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - _mission_item_triplet.current_valid = true; - _mission_item_triplet.next_valid = false; + float climb_alt = _home_pos.alt + _parameters.rtl_alt; - float climb_alt = _home_pos.altitude + _parameters.rtl_alt; - if (_vstatus.condition_landed) - climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt); + if (_vstatus.condition_landed) { + climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt); + } + + _mission_item_valid = true; + + _mission_item.lat = _global_pos.lat; + _mission_item.lon = _global_pos.lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = climb_alt; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _parameters.loiter_radius; + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_TAKEOFF; + _mission_item.acceptance_radius = _parameters.acceptance_radius; + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); + + _pos_sp_triplet.next.valid = false; + + mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm above home", climb_alt - _home_pos.alt); + break; + } - _mission_item_triplet.current.altitude_is_relative = false; - _mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d; - _mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d; - _mission_item_triplet.current.altitude = climb_alt; - _mission_item_triplet.current.yaw = NAN; - _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; - _mission_item_triplet.current.loiter_direction = 1; - _mission_item_triplet.current.nav_cmd = NAV_CMD_TAKEOFF; - _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; - _mission_item_triplet.current.time_inside = 0.0f; - _mission_item_triplet.current.pitch_min = 0.0f; - _mission_item_triplet.current.autocontinue = true; - _mission_item_triplet.current.origin = ORIGIN_ONBOARD; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm", climb_alt - _home_pos.altitude); - break; - } case RTL_STATE_RETURN: { - memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); - _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; + memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - _mission_item_triplet.current_valid = true; - _mission_item_triplet.next_valid = false; + _mission_item_valid = true; + + _mission_item.lat = _home_pos.lat; + _mission_item.lon = _home_pos.lon; + // don't change altitude + _mission_item.yaw = NAN; // TODO set heading to home + _mission_item.loiter_radius = _parameters.loiter_radius; + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.acceptance_radius = _parameters.acceptance_radius; + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); + + _pos_sp_triplet.next.valid = false; + + mavlink_log_info(_mavlink_fd, "[navigator] RTL: return"); + break; + } - _mission_item_triplet.current.altitude_is_relative = false; - _mission_item_triplet.current.lat = _home_pos.lat; - _mission_item_triplet.current.lon = _home_pos.lon; - // don't change altitude setpoint - _mission_item_triplet.current.yaw = NAN; - _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; - _mission_item_triplet.current.loiter_direction = 1; - _mission_item_triplet.current.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; - _mission_item_triplet.current.time_inside = 0.0f; - _mission_item_triplet.current.pitch_min = 0.0f; - _mission_item_triplet.current.autocontinue = true; - _mission_item_triplet.current.origin = ORIGIN_ONBOARD; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: return"); - break; - } case RTL_STATE_DESCEND: { - memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); - _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; + memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - _mission_item_triplet.current_valid = true; - _mission_item_triplet.next_valid = false; + _mission_item_valid = true; - float descend_alt = _home_pos.altitude + _parameters.land_alt; + _mission_item.lat = _home_pos.lat; + _mission_item.lon = _home_pos.lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _home_pos.alt + _parameters.land_alt; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _parameters.loiter_radius; + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.acceptance_radius = _parameters.acceptance_radius; + _mission_item.time_inside = _parameters.rtl_land_delay < 0.0 ? 0.0f : _parameters.rtl_land_delay; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = _parameters.rtl_land_delay > -0.001f; + _mission_item.origin = ORIGIN_ONBOARD; + + position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); + + _pos_sp_triplet.next.valid = false; + + mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm above home", _mission_item.altitude - _home_pos.alt); + break; + } - _mission_item_triplet.current.altitude_is_relative = false; - _mission_item_triplet.current.lat = _home_pos.lat; - _mission_item_triplet.current.lon = _home_pos.lon; - _mission_item_triplet.current.altitude = descend_alt; - _mission_item_triplet.current.yaw = NAN; - _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; - _mission_item_triplet.current.loiter_direction = 1; - _mission_item_triplet.current.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; - _mission_item_triplet.current.time_inside = 0.0f; - _mission_item_triplet.current.pitch_min = 0.0f; - _mission_item_triplet.current.autocontinue = true; - _mission_item_triplet.current.origin = ORIGIN_ONBOARD; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm", descend_alt - _home_pos.altitude); - break; - } case RTL_STATE_LAND: { - memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); - _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; + memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - _mission_item_triplet.current_valid = true; - _mission_item_triplet.next_valid = false; + _mission_item_valid = true; + + _mission_item.lat = _home_pos.lat; + _mission_item.lon = _home_pos.lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _home_pos.alt; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _parameters.loiter_radius; + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_LAND; + _mission_item.acceptance_radius = _parameters.acceptance_radius; + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); + + _pos_sp_triplet.next.valid = false; + + mavlink_log_info(_mavlink_fd, "[navigator] RTL: land"); + break; + } - _mission_item_triplet.current.altitude_is_relative = false; - _mission_item_triplet.current.lat = _home_pos.lat; - _mission_item_triplet.current.lon = _home_pos.lon; - _mission_item_triplet.current.altitude = _home_pos.altitude; - _mission_item_triplet.current.yaw = NAN; - _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; - _mission_item_triplet.current.loiter_direction = 1; - _mission_item_triplet.current.nav_cmd = NAV_CMD_LAND; - _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; - _mission_item_triplet.current.time_inside = 0.0f; - _mission_item_triplet.current.pitch_min = 0.0f; - _mission_item_triplet.current.autocontinue = true; - _mission_item_triplet.current.origin = ORIGIN_ONBOARD; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: land"); - break; - } default: { - mavlink_log_critical(_mavlink_fd, "[navigator] error: unknown RTL state: %d", _rtl_state); - start_loiter(); - break; - } + mavlink_log_critical(_mavlink_fd, "[navigator] error: unknown RTL state: %d", _rtl_state); + start_loiter(); + break; + } } - publish_mission_item_triplet(); + publish_position_setpoint_triplet(); +} + +void +Navigator::position_setpoint_from_mission_item(position_setpoint_s *sp, mission_item_s *item) +{ + sp->valid = true; + + if (item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { + /* set home position for RTL item */ + sp->lat = _home_pos.lat; + sp->lon = _home_pos.lon; + sp->alt = _home_pos.alt + _parameters.rtl_alt; + + } else { + sp->lat = item->lat; + sp->lon = item->lon; + sp->alt = item->altitude_is_relative ? item->altitude + _home_pos.alt : item->altitude; + } + + sp->yaw = item->yaw; + sp->loiter_radius = item->loiter_radius; + sp->loiter_direction = item->loiter_direction; + sp->pitch_min = item->pitch_min; + + if (item->nav_cmd == NAV_CMD_TAKEOFF) { + sp->type = SETPOINT_TYPE_TAKEOFF; + + } else if (item->nav_cmd == NAV_CMD_LAND) { + sp->type = SETPOINT_TYPE_LAND; + + } else if (item->nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + item->nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + item->nav_cmd == NAV_CMD_LOITER_UNLIMITED) { + sp->type = SETPOINT_TYPE_LOITER; + + } else { + sp->type = SETPOINT_TYPE_NORMAL; + } } bool Navigator::check_mission_item_reached() { /* only check if there is actually a mission item to check */ - if (!_mission_item_triplet.current_valid) { + if (!_mission_item_valid) { return false; } - if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) { - return _vstatus.condition_landed; + if (_mission_item.nav_cmd == NAV_CMD_LAND) { + if (_vstatus.is_rotary_wing) { + return _vstatus.condition_landed; + + } else { + /* For fw there is currently no landing detector: + * make sure control is not stopped when overshooting the landing waypoint */ + return false; + } } /* XXX TODO count turns */ - if ((_mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) && - _mission_item_triplet.current.loiter_radius > 0.01f) { + if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) && + _mission_item.loiter_radius > 0.01f) { return false; - } + } uint64_t now = hrt_absolute_time(); if (!_waypoint_position_reached) { float acceptance_radius; - if (_mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT && _mission_item_triplet.current.acceptance_radius > 0.01f) { - acceptance_radius = _mission_item_triplet.current.acceptance_radius; + if (_mission_item.nav_cmd == NAV_CMD_WAYPOINT && _mission_item.acceptance_radius > 0.01f) { + acceptance_radius = _mission_item.acceptance_radius; } else { acceptance_radius = _parameters.acceptance_radius; @@ -1265,20 +1433,22 @@ Navigator::check_mission_item_reached() float dist_xy = -1.0f; float dist_z = -1.0f; - /* current relative or AMSL altitude depending on mission item altitude_is_relative flag */ - float wp_alt_amsl = _mission_item_triplet.current.altitude; - if (_mission_item_triplet.current.altitude_is_relative) - _mission_item_triplet.current.altitude += _home_pos.altitude; + /* calculate AMSL altitude for this waypoint */ + float wp_alt_amsl = _mission_item.altitude; - dist = get_distance_to_point_global_wgs84(_mission_item_triplet.current.lat, _mission_item_triplet.current.lon, wp_alt_amsl, - (double)_global_pos.lat / 1e7d, (double)_global_pos.lon / 1e7d, _global_pos.alt, - &dist_xy, &dist_z); + if (_mission_item.altitude_is_relative) + wp_alt_amsl += _home_pos.alt; + + dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl, + (double)_global_pos.lat, (double)_global_pos.lon, _global_pos.alt, + &dist_xy, &dist_z); if (_do_takeoff) { if (_global_pos.alt > wp_alt_amsl - acceptance_radius) { /* require only altitude for takeoff */ _waypoint_position_reached = true; } + } else { if (dist >= 0.0f && dist <= acceptance_radius) { _waypoint_position_reached = true; @@ -1287,12 +1457,14 @@ Navigator::check_mission_item_reached() } if (!_waypoint_yaw_reached) { - if (_vstatus.is_rotary_wing && !_do_takeoff && isfinite(_mission_item_triplet.current.yaw)) { + if (_vstatus.is_rotary_wing && !_do_takeoff && isfinite(_mission_item.yaw)) { /* check yaw if defined only for rotary wing except takeoff */ - float yaw_err = _wrap_pi(_mission_item_triplet.current.yaw - _global_pos.yaw); + float yaw_err = _wrap_pi(_mission_item.yaw - _global_pos.yaw); + if (fabsf(yaw_err) < 0.05f) { /* XXX get rid of magic number */ _waypoint_yaw_reached = true; } + } else { _waypoint_yaw_reached = true; } @@ -1302,20 +1474,22 @@ Navigator::check_mission_item_reached() if (_waypoint_position_reached && _waypoint_yaw_reached) { if (_time_first_inside_orbit == 0) { _time_first_inside_orbit = now; - if (_mission_item_triplet.current.time_inside > 0.01f) { - mavlink_log_info(_mavlink_fd, "[navigator] waypoint reached, wait for %.1fs", _mission_item_triplet.current.time_inside); + + if (_mission_item.time_inside > 0.01f) { + mavlink_log_info(_mavlink_fd, "[navigator] waypoint reached, wait for %.1fs", _mission_item.time_inside); } } - + /* check if the MAV was long enough inside the waypoint orbit */ - if ((now - _time_first_inside_orbit >= (uint64_t)_mission_item_triplet.current.time_inside * 1e6) - || _mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { + if ((now - _time_first_inside_orbit >= (uint64_t)_mission_item.time_inside * 1e6) + || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) { _time_first_inside_orbit = 0; _waypoint_yaw_reached = false; _waypoint_position_reached = false; return true; } } + return false; } @@ -1328,30 +1502,36 @@ Navigator::on_mission_item_reached() /* takeoff completed */ _do_takeoff = false; mavlink_log_info(_mavlink_fd, "[navigator] takeoff completed"); + } else { /* advance by one mission item */ _mission.move_to_next(); } if (_mission.current_mission_available()) { - advance_mission(); + set_mission_item(); + } else { /* if no more mission items available then finish mission */ /* loiter at last waypoint */ _reset_loiter_pos = false; mavlink_log_info(_mavlink_fd, "[navigator] mission completed"); + if (_vstatus.condition_landed) { dispatch(EVENT_READY_REQUESTED); + } else { dispatch(EVENT_LOITER_REQUESTED); } } + } else { /* RTL finished */ if (_rtl_state == RTL_STATE_LAND) { /* landed at home position */ mavlink_log_info(_mavlink_fd, "[navigator] RTL completed, landed"); dispatch(EVENT_READY_REQUESTED); + } else { /* next RTL step */ _rtl_state = (RTLState)(_rtl_state + 1); @@ -1361,67 +1541,16 @@ Navigator::on_mission_item_reached() } void -Navigator::get_loiter_item(struct mission_item_s *item) -{ - //item->altitude_is_relative - //item->lat - //item->lon - //item->altitude - //item->yaw - item->loiter_radius = _parameters.loiter_radius; - item->loiter_direction = 1; - item->nav_cmd = NAV_CMD_LOITER_UNLIMITED; - item->acceptance_radius = _parameters.acceptance_radius; - item->time_inside = 0.0f; - item->pitch_min = 0.0f; - item->autocontinue = false; - item->origin = ORIGIN_ONBOARD; - -} - -void -Navigator::get_takeoff_item(mission_item_s *item) -{ - //item->altitude_is_relative - //item->lat - //item->lon - //item->altitude - item->yaw = NAN; - item->loiter_radius = _parameters.loiter_radius; - item->loiter_direction = 1; - item->nav_cmd = NAV_CMD_TAKEOFF; - item->acceptance_radius = _parameters.acceptance_radius; - item->time_inside = 0.0f; - item->pitch_min = 0.0; - item->autocontinue = true; - item->origin = ORIGIN_ONBOARD; -} - -void -Navigator::add_home_pos_to_rtl(struct mission_item_s *new_mission_item) -{ - if (new_mission_item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { - /* append the home position to RTL item */ - new_mission_item->lat = _home_pos.lat; - new_mission_item->lon = _home_pos.lon; - new_mission_item->altitude = _home_pos.altitude + _parameters.rtl_alt; - new_mission_item->altitude_is_relative = false; - new_mission_item->loiter_radius = _parameters.loiter_radius; - new_mission_item->acceptance_radius = _parameters.acceptance_radius; - } -} - -void -Navigator::publish_mission_item_triplet() +Navigator::publish_position_setpoint_triplet() { /* lazily publish the mission triplet only once available */ - if (_triplet_pub > 0) { + if (_pos_sp_triplet_pub > 0) { /* publish the mission triplet */ - orb_publish(ORB_ID(mission_item_triplet), _triplet_pub, &_mission_item_triplet); + orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, &_pos_sp_triplet); } else { /* advertise and publish */ - _triplet_pub = orb_advertise(ORB_ID(mission_item_triplet), &_mission_item_triplet); + _pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), &_pos_sp_triplet); } } @@ -1436,42 +1565,85 @@ Navigator::publish_control_mode() _control_mode.flag_system_hil_enabled = _vstatus.hil_state == HIL_STATE_ON; _control_mode.flag_control_offboard_enabled = false; - _control_mode.flag_control_flighttermination_enabled = false; + _control_mode.flag_control_termination_enabled = false; - switch (_vstatus.main_state) { - case MAIN_STATE_MANUAL: - _control_mode.flag_control_manual_enabled = true; - _control_mode.flag_control_rates_enabled = _vstatus.is_rotary_wing; - _control_mode.flag_control_attitude_enabled = _vstatus.is_rotary_wing; + /* set this flag when navigator has control */ + bool navigator_enabled = false; + + switch (_vstatus.failsafe_state) { + case FAILSAFE_STATE_NORMAL: + switch (_vstatus.main_state) { + case MAIN_STATE_MANUAL: + _control_mode.flag_control_manual_enabled = true; + _control_mode.flag_control_rates_enabled = _vstatus.is_rotary_wing; + _control_mode.flag_control_attitude_enabled = _vstatus.is_rotary_wing; + _control_mode.flag_control_altitude_enabled = false; + _control_mode.flag_control_climb_rate_enabled = false; + _control_mode.flag_control_position_enabled = false; + _control_mode.flag_control_velocity_enabled = false; + break; + + case MAIN_STATE_SEATBELT: + _control_mode.flag_control_manual_enabled = true; + _control_mode.flag_control_rates_enabled = true; + _control_mode.flag_control_attitude_enabled = true; + _control_mode.flag_control_altitude_enabled = true; + _control_mode.flag_control_climb_rate_enabled = true; + _control_mode.flag_control_position_enabled = false; + _control_mode.flag_control_velocity_enabled = false; + break; + + case MAIN_STATE_EASY: + _control_mode.flag_control_manual_enabled = true; + _control_mode.flag_control_rates_enabled = true; + _control_mode.flag_control_attitude_enabled = true; + _control_mode.flag_control_altitude_enabled = true; + _control_mode.flag_control_climb_rate_enabled = true; + _control_mode.flag_control_position_enabled = true; + _control_mode.flag_control_velocity_enabled = true; + break; + + case MAIN_STATE_AUTO: + navigator_enabled = true; + break; + + default: + break; + } + + break; + + case FAILSAFE_STATE_RTL: + navigator_enabled = true; + break; + + case FAILSAFE_STATE_LAND: + navigator_enabled = true; + break; + + case FAILSAFE_STATE_TERMINATION: + navigator_enabled = true; + /* disable all controllers on termination */ + _control_mode.flag_control_manual_enabled = false; + _control_mode.flag_control_rates_enabled = false; + _control_mode.flag_control_attitude_enabled = false; + _control_mode.flag_control_position_enabled = false; + _control_mode.flag_control_velocity_enabled = false; _control_mode.flag_control_altitude_enabled = false; _control_mode.flag_control_climb_rate_enabled = false; - _control_mode.flag_control_position_enabled = false; - _control_mode.flag_control_velocity_enabled = false; + _control_mode.flag_control_termination_enabled = true; break; - case MAIN_STATE_SEATBELT: - _control_mode.flag_control_manual_enabled = true; - _control_mode.flag_control_rates_enabled = true; - _control_mode.flag_control_attitude_enabled = true; - _control_mode.flag_control_altitude_enabled = true; - _control_mode.flag_control_climb_rate_enabled = true; - _control_mode.flag_control_position_enabled = false; - _control_mode.flag_control_velocity_enabled = false; + default: break; + } - case MAIN_STATE_EASY: - _control_mode.flag_control_manual_enabled = true; - _control_mode.flag_control_rates_enabled = true; - _control_mode.flag_control_attitude_enabled = true; - _control_mode.flag_control_altitude_enabled = true; - _control_mode.flag_control_climb_rate_enabled = true; - _control_mode.flag_control_position_enabled = true; - _control_mode.flag_control_velocity_enabled = true; - break; - - case MAIN_STATE_AUTO: + /* navigator has control, set control mode flags according to nav state*/ + if (navigator_enabled) { _control_mode.flag_control_manual_enabled = false; - if (myState == NAV_STATE_READY) { + + switch (myState) { + case NAV_STATE_READY: /* disable all controllers, armed but idle */ _control_mode.flag_control_rates_enabled = false; _control_mode.flag_control_attitude_enabled = false; @@ -1479,18 +1651,28 @@ Navigator::publish_control_mode() _control_mode.flag_control_velocity_enabled = false; _control_mode.flag_control_altitude_enabled = false; _control_mode.flag_control_climb_rate_enabled = false; - } else { + break; + + case NAV_STATE_LAND: + /* land with or without position control */ + _control_mode.flag_control_manual_enabled = false; + _control_mode.flag_control_rates_enabled = true; + _control_mode.flag_control_attitude_enabled = true; + _control_mode.flag_control_position_enabled = _vstatus.condition_global_position_valid; + _control_mode.flag_control_velocity_enabled = _vstatus.condition_global_position_valid; + _control_mode.flag_control_altitude_enabled = true; + _control_mode.flag_control_climb_rate_enabled = true; + break; + + default: _control_mode.flag_control_rates_enabled = true; _control_mode.flag_control_attitude_enabled = true; _control_mode.flag_control_position_enabled = true; _control_mode.flag_control_velocity_enabled = true; _control_mode.flag_control_altitude_enabled = true; _control_mode.flag_control_climb_rate_enabled = true; + break; } - break; - - default: - break; } _control_mode.timestamp = hrt_absolute_time(); @@ -1506,24 +1688,6 @@ Navigator::publish_control_mode() } } -bool Navigator::cmp_mission_item_equivalent(const struct mission_item_s a, const struct mission_item_s b) { - if (a.altitude_is_relative == b.altitude_is_relative && - fabs(a.lat - b.lat) < FLT_EPSILON && - fabs(a.lon - b.lon) < FLT_EPSILON && - fabsf(a.altitude - b.altitude) < FLT_EPSILON && - fabsf(a.yaw - b.yaw) < FLT_EPSILON && - fabsf(a.loiter_radius - b.loiter_radius) < FLT_EPSILON && - a.loiter_direction == b.loiter_direction && - a.nav_cmd == b.nav_cmd && - fabsf(a.acceptance_radius - b.acceptance_radius) < FLT_EPSILON && - fabsf(a.time_inside - b.time_inside) < FLT_EPSILON && - a.autocontinue == b.autocontinue) { - return true; - } else { - return false; - } -} - void Navigator::add_fence_point(int argc, char *argv[]) { _geofence.addPoint(argc, argv); @@ -1579,8 +1743,9 @@ int navigator_main(int argc, char *argv[]) } else if (!strcmp(argv[1], "fence")) { navigator::g_navigator->add_fence_point(argc - 2, argv + 2); + } else if (!strcmp(argv[1], "fencefile")) { - navigator::g_navigator->load_fence_from_file(GEOFENCE_FILENAME); + navigator::g_navigator->load_fence_from_file(GEOFENCE_FILENAME); } else { usage(); diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index 8df47fc3b6..af1d9d7d58 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -1,8 +1,9 @@ /**************************************************************************** * * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: @autho Lorenz Meier + * Author: @author Lorenz Meier * @author Julian Oes + * @author Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -40,6 +41,7 @@ * * @author Lorenz Meier * @author Julian Oes + * @author Anton Babushkin */ #include @@ -55,6 +57,7 @@ PARAM_DEFINE_FLOAT(NAV_MIN_ALT, 50.0f); PARAM_DEFINE_FLOAT(NAV_ACCEPT_RAD, 10.0f); PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 100.0f); PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0); -PARAM_DEFINE_FLOAT(NAV_TAAKEOFF_ALT, 10.0f); // default TAKEOFF altitude +PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f); // default TAKEOFF altitude PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f); // slow descend from this altitude when landing PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f); // min altitude for going home in RTL mode +PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, 10.0f); // delay after descend before landing 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 5bf0fba30e..af04bb0bc4 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -71,15 +71,21 @@ #include "position_estimator_inav_params.h" #include "inertial_filter.h" +#define MIN_VALID_W 0.00001f + static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int position_estimator_inav_task; /**< Handle of deamon task / thread */ static bool verbose_mode = false; -static const hrt_abstime gps_timeout = 1000000; // GPS timeout = 1s -static const hrt_abstime flow_timeout = 1000000; // optical flow timeout = 1s +static const hrt_abstime gps_topic_timeout = 1000000; // GPS topic timeout = 1s +static const hrt_abstime flow_topic_timeout = 1000000; // optical flow topic timeout = 1s +static const hrt_abstime sonar_timeout = 150000; // sonar timeout = 150ms +static const hrt_abstime sonar_valid_timeout = 1000000; // estimate sonar distance during this time after sonar loss +static const hrt_abstime xy_src_timeout = 2000000; // estimate position during this time after position sources loss static const uint32_t updates_counter_len = 1000000; -static const uint32_t pub_interval = 4000; // limit publish rate to 250 Hz +static const uint32_t pub_interval = 10000; // limit publish rate to 100 Hz +static const float max_flow = 1.0f; // max flow value that can be used, rad/s __EXPORT int position_estimator_inav_main(int argc, char *argv[]); @@ -95,8 +101,7 @@ static void usage(const char *reason) if (reason) fprintf(stderr, "%s\n", reason); - fprintf(stderr, - "usage: position_estimator_inav {start|stop|status} [-v]\n\n"); + fprintf(stderr, "usage: position_estimator_inav {start|stop|status} [-v]\n\n"); exit(1); } @@ -115,7 +120,7 @@ int position_estimator_inav_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { if (thread_running) { - printf("position_estimator_inav already running\n"); + warnx("already running"); /* this is not an error */ exit(0); } @@ -135,16 +140,23 @@ int position_estimator_inav_main(int argc, char *argv[]) } if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; + if (thread_running) { + warnx("stop"); + thread_should_exit = true; + + } else { + warnx("app not started"); + } + exit(0); } if (!strcmp(argv[1], "status")) { if (thread_running) { - printf("\tposition_estimator_inav is running\n"); + warnx("app is running"); } else { - printf("\tposition_estimator_inav not started\n"); + warnx("app not started"); } exit(0); @@ -154,32 +166,92 @@ int position_estimator_inav_main(int argc, char *argv[]) exit(1); } +void write_debug_log(const char *msg, float dt, float x_est[3], float y_est[3], float z_est[3], float corr_acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v) { + FILE *f = fopen("/fs/microsd/inav.log", "a"); + if (f) { + char *s = malloc(256); + snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f %.5f] y_est=[%.5f %.5f %.5f] z_est=[%.5f %.5f %.5f]\n", hrt_absolute_time(), msg, dt, x_est[0], x_est[1], x_est[2], y_est[0], y_est[1], y_est[2], z_est[0], z_est[1], z_est[2]); + fputs(f, s); + snprintf(s, 256, "\tacc_corr=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f\n", corr_acc[0], corr_acc[1], corr_acc[2], corr_gps[0][0], corr_gps[1][0], corr_gps[2][0], corr_gps[0][1], corr_gps[1][1], corr_gps[2][1], w_xy_gps_p, w_xy_gps_v); + fputs(f, s); + free(s); + } + fclose(f); +} + /**************************************************************************** * main ****************************************************************************/ int position_estimator_inav_thread_main(int argc, char *argv[]) { - warnx("started."); + warnx("started"); int mavlink_fd; mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); mavlink_log_info(mavlink_fd, "[inav] started"); - /* initialize values */ float x_est[3] = { 0.0f, 0.0f, 0.0f }; float y_est[3] = { 0.0f, 0.0f, 0.0f }; float z_est[3] = { 0.0f, 0.0f, 0.0f }; int baro_init_cnt = 0; int baro_init_num = 200; - float baro_alt0 = 0.0f; /* to determine while start up */ + float baro_offset = 0.0f; // baro offset for reference altitude, initialized on start, then adjusted + float surface_offset = 0.0f; // ground level offset from reference altitude + float surface_offset_rate = 0.0f; // surface offset change rate float alt_avg = 0.0f; bool landed = true; hrt_abstime landed_time = 0; + bool flag_armed = false; uint32_t accel_counter = 0; uint32_t baro_counter = 0; + bool ref_inited = false; + hrt_abstime ref_init_start = 0; + const hrt_abstime ref_init_delay = 1000000; // wait for 1s after 3D fix + + uint16_t accel_updates = 0; + uint16_t baro_updates = 0; + uint16_t gps_updates = 0; + uint16_t attitude_updates = 0; + uint16_t flow_updates = 0; + + hrt_abstime updates_counter_start = hrt_absolute_time(); + hrt_abstime pub_last = hrt_absolute_time(); + + hrt_abstime t_prev = 0; + + /* acceleration in NED frame */ + float accel_NED[3] = { 0.0f, 0.0f, -CONSTANTS_ONE_G }; + + /* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */ + float corr_acc[] = { 0.0f, 0.0f, 0.0f }; // N E D + float acc_bias[] = { 0.0f, 0.0f, 0.0f }; // body frame + float corr_baro = 0.0f; // D + float corr_gps[3][2] = { + { 0.0f, 0.0f }, // N (pos, vel) + { 0.0f, 0.0f }, // E (pos, vel) + { 0.0f, 0.0f }, // D (pos, vel) + }; + float w_gps_xy = 1.0f; + float w_gps_z = 1.0f; + float corr_sonar = 0.0f; + float corr_sonar_filtered = 0.0f; + + float corr_flow[] = { 0.0f, 0.0f }; // N E + float w_flow = 0.0f; + + float sonar_prev = 0.0f; + hrt_abstime sonar_time = 0; // time of last sonar measurement (not filtered) + hrt_abstime sonar_valid_time = 0; // time of last sonar measurement used for correction (filtered) + hrt_abstime xy_src_time = 0; // time of last available position data + + bool gps_valid = false; // GPS is valid + bool sonar_valid = false; // sonar is valid + bool flow_valid = false; // flow is valid + bool flow_accurate = false; // flow should be accurate (this flag not updated if flow_valid == false) + /* declare and safely initialize all structs */ struct actuator_controls_s actuator; memset(&actuator, 0, sizeof(actuator)); @@ -247,75 +319,29 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* mean calculation over several measurements */ if (baro_init_cnt < baro_init_num) { - baro_alt0 += sensor.baro_alt_meter; + baro_offset += sensor.baro_alt_meter; baro_init_cnt++; } else { wait_baro = false; - baro_alt0 /= (float) baro_init_cnt; - warnx("init baro: alt = %.3f", baro_alt0); - mavlink_log_info(mavlink_fd, "[inav] init baro: alt = %.3f", baro_alt0); - local_pos.ref_alt = baro_alt0; - local_pos.ref_timestamp = hrt_absolute_time(); + baro_offset /= (float) baro_init_cnt; + warnx("baro offs: %.2f", baro_offset); + mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", baro_offset); local_pos.z_valid = true; local_pos.v_z_valid = true; - local_pos.z_global = true; } } } } } - bool ref_xy_inited = false; - hrt_abstime ref_xy_init_start = 0; - const hrt_abstime ref_xy_init_delay = 5000000; // wait for 5s after 3D fix - - hrt_abstime t_prev = 0; - - uint16_t accel_updates = 0; - uint16_t baro_updates = 0; - uint16_t gps_updates = 0; - uint16_t attitude_updates = 0; - uint16_t flow_updates = 0; - - hrt_abstime updates_counter_start = hrt_absolute_time(); - hrt_abstime pub_last = hrt_absolute_time(); - - /* acceleration in NED frame */ - float accel_NED[3] = { 0.0f, 0.0f, -CONSTANTS_ONE_G }; - - /* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */ - float accel_corr[] = { 0.0f, 0.0f, 0.0f }; // N E D - float accel_bias[] = { 0.0f, 0.0f, 0.0f }; // body frame - float baro_corr = 0.0f; // D - float gps_corr[2][2] = { - { 0.0f, 0.0f }, // N (pos, vel) - { 0.0f, 0.0f }, // E (pos, vel) - }; - float sonar_corr = 0.0f; - float sonar_corr_filtered = 0.0f; - float flow_corr[] = { 0.0f, 0.0f }; // X, Y - - float sonar_prev = 0.0f; - hrt_abstime sonar_time = 0; - /* main loop */ - struct pollfd fds[7] = { - { .fd = parameter_update_sub, .events = POLLIN }, - { .fd = actuator_sub, .events = POLLIN }, - { .fd = armed_sub, .events = POLLIN }, + struct pollfd fds[1] = { { .fd = vehicle_attitude_sub, .events = POLLIN }, - { .fd = sensor_combined_sub, .events = POLLIN }, - { .fd = optical_flow_sub, .events = POLLIN }, - { .fd = vehicle_gps_position_sub, .events = POLLIN } }; - if (!thread_should_exit) { - warnx("main loop started."); - } - while (!thread_should_exit) { - int ret = poll(fds, 7, 10); // wait maximal this 10 ms = 100 Hz minimum rate + int ret = poll(fds, 1, 20); // wait maximal 20 ms = 50 Hz minimum rate hrt_abstime t = hrt_absolute_time(); if (ret < 0) { @@ -324,40 +350,60 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) continue; } else if (ret > 0) { + /* act on attitude updates */ + + /* vehicle attitude */ + orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); + attitude_updates++; + + bool updated; + /* parameter update */ - if (fds[0].revents & POLLIN) { - /* read from param to clear updated flag */ + orb_check(parameter_update_sub, &updated); + + if (updated) { struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), parameter_update_sub, - &update); - /* update parameters */ + orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); parameters_update(&pos_inav_param_handles, ¶ms); } /* actuator */ - if (fds[1].revents & POLLIN) { + orb_check(actuator_sub, &updated); + + if (updated) { orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_sub, &actuator); } /* armed */ - if (fds[2].revents & POLLIN) { - orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); - } + orb_check(armed_sub, &updated); - /* vehicle attitude */ - if (fds[3].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); - attitude_updates++; + if (updated) { + orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); + + /* reset ground level on arm */ + if (armed.armed && !flag_armed) { + flag_armed = armed.armed; + baro_offset -= z_est[0]; + corr_baro = 0.0f; + local_pos.ref_alt -= z_est[0]; + local_pos.ref_timestamp = t; + z_est[0] = 0.0f; + alt_avg = 0.0f; + } } /* sensor combined */ - if (fds[4].revents & POLLIN) { + orb_check(sensor_combined_sub, &updated); + + if (updated) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); if (sensor.accelerometer_counter != accel_counter) { if (att.R_valid) { - /* correct accel bias, now only for Z */ - sensor.accelerometer_m_s2[2] -= accel_bias[2]; + /* correct accel bias */ + sensor.accelerometer_m_s2[0] -= acc_bias[0]; + sensor.accelerometer_m_s2[1] -= acc_bias[1]; + sensor.accelerometer_m_s2[2] -= acc_bias[2]; /* transform acceleration vector from body frame to NED frame */ for (int i = 0; i < 3; i++) { @@ -368,12 +414,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } - accel_corr[0] = accel_NED[0] - x_est[2]; - accel_corr[1] = accel_NED[1] - y_est[2]; - accel_corr[2] = accel_NED[2] + CONSTANTS_ONE_G - z_est[2]; + corr_acc[0] = accel_NED[0] - x_est[2]; + corr_acc[1] = accel_NED[1] - y_est[2]; + corr_acc[2] = accel_NED[2] + CONSTANTS_ONE_G - z_est[2]; } else { - memset(accel_corr, 0, sizeof(accel_corr)); + memset(corr_acc, 0, sizeof(corr_acc)); } accel_counter = sensor.accelerometer_counter; @@ -381,180 +427,352 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } if (sensor.baro_counter != baro_counter) { - baro_corr = - sensor.baro_alt_meter - z_est[0]; + corr_baro = baro_offset - sensor.baro_alt_meter - z_est[0]; baro_counter = sensor.baro_counter; baro_updates++; } } /* optical flow */ - if (fds[5].revents & POLLIN) { + orb_check(optical_flow_sub, &updated); + + if (updated) { orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow); - if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && (flow.ground_distance_m != sonar_prev || t - sonar_time < 150000)) { - if (flow.ground_distance_m != sonar_prev) { - sonar_time = t; - sonar_prev = flow.ground_distance_m; - sonar_corr = -flow.ground_distance_m - z_est[0]; - sonar_corr_filtered += (sonar_corr - sonar_corr_filtered) * params.sonar_filt; + if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && att.R[2][2] > 0.7 && flow.ground_distance_m != sonar_prev) { + sonar_time = t; + sonar_prev = flow.ground_distance_m; + corr_sonar = flow.ground_distance_m + surface_offset + z_est[0]; + corr_sonar_filtered += (corr_sonar - corr_sonar_filtered) * params.sonar_filt; - if (fabsf(sonar_corr) > params.sonar_err) { - // correction is too large: spike or new ground level? - if (fabsf(sonar_corr - sonar_corr_filtered) > params.sonar_err) { - // spike detected, ignore - sonar_corr = 0.0f; + if (fabsf(corr_sonar) > params.sonar_err) { + /* correction is too large: spike or new ground level? */ + if (fabsf(corr_sonar - corr_sonar_filtered) > params.sonar_err) { + /* spike detected, ignore */ + corr_sonar = 0.0f; + sonar_valid = false; - } else { - // new ground level - baro_alt0 += sonar_corr; - mavlink_log_info(mavlink_fd, "[inav] new home: alt = %.3f", baro_alt0); - local_pos.ref_alt = baro_alt0; - local_pos.ref_timestamp = hrt_absolute_time(); - z_est[0] += sonar_corr; - sonar_corr = 0.0f; - sonar_corr_filtered = 0.0f; - } + } else { + /* new ground level */ + surface_offset -= corr_sonar; + surface_offset_rate = 0.0f; + corr_sonar = 0.0f; + corr_sonar_filtered = 0.0f; + sonar_valid_time = t; + sonar_valid = true; + local_pos.surface_bottom_timestamp = t; + mavlink_log_info(mavlink_fd, "[inav] new surface level: %.2f", surface_offset); + } + + } else { + /* correction is ok, use it */ + sonar_valid_time = t; + sonar_valid = true; + } + } + + float flow_q = flow.quality / 255.0f; + float dist_bottom = - z_est[0] - surface_offset; + + if (dist_bottom > 0.3f && flow_q > params.flow_q_min && (t < sonar_valid_time + sonar_valid_timeout) && att.R[2][2] > 0.7) { + /* distance to surface */ + float flow_dist = dist_bottom / att.R[2][2]; + /* check if flow if too large for accurate measurements */ + /* calculate estimated velocity in body frame */ + float body_v_est[2] = { 0.0f, 0.0f }; + + for (int i = 0; i < 2; i++) { + body_v_est[i] = att.R[0][i] * x_est[1] + att.R[1][i] * y_est[1] + att.R[2][i] * z_est[1]; + } + + /* set this flag if flow should be accurate according to current velocity and attitude rate estimate */ + flow_accurate = fabsf(body_v_est[1] / flow_dist - att.rollspeed) < max_flow && + fabsf(body_v_est[0] / flow_dist + att.pitchspeed) < max_flow; + + /* convert raw flow to angular flow */ + float flow_ang[2]; + flow_ang[0] = flow.flow_raw_x * params.flow_k; + flow_ang[1] = flow.flow_raw_y * params.flow_k; + /* flow measurements vector */ + float flow_m[3]; + flow_m[0] = -flow_ang[0] * flow_dist; + flow_m[1] = -flow_ang[1] * flow_dist; + flow_m[2] = z_est[1]; + /* velocity in NED */ + float flow_v[2] = { 0.0f, 0.0f }; + + /* project measurements vector to NED basis, skip Z component */ + for (int i = 0; i < 2; i++) { + for (int j = 0; j < 3; j++) { + flow_v[i] += att.R[i][j] * flow_m[j]; } } + /* velocity correction */ + corr_flow[0] = flow_v[0] - x_est[1]; + corr_flow[1] = flow_v[1] - y_est[1]; + /* adjust correction weight */ + float flow_q_weight = (flow_q - params.flow_q_min) / (1.0f - params.flow_q_min); + w_flow = att.R[2][2] * flow_q_weight / fmaxf(1.0f, flow_dist); + + /* if flow is not accurate, reduce weight for it */ + // TODO make this more fuzzy + if (!flow_accurate) + w_flow *= 0.05f; + + flow_valid = true; + } else { - sonar_corr = 0.0f; + w_flow = 0.0f; + flow_valid = false; } flow_updates++; } /* vehicle GPS position */ - if (fds[6].revents & POLLIN) { + orb_check(vehicle_gps_position_sub, &updated); + + if (updated) { orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); - if (gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout) { - /* initialize reference position if needed */ - if (!ref_xy_inited) { - /* require EPH < 10m */ - if (gps.eph_m < 10.0f) { - if (ref_xy_init_start == 0) { - ref_xy_init_start = t; - - } else if (t > ref_xy_init_start + ref_xy_init_delay) { - ref_xy_inited = true; - /* reference GPS position */ - double lat = gps.lat * 1e-7; - double lon = gps.lon * 1e-7; - - local_pos.ref_lat = gps.lat; - local_pos.ref_lon = gps.lon; - local_pos.ref_timestamp = t; - - /* initialize projection */ - map_projection_init(lat, lon); - warnx("init GPS: lat = %.10f, lon = %.10f", lat, lon); - mavlink_log_info(mavlink_fd, "[inav] init GPS: %.7f, %.7f", lat, lon); - } - } else { - ref_xy_init_start = 0; + if (gps.fix_type >= 3) { + /* hysteresis for GPS quality */ + if (gps_valid) { + if (gps.eph_m > 10.0f || gps.epv_m > 10.0f) { + gps_valid = false; + mavlink_log_info(mavlink_fd, "[inav] GPS signal lost"); } - } - if (ref_xy_inited) { - /* project GPS lat lon to plane */ - float gps_proj[2]; - map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1])); - /* calculate correction for position */ - gps_corr[0][0] = gps_proj[0] - x_est[0]; - gps_corr[1][0] = gps_proj[1] - y_est[0]; - - /* calculate correction for velocity */ - if (gps.vel_ned_valid) { - gps_corr[0][1] = gps.vel_n_m_s - x_est[1]; - gps_corr[1][1] = gps.vel_e_m_s - y_est[1]; - - } else { - gps_corr[0][1] = 0.0f; - gps_corr[1][1] = 0.0f; + } else { + if (gps.eph_m < 5.0f && gps.epv_m < 5.0f) { + gps_valid = true; + mavlink_log_info(mavlink_fd, "[inav] GPS signal found"); } } + } else { + gps_valid = false; + } + + if (gps_valid) { + /* initialize reference position if needed */ + if (!ref_inited) { + if (ref_init_start == 0) { + ref_init_start = t; + + } else if (t > ref_init_start + ref_init_delay) { + ref_inited = true; + /* reference GPS position */ + double lat = gps.lat * 1e-7; + double lon = gps.lon * 1e-7; + float alt = gps.alt * 1e-3; + + local_pos.ref_lat = gps.lat; + local_pos.ref_lon = gps.lon; + local_pos.ref_alt = alt + z_est[0]; + local_pos.ref_timestamp = t; + + /* initialize projection */ + map_projection_init(lat, lon); + warnx("init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt); + mavlink_log_info(mavlink_fd, "[inav] init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt); + } + } + + if (ref_inited) { + /* project GPS lat lon to plane */ + float gps_proj[2]; + map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1])); + /* calculate correction for position */ + corr_gps[0][0] = gps_proj[0] - x_est[0]; + corr_gps[1][0] = gps_proj[1] - y_est[0]; + corr_gps[2][0] = local_pos.ref_alt - gps.alt * 1e-3 - z_est[0]; + + /* calculate correction for velocity */ + if (gps.vel_ned_valid) { + corr_gps[0][1] = gps.vel_n_m_s - x_est[1]; + corr_gps[1][1] = gps.vel_e_m_s - y_est[1]; + corr_gps[2][1] = gps.vel_d_m_s - z_est[1]; + + } else { + corr_gps[0][1] = 0.0f; + corr_gps[1][1] = 0.0f; + corr_gps[2][1] = 0.0f; + } + + w_gps_xy = 1.0f / fmaxf(1.0f, gps.eph_m); + w_gps_z = 1.0f / fmaxf(1.0f, gps.epv_m); + } + } else { /* no GPS lock */ - memset(gps_corr, 0, sizeof(gps_corr)); - ref_xy_init_start = 0; + memset(corr_gps, 0, sizeof(corr_gps)); + ref_init_start = 0; } gps_updates++; } } - /* end of poll return value check */ + /* check for timeout on FLOW topic */ + if ((flow_valid || sonar_valid) && t > flow.timestamp + flow_topic_timeout) { + flow_valid = false; + sonar_valid = false; + warnx("FLOW timeout"); + mavlink_log_info(mavlink_fd, "[inav] FLOW timeout"); + } + + /* check for timeout on GPS topic */ + if (gps_valid && t > gps.timestamp_position + gps_topic_timeout) { + gps_valid = false; + warnx("GPS timeout"); + mavlink_log_info(mavlink_fd, "[inav] GPS timeout"); + } + + /* check for sonar measurement timeout */ + if (sonar_valid && t > sonar_time + sonar_timeout) { + corr_sonar = 0.0f; + sonar_valid = false; + } float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f; t_prev = t; - /* reset ground level on arm */ - if (armed.armed && !flag_armed) { - baro_alt0 -= z_est[0]; - z_est[0] = 0.0f; - local_pos.ref_alt = baro_alt0; - local_pos.ref_timestamp = hrt_absolute_time(); - mavlink_log_info(mavlink_fd, "[inav] new home on arm: alt = %.3f", baro_alt0); + /* use GPS if it's valid and reference position initialized */ + bool use_gps_xy = ref_inited && gps_valid && params.w_xy_gps_p > MIN_VALID_W; + bool use_gps_z = ref_inited && gps_valid && params.w_z_gps_p > MIN_VALID_W; + /* use flow if it's valid and (accurate or no GPS available) */ + bool use_flow = flow_valid && (flow_accurate || !use_gps_xy); + + /* try to estimate position during some time after position sources lost */ + if (use_gps_xy || use_flow) { + xy_src_time = t; } - /* accel bias correction, now only for Z - * not strictly correct, but stable and works */ - accel_bias[2] += (accel_NED[2] + CONSTANTS_ONE_G) * params.w_acc_bias * dt; + bool can_estimate_xy = (t < xy_src_time + xy_src_timeout); + + bool dist_bottom_valid = (t < sonar_valid_time + sonar_valid_timeout); + + if (dist_bottom_valid) { + /* surface distance prediction */ + surface_offset += surface_offset_rate * dt; + + /* surface distance correction */ + if (sonar_valid) { + surface_offset_rate -= corr_sonar * 0.5f * params.w_z_sonar * params.w_z_sonar * dt; + surface_offset -= corr_sonar * params.w_z_sonar * dt; + } + } + + float w_xy_gps_p = params.w_xy_gps_p * w_gps_xy; + float w_xy_gps_v = params.w_xy_gps_v * w_gps_xy; + float w_z_gps_p = params.w_z_gps_p * w_gps_z; + + /* reduce GPS weight if optical flow is good */ + if (use_flow && flow_accurate) { + w_xy_gps_p *= params.w_gps_flow; + w_xy_gps_v *= params.w_gps_flow; + } + + /* baro offset correction */ + if (use_gps_z) { + float offs_corr = corr_gps[2][0] * w_z_gps_p * dt; + baro_offset += offs_corr; + baro_counter += offs_corr; + } + + /* accelerometer bias correction */ + float accel_bias_corr[3] = { 0.0f, 0.0f, 0.0f }; + + if (use_gps_xy) { + accel_bias_corr[0] -= corr_gps[0][0] * w_xy_gps_p * w_xy_gps_p; + accel_bias_corr[0] -= corr_gps[0][1] * w_xy_gps_v; + accel_bias_corr[1] -= corr_gps[1][0] * w_xy_gps_p * w_xy_gps_p; + accel_bias_corr[1] -= corr_gps[1][1] * w_xy_gps_v; + } + + if (use_gps_z) { + accel_bias_corr[2] -= corr_gps[2][0] * w_z_gps_p * w_z_gps_p; + } + + if (use_flow) { + accel_bias_corr[0] -= corr_flow[0] * params.w_xy_flow; + accel_bias_corr[1] -= corr_flow[1] * params.w_xy_flow; + } + + accel_bias_corr[2] -= corr_baro * params.w_z_baro * params.w_z_baro; + + /* transform error vector from NED frame to body frame */ + for (int i = 0; i < 3; i++) { + float c = 0.0f; + + for (int j = 0; j < 3; j++) { + c += att.R[j][i] * accel_bias_corr[j]; + } + + acc_bias[i] += c * params.w_acc_bias * dt; + } /* inertial filter prediction for altitude */ inertial_filter_predict(dt, z_est); /* inertial filter correction for altitude */ - baro_alt0 += sonar_corr * params.w_alt_sonar * dt; - inertial_filter_correct(baro_corr + baro_alt0, dt, z_est, 0, params.w_alt_baro); - inertial_filter_correct(sonar_corr, dt, z_est, 0, params.w_alt_sonar); - inertial_filter_correct(accel_corr[2], dt, z_est, 2, params.w_alt_acc); - - bool gps_valid = ref_xy_inited && gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout; - bool flow_valid = false; // TODO implement opt flow - - /* try to estimate xy even if no absolute position source available, - * if using optical flow velocity will be correct in this case */ - bool can_estimate_xy = gps_valid || flow_valid; + inertial_filter_correct(corr_baro, dt, z_est, 0, params.w_z_baro); + inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p); + inertial_filter_correct(corr_acc[2], dt, z_est, 2, params.w_z_acc); if (can_estimate_xy) { /* inertial filter prediction for position */ inertial_filter_predict(dt, x_est); inertial_filter_predict(dt, y_est); + if (!isfinite(x_est[0]) || !isfinite(y_est[0])) { + write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v); + thread_should_exit = true; + } + /* inertial filter correction for position */ - inertial_filter_correct(accel_corr[0], dt, x_est, 2, params.w_pos_acc); - inertial_filter_correct(accel_corr[1], dt, y_est, 2, params.w_pos_acc); + inertial_filter_correct(corr_acc[0], dt, x_est, 2, params.w_xy_acc); + inertial_filter_correct(corr_acc[1], dt, y_est, 2, params.w_xy_acc); - if (gps_valid) { - inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, params.w_pos_gps_p); - inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, params.w_pos_gps_p); + if (use_flow) { + inertial_filter_correct(corr_flow[0], dt, x_est, 1, params.w_xy_flow * w_flow); + inertial_filter_correct(corr_flow[1], dt, y_est, 1, params.w_xy_flow * w_flow); + } - if (gps.vel_ned_valid && t < gps.timestamp_velocity + gps_timeout) { - inertial_filter_correct(gps_corr[0][1], dt, x_est, 1, params.w_pos_gps_v); - inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, params.w_pos_gps_v); + if (use_gps_xy) { + inertial_filter_correct(corr_gps[0][0], dt, x_est, 0, w_xy_gps_p); + inertial_filter_correct(corr_gps[1][0], dt, y_est, 0, w_xy_gps_p); + + if (gps.vel_ned_valid && t < gps.timestamp_velocity + gps_topic_timeout) { + inertial_filter_correct(corr_gps[0][1], dt, x_est, 1, w_xy_gps_v); + inertial_filter_correct(corr_gps[1][1], dt, y_est, 1, w_xy_gps_v); } } + + if (!isfinite(x_est[0]) || !isfinite(y_est[0])) { + write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v); + thread_should_exit = true; + } } /* detect land */ - alt_avg += (z_est[0] - alt_avg) * dt / params.land_t; - float alt_disp = z_est[0] - alt_avg; - alt_disp = alt_disp * alt_disp; + alt_avg += (- z_est[0] - alt_avg) * dt / params.land_t; + float alt_disp2 = - z_est[0] - alt_avg; + alt_disp2 = alt_disp2 * alt_disp2; float land_disp2 = params.land_disp * params.land_disp; /* get actual thrust output */ float thrust = armed.armed ? actuator.control[3] : 0.0f; if (landed) { - if (alt_disp > land_disp2 && thrust > params.land_thr) { + if (alt_disp2 > land_disp2 && thrust > params.land_thr) { landed = false; landed_time = 0; } } else { - if (alt_disp < land_disp2 && thrust < params.land_thr) { + if (alt_disp2 < land_disp2 && thrust < params.land_thr) { if (landed_time == 0) { landed_time = t; // land detected first time @@ -593,10 +811,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (t > pub_last + pub_interval) { pub_last = t; /* publish local position */ - local_pos.timestamp = t; - local_pos.xy_valid = can_estimate_xy && gps_valid; + local_pos.xy_valid = can_estimate_xy && use_gps_xy; local_pos.v_xy_valid = can_estimate_xy; - local_pos.xy_global = local_pos.xy_valid && gps_valid; // will make sense when local position sources (e.g. vicon) will be implemented + local_pos.xy_global = local_pos.xy_valid && use_gps_xy; + local_pos.z_global = local_pos.z_valid && use_gps_z; local_pos.x = x_est[0]; local_pos.vx = x_est[1]; local_pos.y = y_est[0]; @@ -605,6 +823,14 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) local_pos.vz = z_est[1]; local_pos.landed = landed; local_pos.yaw = att.yaw; + local_pos.dist_bottom_valid = dist_bottom_valid; + + if (local_pos.dist_bottom_valid) { + local_pos.dist_bottom = -z_est[0] - surface_offset; + local_pos.dist_bottom_rate = -z_est[1] - surface_offset_rate; + } + + local_pos.timestamp = t; orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos); @@ -614,19 +840,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (local_pos.xy_global) { double est_lat, est_lon; map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon); - global_pos.lat = (int32_t)(est_lat * 1e7d); - global_pos.lon = (int32_t)(est_lon * 1e7d); + global_pos.lat = est_lat; + global_pos.lon = est_lon; global_pos.time_gps_usec = gps.time_gps_usec; } /* set valid values even if position is not valid */ if (local_pos.v_xy_valid) { - global_pos.vx = local_pos.vx; - global_pos.vy = local_pos.vy; - } - - if (local_pos.z_valid) { - global_pos.relative_alt = -local_pos.z; + global_pos.vel_n = local_pos.vx; + global_pos.vel_e = local_pos.vy; } if (local_pos.z_global) { @@ -634,19 +856,19 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } if (local_pos.v_z_valid) { - global_pos.vz = local_pos.vz; + global_pos.vel_d = local_pos.vz; } + global_pos.yaw = local_pos.yaw; global_pos.timestamp = t; orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos); } - flag_armed = armed.armed; } - warnx("exiting."); - mavlink_log_info(mavlink_fd, "[inav] exiting"); + warnx("stopped"); + mavlink_log_info(mavlink_fd, "[inav] stopped"); thread_running = false; return 0; } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index 4f9ddd009d..e1bbd75a6e 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -40,16 +40,19 @@ #include "position_estimator_inav_params.h" -PARAM_DEFINE_FLOAT(INAV_W_ALT_BARO, 0.5f); -PARAM_DEFINE_FLOAT(INAV_W_ALT_ACC, 50.0f); -PARAM_DEFINE_FLOAT(INAV_W_ALT_SONAR, 3.0f); -PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_P, 1.0f); -PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_V, 2.0f); -PARAM_DEFINE_FLOAT(INAV_W_POS_ACC, 10.0f); -PARAM_DEFINE_FLOAT(INAV_W_POS_FLOW, 0.0f); -PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.0f); -PARAM_DEFINE_FLOAT(INAV_FLOW_K, 1.0f); -PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.02f); +PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 1.0f); +PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.005f); +PARAM_DEFINE_FLOAT(INAV_W_Z_ACC, 20.0f); +PARAM_DEFINE_FLOAT(INAV_W_Z_SONAR, 3.0f); +PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_P, 1.0f); +PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f); +PARAM_DEFINE_FLOAT(INAV_W_XY_ACC, 20.0f); +PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f); +PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f); +PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.05f); +PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.0165f); +PARAM_DEFINE_FLOAT(INAV_FLOW_Q_MIN, 0.5f); +PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.05f); PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f); PARAM_DEFINE_FLOAT(INAV_LAND_T, 3.0f); PARAM_DEFINE_FLOAT(INAV_LAND_DISP, 0.7f); @@ -57,15 +60,18 @@ PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.3f); int parameters_init(struct position_estimator_inav_param_handles *h) { - h->w_alt_baro = param_find("INAV_W_ALT_BARO"); - h->w_alt_acc = param_find("INAV_W_ALT_ACC"); - h->w_alt_sonar = param_find("INAV_W_ALT_SONAR"); - h->w_pos_gps_p = param_find("INAV_W_POS_GPS_P"); - h->w_pos_gps_v = param_find("INAV_W_POS_GPS_V"); - h->w_pos_acc = param_find("INAV_W_POS_ACC"); - h->w_pos_flow = param_find("INAV_W_POS_FLOW"); + h->w_z_baro = param_find("INAV_W_Z_BARO"); + h->w_z_gps_p = param_find("INAV_W_Z_GPS_P"); + h->w_z_acc = param_find("INAV_W_Z_ACC"); + h->w_z_sonar = param_find("INAV_W_Z_SONAR"); + h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P"); + h->w_xy_gps_v = param_find("INAV_W_XY_GPS_V"); + h->w_xy_acc = param_find("INAV_W_XY_ACC"); + h->w_xy_flow = param_find("INAV_W_XY_FLOW"); + h->w_gps_flow = param_find("INAV_W_GPS_FLOW"); h->w_acc_bias = param_find("INAV_W_ACC_BIAS"); h->flow_k = param_find("INAV_FLOW_K"); + h->flow_q_min = param_find("INAV_FLOW_Q_MIN"); h->sonar_filt = param_find("INAV_SONAR_FILT"); h->sonar_err = param_find("INAV_SONAR_ERR"); h->land_t = param_find("INAV_LAND_T"); @@ -77,15 +83,18 @@ int parameters_init(struct position_estimator_inav_param_handles *h) int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p) { - param_get(h->w_alt_baro, &(p->w_alt_baro)); - param_get(h->w_alt_acc, &(p->w_alt_acc)); - param_get(h->w_alt_sonar, &(p->w_alt_sonar)); - param_get(h->w_pos_gps_p, &(p->w_pos_gps_p)); - param_get(h->w_pos_gps_v, &(p->w_pos_gps_v)); - param_get(h->w_pos_acc, &(p->w_pos_acc)); - param_get(h->w_pos_flow, &(p->w_pos_flow)); + param_get(h->w_z_baro, &(p->w_z_baro)); + param_get(h->w_z_gps_p, &(p->w_z_gps_p)); + param_get(h->w_z_acc, &(p->w_z_acc)); + param_get(h->w_z_sonar, &(p->w_z_sonar)); + param_get(h->w_xy_gps_p, &(p->w_xy_gps_p)); + param_get(h->w_xy_gps_v, &(p->w_xy_gps_v)); + param_get(h->w_xy_acc, &(p->w_xy_acc)); + param_get(h->w_xy_flow, &(p->w_xy_flow)); + param_get(h->w_gps_flow, &(p->w_gps_flow)); param_get(h->w_acc_bias, &(p->w_acc_bias)); param_get(h->flow_k, &(p->flow_k)); + param_get(h->flow_q_min, &(p->flow_q_min)); param_get(h->sonar_filt, &(p->sonar_filt)); param_get(h->sonar_err, &(p->sonar_err)); param_get(h->land_t, &(p->land_t)); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h index 61570aea7a..e2be079d35 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -41,15 +41,18 @@ #include struct position_estimator_inav_params { - float w_alt_baro; - float w_alt_acc; - float w_alt_sonar; - float w_pos_gps_p; - float w_pos_gps_v; - float w_pos_acc; - float w_pos_flow; + float w_z_baro; + float w_z_gps_p; + float w_z_acc; + float w_z_sonar; + float w_xy_gps_p; + float w_xy_gps_v; + float w_xy_acc; + float w_xy_flow; + float w_gps_flow; float w_acc_bias; float flow_k; + float flow_q_min; float sonar_filt; float sonar_err; float land_t; @@ -58,15 +61,18 @@ struct position_estimator_inav_params { }; struct position_estimator_inav_param_handles { - param_t w_alt_baro; - param_t w_alt_acc; - param_t w_alt_sonar; - param_t w_pos_gps_p; - param_t w_pos_gps_v; - param_t w_pos_acc; - param_t w_pos_flow; + param_t w_z_baro; + param_t w_z_gps_p; + param_t w_z_acc; + param_t w_z_sonar; + param_t w_xy_gps_p; + param_t w_xy_gps_v; + param_t w_xy_acc; + param_t w_xy_flow; + param_t w_gps_flow; param_t w_acc_bias; param_t flow_k; + param_t flow_q_min; param_t sonar_filt; param_t sonar_err; param_t land_t; diff --git a/src/modules/px4iofirmware/adc.c b/src/modules/px4iofirmware/adc.c index 81566eb2a2..2f5908ac5e 100644 --- a/src/modules/px4iofirmware/adc.c +++ b/src/modules/px4iofirmware/adc.c @@ -83,6 +83,14 @@ adc_init(void) { adc_perf = perf_alloc(PC_ELAPSED, "adc"); + /* put the ADC into power-down mode */ + rCR2 &= ~ADC_CR2_ADON; + up_udelay(10); + + /* bring the ADC out of power-down mode */ + rCR2 |= ADC_CR2_ADON; + up_udelay(10); + /* do calibration if supported */ #ifdef ADC_CR2_CAL rCR2 |= ADC_CR2_RSTCAL; @@ -96,41 +104,25 @@ adc_init(void) if (rCR2 & ADC_CR2_CAL) return -1; - #endif - /* arbitrarily configure all channels for 55 cycle sample time */ - rSMPR1 = 0b00000011011011011011011011011011; + /* + * Configure sampling time. + * + * For electrical protection reasons, we want to be able to have + * 10K in series with ADC inputs that leave the board. At 12MHz this + * means we need 28.5 cycles of sampling time (per table 43 in the + * datasheet). + */ + rSMPR1 = 0b00000000011011011011011011011011; rSMPR2 = 0b00011011011011011011011011011011; - /* XXX for F2/4, might want to select 12-bit mode? */ - rCR1 = 0; - - /* enable the temperature sensor / Vrefint channel if supported*/ - rCR2 = -#ifdef ADC_CR2_TSVREFE - /* enable the temperature sensor in CR2 */ - ADC_CR2_TSVREFE | -#endif - 0; - -#ifdef ADC_CCR_TSVREFE - /* enable temperature sensor in CCR */ - rCCR = ADC_CCR_TSVREFE; -#endif + rCR2 |= ADC_CR2_TSVREFE; /* enable the temperature sensor / Vrefint channel */ /* configure for a single-channel sequence */ rSQR1 = 0; rSQR2 = 0; - rSQR3 = 0; /* will be updated with the channel each tick */ - - /* power-cycle the ADC and turn it on */ - rCR2 &= ~ADC_CR2_ADON; - up_udelay(10); - rCR2 |= ADC_CR2_ADON; - up_udelay(10); - rCR2 |= ADC_CR2_ADON; - up_udelay(10); + rSQR3 = 0; /* will be updated with the channel at conversion time */ return 0; } @@ -141,11 +133,12 @@ adc_init(void) uint16_t adc_measure(unsigned channel) { + perf_begin(adc_perf); /* clear any previous EOC */ - if (rSR & ADC_SR_EOC) - rSR &= ~ADC_SR_EOC; + rSR = 0; + (void)rDR; /* run a single conversion right now - should take about 60 cycles (a few microseconds) max */ rSQR3 = channel; @@ -158,7 +151,6 @@ adc_measure(unsigned channel) /* never spin forever - this will give a bogus result though */ if (hrt_elapsed_time(&now) > 100) { - debug("adc timeout"); perf_end(adc_perf); return 0xffff; } @@ -166,6 +158,7 @@ adc_measure(unsigned channel) /* read the result and clear EOC */ uint16_t result = rDR; + rSR = 0; perf_end(adc_perf); return result; diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 541eed0e1c..5e2c92bf46 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -114,9 +114,20 @@ controls_tick() { perf_begin(c_gather_sbus); bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &rssi, PX4IO_RC_INPUT_CHANNELS); + + bool sbus_status = (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS); + if (sbus_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS; } + + /* switch S.Bus output pin as needed */ + if (sbus_status != (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS)) { + #ifdef ENABLE_SBUS_OUT + ENABLE_SBUS_OUT((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS)); + #endif + } + perf_end(c_gather_sbus); /* diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index 4d306d6d00..60eda23192 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/src/modules/px4iofirmware/dsm.c @@ -203,6 +203,12 @@ dsm_guess_format(bool reset) int dsm_init(const char *device) { + +#ifdef CONFIG_ARCH_BOARD_PX4IO_V2 + // enable power on DSM connector + POWER_SPEKTRUM(true); +#endif + if (dsm_fd < 0) dsm_fd = open(device, O_RDONLY | O_NONBLOCK); diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index e55ef784a9..2e79f0ac65 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 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 @@ -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 volatile bool in_mixer = false; /* selected control values and count for mixing */ enum mixer_source { @@ -95,6 +96,7 @@ static void mixer_set_failsafe(); void 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) { @@ -199,13 +201,17 @@ mixer_tick(void) } - } else if (source != MIX_NONE) { + } else if (source != MIX_NONE && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { float outputs[PX4IO_SERVO_COUNT]; unsigned mixed; /* mix */ + + /* poor mans mutex */ + in_mixer = true; mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT); + in_mixer = false; pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); @@ -297,12 +303,17 @@ mixer_callback(uintptr_t handle, static char mixer_text[256]; /* large enough for one mixer */ static unsigned mixer_text_length = 0; -void +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)) { - return; + return 1; + } + + /* abort if we're in the mixer */ + if (in_mixer) { + return 1; } px4io_mixdata *msg = (px4io_mixdata *)buffer; @@ -310,7 +321,7 @@ mixer_handle_text(const void *buffer, size_t length) isr_debug(2, "mix txt %u", length); if (length < sizeof(px4io_mixdata)) - return; + return 0; unsigned text_length = length - sizeof(px4io_mixdata); @@ -328,13 +339,16 @@ 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; - return; + return 0; } - /* append mixer text and nul-terminate */ + /* append mixer text and nul-terminate, guard against overflow */ memcpy(&mixer_text[mixer_text_length], msg->text, text_length); mixer_text_length += text_length; mixer_text[mixer_text_length] = '\0'; @@ -369,6 +383,8 @@ mixer_handle_text(const void *buffer, size_t length) break; } + + return 0; } static void diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index 745bd5705f..d4c25911e3 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 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 @@ -125,6 +125,25 @@ heartbeat_blink(void) LED_BLUE(heartbeat = !heartbeat); } +static uint64_t reboot_time; + +/** + schedule a reboot in time_delta_usec microseconds + */ +void schedule_reboot(uint32_t time_delta_usec) +{ + reboot_time = hrt_absolute_time() + time_delta_usec; +} + +/** + check for a scheduled reboot + */ +static void check_reboot(void) +{ + if (reboot_time != 0 && hrt_absolute_time() > reboot_time) { + up_systemreset(); + } +} static void calculate_fw_crc(void) @@ -177,6 +196,11 @@ user_start(int argc, char *argv[]) POWER_SERVO(true); #endif + /* turn off S.Bus out (if supported) */ +#ifdef ENABLE_SBUS_OUT + ENABLE_SBUS_OUT(false); +#endif + /* start the safety switch handler */ safety_init(); @@ -186,6 +210,9 @@ user_start(int argc, char *argv[]) /* initialise the control inputs */ controls_init(); + /* set up the ADC */ + adc_init(); + /* start the FMU interface */ interface_init(); @@ -204,24 +231,41 @@ user_start(int argc, char *argv[]) /* initialize PWM limit lib */ pwm_limit_init(&pwm_limit); -#if 0 - /* not enough memory, lock down */ - if (minfo.mxordblk < 500) { + /* + * P O L I C E L I G H T S + * + * Not enough memory, lock down. + * + * We might need to allocate mixers later, and this will + * ensure that a developer doing a change will notice + * that he just burned the remaining RAM with static + * allocations. We don't want him to be able to + * get past that point. This needs to be clearly + * documented in the dev guide. + * + */ + if (minfo.mxordblk < 600) { + lowsyslog("ERR: not enough MEM"); bool phase = false; - if (phase) { - LED_AMBER(true); - LED_BLUE(false); - } else { - LED_AMBER(false); - LED_BLUE(true); - } + while (true) { - phase = !phase; - usleep(300000); + if (phase) { + LED_AMBER(true); + LED_BLUE(false); + } else { + LED_AMBER(false); + LED_BLUE(true); + } + up_udelay(250000); + + phase = !phase; + } } -#endif + + /* Start the failsafe led init */ + failsafe_led_init(); /* * Run everything in a tight loop. @@ -249,11 +293,14 @@ user_start(int argc, char *argv[]) heartbeat_blink(); } -#if 0 - /* check for debug activity */ + check_reboot(); + + /* check for debug activity (default: none) */ show_debug_messages(); - /* post debug state at ~1Hz */ + /* post debug state at ~1Hz - this is via an auxiliary serial port + * DEFAULTS TO OFF! + */ if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) { struct mallinfo minfo = mallinfo(); @@ -266,7 +313,6 @@ user_start(int argc, char *argv[]) (unsigned)minfo.mxordblk); last_debug_time = hrt_absolute_time(); } -#endif } } diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index dea04a663b..393e0560e8 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -160,6 +160,7 @@ extern pwm_limit_t pwm_limit; # define PX4IO_RELAY_CHANNELS 0 # define POWER_SPEKTRUM(_s) stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_s)) +# define ENABLE_SBUS_OUT(_s) stm32_gpiowrite(GPIO_SBUS_OENABLE, !(_s)) # define VDD_SERVO_FAULT (!stm32_gpioread(GPIO_SERVO_FAULT_DETECT)) @@ -177,12 +178,13 @@ extern pwm_limit_t pwm_limit; * Mixer */ extern void mixer_tick(void); -extern void mixer_handle_text(const void *buffer, size_t length); +extern int mixer_handle_text(const void *buffer, size_t length); /** * Safety switch/LED. */ extern void safety_init(void); +extern void failsafe_led_init(void); /** * FMU communications @@ -220,3 +222,7 @@ extern volatile uint8_t debug_level; /** send a debug message to the console */ extern void isr_debug(uint8_t level, const char *fmt, ...); + +/** schedule a reboot */ +extern void schedule_reboot(uint32_t time_delta_usec); + diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index bfc0337f67..2c437d2c0c 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -382,7 +382,10 @@ 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: - mixer_handle_text(values, num_values * sizeof(*values)); + 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)); + } break; default: @@ -509,8 +512,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_P_SETUP_REBOOT_BL: if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) || - (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) || - (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { + (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { // don't allow reboot while armed break; } @@ -518,16 +520,11 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) // check the magic value if (value != PX4IO_REBOOT_BL_MAGIC) break; - - // note that we don't set BL_WAIT_MAGIC in - // BKP_DR1 as that is not necessary given the - // timing of the forceupdate command. The - // bootloader on px4io waits for enough time - // anyway, and this method works with older - // bootloader versions (tested with both - // revision 3 and revision 4). - up_systemreset(); + // we schedule a reboot rather than rebooting + // immediately to allow the IO board to ACK + // the reboot command + schedule_reboot(100000); break; case PX4IO_P_SETUP_DSM: @@ -545,8 +542,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) * do not allow a RC config change while outputs armed */ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) || - (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) || - (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { + (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { break; } @@ -606,7 +602,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] == UINT8_MAX) { disabled = true; - } else if (REG_TO_SIGNED(conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]) < 0 || conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) { + } else if ((int)(conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]) < 0 || conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) { count++; } diff --git a/src/modules/px4iofirmware/safety.c b/src/modules/px4iofirmware/safety.c index 83bd3026eb..ff2e4af6ea 100644 --- a/src/modules/px4iofirmware/safety.c +++ b/src/modules/px4iofirmware/safety.c @@ -83,7 +83,11 @@ safety_init(void) { /* arrange for the button handler to be called at 10Hz */ hrt_call_every(&arming_call, 1000, 100000, safety_check_button, NULL); +} +void +failsafe_led_init(void) +{ /* arrange for the failsafe blinker to be called at 8Hz */ hrt_call_every(&failsafe_call, 1000, 125000, failsafe_blink, NULL); } @@ -164,8 +168,8 @@ failsafe_blink(void *arg) /* indicate that a serious initialisation error occured */ if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)) { LED_AMBER(true); - return; - } + return; + } static bool failsafe = false; diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 11ccd7356a..4954477402 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 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 @@ -218,11 +218,33 @@ static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_values) { /* check frame boundary markers to avoid out-of-sync cases */ - if ((frame[0] != 0x0f) || (frame[24] != 0x00)) { + if ((frame[0] != 0x0f)) { sbus_frame_drops++; return false; } + switch (frame[24]) { + case 0x00: + /* this is S.BUS 1 */ + break; + case 0x03: + /* S.BUS 2 SLOT0: RX battery and external voltage */ + break; + case 0x83: + /* S.BUS 2 SLOT1 */ + break; + case 0x43: + case 0xC3: + case 0x23: + case 0xA3: + case 0x63: + case 0xE3: + break; + default: + /* we expect one of the bits above, but there are some we don't know yet */ + break; + } + /* we have received something we think is a frame */ last_frame_time = frame_time; diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 6833ec43f4..9bac2958e9 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.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. * Author: Lorenz Meier * Anton Babushkin * @@ -62,6 +62,7 @@ #include #include +#include #include #include #include @@ -72,7 +73,7 @@ #include #include #include -#include +#include #include #include #include @@ -85,13 +86,13 @@ #include #include +#include #include #include "logbuffer.h" #include "sdlog2_format.h" #include "sdlog2_messages.h" -#include "sdlog2_version.h" #define LOGBUFFER_WRITE_AND_COUNT(_msg) if (logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(_msg))) { \ log_msgs_written++; \ @@ -108,13 +109,13 @@ static bool main_thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int deamon_task; /**< Handle of deamon task / thread */ static bool logwriter_should_exit = false; /**< Logwriter thread exit flag */ -static const int MAX_NO_LOGFOLDER = 999; /**< Maximum number of log folders */ +static const int MAX_NO_LOGFOLDER = 999; /**< Maximum number of log dirs */ static const int MAX_NO_LOGFILE = 999; /**< Maximum number of log files */ static const int LOG_BUFFER_SIZE_DEFAULT = 8192; static const int MAX_WRITE_CHUNK = 512; static const int MIN_BYTES_TO_WRITE = 512; -static const char *mountpoint = "/fs/microsd"; +static const char *log_root = "/fs/microsd/log"; static int mavlink_fd = -1; struct logbuffer_s lb; @@ -122,14 +123,17 @@ struct logbuffer_s lb; static pthread_mutex_t logbuffer_mutex; static pthread_cond_t logbuffer_cond; -static char folder_path[64]; +static char log_dir[32]; /* statistics counters */ -static unsigned long log_bytes_written = 0; static uint64_t start_time = 0; +static unsigned long log_bytes_written = 0; static unsigned long log_msgs_written = 0; static unsigned long log_msgs_skipped = 0; +/* GPS time, used for log files naming */ +static uint64_t gps_time = 0; + /* current state of logging */ static bool logging_enabled = false; /* enable logging on start (-e option) */ @@ -138,11 +142,14 @@ static bool log_on_start = false; static bool log_when_armed = false; /* delay = 1 / rate (rate defined by -r option) */ static useconds_t sleep_delay = 0; +/* use date/time for naming directories and files (-t option) */ +static bool log_name_timestamp = false; /* helper flag to track system state changes */ static bool flag_system_armed = false; static pthread_t logwriter_pthread = 0; +static pthread_attr_t logwriter_attr; /** * Log buffer writing thread. Open and close file here. @@ -203,14 +210,14 @@ static void handle_command(struct vehicle_command_s *cmd); static void handle_status(struct vehicle_status_s *cmd); /** - * Create folder for current logging session. Store folder name in 'log_folder'. + * Create dir for current logging session. Store dir name in 'log_dir'. */ -static int create_logfolder(void); +static int create_log_dir(void); /** * Select first free log file name and open it. */ -static int open_logfile(void); +static int open_log_file(void); static void sdlog2_usage(const char *reason) @@ -218,11 +225,12 @@ sdlog2_usage(const char *reason) if (reason) fprintf(stderr, "%s\n", reason); - errx(1, "usage: sdlog2 {start|stop|status} [-r ] [-b ] -e -a\n" + errx(1, "usage: sdlog2 {start|stop|status} [-r ] [-b ] -e -a -t\n" "\t-r\tLog rate in Hz, 0 means unlimited rate\n" "\t-b\tLog buffer size in KiB, default is 8\n" "\t-e\tEnable logging by default (if not, can be started by command)\n" - "\t-a\tLog only when armed (can be still overriden by command)\n"); + "\t-a\tLog only when armed (can be still overriden by command)\n" + "\t-t\tUse date/time for naming log directories and files\n"); } /** @@ -280,82 +288,112 @@ int sdlog2_main(int argc, char *argv[]) exit(1); } -int create_logfolder() +int create_log_dir() { - /* make folder on sdcard */ - uint16_t folder_number = 1; // start with folder sess001 + /* create dir on sdcard if needed */ + uint16_t dir_number = 1; // start with dir sess001 int mkdir_ret; - /* look for the next folder that does not exist */ - while (folder_number <= MAX_NO_LOGFOLDER) { - /* set up folder path: e.g. /fs/microsd/sess001 */ - sprintf(folder_path, "%s/sess%03u", mountpoint, folder_number); - mkdir_ret = mkdir(folder_path, S_IRWXU | S_IRWXG | S_IRWXO); - /* the result is -1 if the folder exists */ + if (log_name_timestamp && gps_time != 0) { + /* use GPS date for log dir naming: e.g. /fs/microsd/2014-01-19 */ + time_t gps_time_sec = gps_time / 1000000; + struct tm t; + gmtime_r(&gps_time_sec, &t); + int n = snprintf(log_dir, sizeof(log_dir), "%s/", log_root); + strftime(log_dir + n, sizeof(log_dir) - n, "%Y-%m-%d", &t); + mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO); - if (mkdir_ret == 0) { - /* folder does not exist, success */ - break; + if (mkdir_ret == OK) { + warnx("log dir created: %s", log_dir); - } else if (mkdir_ret == -1) { - /* folder exists already */ - folder_number++; + } else if (errno != EEXIST) { + warn("failed creating new dir: %s", log_dir); + return -1; + } + + } else { + /* look for the next dir that does not exist */ + while (dir_number <= MAX_NO_LOGFOLDER) { + /* format log dir: e.g. /fs/microsd/sess001 */ + sprintf(log_dir, "%s/sess%03u", log_root, dir_number); + mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO); + + if (mkdir_ret == 0) { + warnx("log dir created: %s", log_dir); + break; + + } else if (errno != EEXIST) { + warn("failed creating new dir: %s", log_dir); + return -1; + } + + /* dir exists already */ + dir_number++; continue; + } - } else { - warn("failed creating new folder"); + if (dir_number >= MAX_NO_LOGFOLDER) { + /* we should not end up here, either we have more than MAX_NO_LOGFOLDER on the SD card, or another problem */ + warnx("all %d possible dirs exist already", MAX_NO_LOGFOLDER); return -1; } } - if (folder_number >= MAX_NO_LOGFOLDER) { - /* we should not end up here, either we have more than MAX_NO_LOGFOLDER on the SD card, or another problem */ - warnx("all %d possible folders exist already.", MAX_NO_LOGFOLDER); - return -1; - } - + /* print logging path, important to find log file later */ + warnx("log dir: %s", log_dir); + mavlink_log_info(mavlink_fd, "[sdlog2] log dir: %s", log_dir); return 0; } -int open_logfile() +int open_log_file() { - /* make folder on sdcard */ - uint16_t file_number = 1; // start with file log001 - /* string to hold the path to the log */ - char path_buf[64] = ""; + char log_file_name[16] = ""; + char log_file_path[48] = ""; - int fd = 0; + if (log_name_timestamp && gps_time != 0) { + /* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ + time_t gps_time_sec = gps_time / 1000000; + struct tm t; + gmtime_r(&gps_time_sec, &t); + strftime(log_file_name, sizeof(log_file_name), "%H_%M_%S.bin", &t); + snprintf(log_file_path, sizeof(log_file_path), "%s/%s", log_dir, log_file_name); - /* look for the next file that does not exist */ - while (file_number <= MAX_NO_LOGFILE) { - /* set up file path: e.g. /fs/microsd/sess001/log001.bin */ - sprintf(path_buf, "%s/log%03u.bin", folder_path, file_number); + } else { + uint16_t file_number = 1; // start with file log001 + + /* look for the next file that does not exist */ + while (file_number <= MAX_NO_LOGFILE) { + /* format log file path: e.g. /fs/microsd/sess001/log001.bin */ + snprintf(log_file_name, sizeof(log_file_name), "log%03u.bin", file_number); + snprintf(log_file_path, sizeof(log_file_path), "%s/%s", log_dir, log_file_name); + + if (!file_exist(log_file_path)) { + break; + } - if (file_exist(path_buf)) { file_number++; - continue; } - fd = open(path_buf, O_CREAT | O_WRONLY | O_DSYNC); - - if (fd == 0) { - warn("opening %s failed", path_buf); + if (file_number > MAX_NO_LOGFILE) { + /* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */ + warnx("all %d possible files exist already", MAX_NO_LOGFILE); + return -1; } - - warnx("logging to: %s.", path_buf); - mavlink_log_info(mavlink_fd, "[sdlog2] log: %s", path_buf); - - return fd; } - if (file_number > MAX_NO_LOGFILE) { - /* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */ - warnx("all %d possible files exist already.", MAX_NO_LOGFILE); - return -1; + int fd = open(log_file_path, O_CREAT | O_WRONLY | O_DSYNC); + + if (fd < 0) { + warn("failed opening log: %s", log_file_name); + mavlink_log_info(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name); + + } else { + warnx("log file: %s", log_file_name); + mavlink_log_info(mavlink_fd, "[sdlog2] log file: %s", log_file_name); } - return 0; + return fd; } static void *logwriter_thread(void *arg) @@ -363,9 +401,12 @@ static void *logwriter_thread(void *arg) /* set name */ prctl(PR_SET_NAME, "sdlog2_writer", 0); - struct logbuffer_s *logbuf = (struct logbuffer_s *)arg; + int log_fd = open_log_file(); - int log_fd = open_logfile(); + if (log_fd < 0) + return; + + struct logbuffer_s *logbuf = (struct logbuffer_s *)arg; /* write log messages formats, version and parameters */ log_bytes_written += write_formats(log_fd); @@ -443,14 +484,20 @@ static void *logwriter_thread(void *arg) fsync(log_fd); close(log_fd); - return OK; + return; } void sdlog2_start_log() { - warnx("start logging."); + warnx("start logging"); mavlink_log_info(mavlink_fd, "[sdlog2] start logging"); + /* create log dir if needed */ + if (create_log_dir() != 0) { + mavlink_log_critical(mavlink_fd, "[sdlog2] error creating log dir"); + errx(1, "error creating log dir"); + } + /* initialize statistics counter */ log_bytes_written = 0; start_time = hrt_absolute_time(); @@ -458,30 +505,28 @@ void sdlog2_start_log() log_msgs_skipped = 0; /* initialize log buffer emptying thread */ - pthread_attr_t receiveloop_attr; - pthread_attr_init(&receiveloop_attr); + pthread_attr_init(&logwriter_attr); struct sched_param param; /* low priority, as this is expensive disk I/O */ param.sched_priority = SCHED_PRIORITY_DEFAULT - 40; - (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); + (void)pthread_attr_setschedparam(&logwriter_attr, ¶m); - pthread_attr_setstacksize(&receiveloop_attr, 2048); + pthread_attr_setstacksize(&logwriter_attr, 2048); logwriter_should_exit = false; /* start log buffer emptying thread */ - if (0 != pthread_create(&logwriter_pthread, &receiveloop_attr, logwriter_thread, &lb)) { + if (0 != pthread_create(&logwriter_pthread, &logwriter_attr, logwriter_thread, &lb)) { errx(1, "error creating logwriter thread"); } logging_enabled = true; - // XXX we have to destroy the attr at some point } void sdlog2_stop_log() { - warnx("stop logging."); + warnx("stop logging"); mavlink_log_info(mavlink_fd, "[sdlog2] stop logging"); logging_enabled = false; @@ -501,6 +546,7 @@ void sdlog2_stop_log() } logwriter_pthread = 0; + pthread_attr_destroy(&logwriter_attr); sdlog2_status(); } @@ -569,8 +615,8 @@ int write_parameters(int fd) } case PARAM_TYPE_FLOAT: - param_get(param, &value); - break; + param_get(param, &value); + break; default: break; @@ -588,18 +634,25 @@ int sdlog2_thread_main(int argc, char *argv[]) mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); if (mavlink_fd < 0) { - warnx("failed to open MAVLink log stream, start mavlink app first."); + warnx("failed to open MAVLink log stream, start mavlink app first"); } /* log buffer size */ int log_buffer_size = LOG_BUFFER_SIZE_DEFAULT; + logging_enabled = false; + log_on_start = false; + log_when_armed = false; + log_name_timestamp = false; + + flag_system_armed = false; + /* work around some stupidity in task_create's argv handling */ argc -= 2; argv += 2; int ch; - while ((ch = getopt(argc, argv, "r:b:ea")) != EOF) { + while ((ch = getopt(argc, argv, "r:b:eat")) != EOF) { switch (ch) { case 'r': { unsigned long r = strtoul(optarg, NULL, 10); @@ -632,49 +685,52 @@ int sdlog2_thread_main(int argc, char *argv[]) log_when_armed = true; break; + case 't': + log_name_timestamp = true; + break; + case '?': if (optopt == 'c') { - warnx("Option -%c requires an argument.", optopt); + warnx("option -%c requires an argument", optopt); } else if (isprint(optopt)) { - warnx("Unknown option `-%c'.", optopt); + warnx("unknown option `-%c'", optopt); } else { - warnx("Unknown option character `\\x%x'.", optopt); + warnx("unknown option character `\\x%x'", optopt); } default: sdlog2_usage("unrecognized flag"); - errx(1, "exiting."); + errx(1, "exiting"); } } - if (!file_exist(mountpoint)) { - errx(1, "logging mount point %s not present, exiting.", mountpoint); - } - - if (create_logfolder()) { - errx(1, "unable to create logging folder, exiting."); + gps_time = 0; + + /* create log root dir */ + int mkdir_ret = mkdir(log_root, S_IRWXU | S_IRWXG | S_IRWXO); + + if (mkdir_ret != 0 && errno != EEXIST) { + err("failed creating log root dir: %s", log_root); } + /* copy conversion scripts */ const char *converter_in = "/etc/logging/conv.zip"; - char *converter_out = malloc(120); - sprintf(converter_out, "%s/conv.zip", folder_path); + char *converter_out = malloc(64); + snprintf(converter_out, 64, "%s/conv.zip", log_root); - if (file_copy(converter_in, converter_out)) { - errx(1, "unable to copy conversion scripts, exiting."); + if (file_copy(converter_in, converter_out) != OK) { + warn("unable to copy conversion scripts"); } free(converter_out); - /* only print logging path, important to find log file later */ - warnx("logging to directory: %s", folder_path); - /* initialize log buffer with specified size */ - warnx("log buffer size: %i bytes.", log_buffer_size); + warnx("log buffer size: %i bytes", log_buffer_size); if (OK != logbuffer_init(&lb, log_buffer_size)) { - errx(1, "can't allocate log buffer, exiting."); + errx(1, "can't allocate log buffer, exiting"); } struct vehicle_status_s buf_status; @@ -684,6 +740,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* warning! using union here to save memory, elements should be used separately! */ union { struct vehicle_command_s cmd; + struct vehicle_control_mode_s control_mode; struct sensor_combined_s sensor; struct vehicle_attitude_s att; struct vehicle_attitude_setpoint_s att_sp; @@ -693,7 +750,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_local_position_s local_pos; struct vehicle_local_position_setpoint_s local_pos_sp; struct vehicle_global_position_s global_pos; - struct mission_item_triplet_s triplet; + struct position_setpoint_triplet_s triplet; struct vehicle_gps_position_s gps_pos; struct vehicle_vicon_position_s vicon_pos; struct optical_flow_s flow; @@ -702,6 +759,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct airspeed_s airspeed; struct esc_status_s esc; struct vehicle_global_velocity_setpoint_s global_vel_sp; + struct battery_status_s battery; } buf; memset(&buf, 0, sizeof(buf)); @@ -709,6 +767,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct { int cmd_sub; int status_sub; + int control_mode_sub; int sensor_sub; int att_sub; int att_sp_sub; @@ -726,6 +785,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int airspeed_sub; int esc_sub; int global_vel_sp_sub; + int battery_sub; } subs; /* log message buffer: header + body */ @@ -752,6 +812,8 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_GPSP_s log_GPSP; struct log_ESC_s log_ESC; struct log_GVSP_s log_GVSP; + struct log_BATT_s log_BATT; + struct log_DIST_s log_DIST; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -760,9 +822,9 @@ int sdlog2_thread_main(int argc, char *argv[]) memset(&log_msg.body, 0, sizeof(log_msg.body)); /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ - /* number of subscriptions */ - const ssize_t fdsc = 19; - /* sanity check variable and index */ + /* number of messages */ + const ssize_t fdsc = 25; + /* Sanity check variable and index */ ssize_t fdsc_count = 0; /* file descriptors to wait for */ struct pollfd fds[fdsc]; @@ -785,6 +847,12 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; + /* --- VEHICLE CONTROL MODE --- */ + subs.control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + fds[fdsc_count].fd = subs.control_mode_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + /* --- SENSORS COMBINED --- */ subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); fds[fdsc_count].fd = subs.sensor_sub; @@ -840,7 +908,7 @@ int sdlog2_thread_main(int argc, char *argv[]) fdsc_count++; /* --- GLOBAL POSITION SETPOINT--- */ - subs.triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet)); + subs.triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); fds[fdsc_count].fd = subs.triplet_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; @@ -881,12 +949,18 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; + /* --- BATTERY --- */ + subs.battery_sub = orb_subscribe(ORB_ID(battery_status)); + fds[fdsc_count].fd = subs.battery_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + /* WARNING: If you get the error message below, * then the number of registered messages (fdsc) * differs from the number of messages in the above list. */ if (fdsc_count > fdsc) { - warn("WARNING: Not enough space for poll fds allocated. Check %s:%d.", __FILE__, __LINE__); + warn("WARNING: Not enough space for poll fds allocated. Check %s:%d", __FILE__, __LINE__); fdsc_count = fdsc; } @@ -909,20 +983,31 @@ int sdlog2_thread_main(int argc, char *argv[]) uint16_t baro_counter = 0; uint16_t differential_pressure_counter = 0; + /* track changes in distance status */ + bool dist_bottom_present = false; + /* enable logging on start if needed */ - if (log_on_start) + if (log_on_start) { + /* check GPS topic to get GPS time */ + if (log_name_timestamp) { + if (OK == orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos)) { + gps_time = buf.gps_pos.time_gps_usec; + } + } + sdlog2_start_log(); + } while (!main_thread_should_exit) { /* decide use usleep() or blocking poll() */ bool use_sleep = sleep_delay > 0 && logging_enabled; /* poll all topics if logging enabled or only management (first 2) if not */ - int poll_ret = poll(fds, logging_enabled ? fdsc_count : 2, use_sleep ? 0 : poll_timeout); + int poll_ret = poll(fds, logging_enabled ? fdsc_count : 3, use_sleep ? 0 : poll_timeout); /* handle the poll result */ if (poll_ret < 0) { - warnx("ERROR: poll error, stop logging."); + warnx("ERROR: poll error, stop logging"); main_thread_should_exit = true; } else if (poll_ret > 0) { @@ -936,6 +1021,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- VEHICLE COMMAND - LOG MANAGEMENT --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd); + handle_command(&buf.cmd); handled_topics++; } @@ -951,11 +1037,22 @@ int sdlog2_thread_main(int argc, char *argv[]) handled_topics++; } + /* --- GPS POSITION - LOG MANAGEMENT --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); + + if (log_name_timestamp) { + gps_time = buf.gps_pos.time_gps_usec; + } + + handled_topics++; + } + if (!logging_enabled || !check_data || handled_topics >= poll_ret) { continue; } - ifds = 1; // begin from fds[1] again + ifds = 1; // begin from VEHICLE STATUS again pthread_mutex_lock(&logbuffer_mutex); @@ -966,14 +1063,13 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- VEHICLE STATUS --- */ if (fds[ifds++].revents & POLLIN) { - // Don't orb_copy, it's already done few lines above + /* don't orb_copy, it's already done few lines above */ + /* copy VEHICLE CONTROL MODE control mode here to construct STAT message */ + orb_copy(ORB_ID(vehicle_control_mode), subs.control_mode_sub, &buf.control_mode); log_msg.msg_type = LOG_STAT_MSG; - log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state; - // TODO use control_mode topic - //log_msg.body.log_STAT.navigation_state = (uint8_t) buf_status.navigation_state; + log_msg.body.log_STAT.main_state = (uint8_t) buf.control_mode.main_state; + log_msg.body.log_STAT.navigation_state = (uint8_t) buf.control_mode.nav_state; log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state; - log_msg.body.log_STAT.battery_voltage = buf_status.battery_voltage; - log_msg.body.log_STAT.battery_current = buf_status.battery_current; log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning; log_msg.body.log_STAT.landed = (uint8_t) buf_status.condition_landed; @@ -998,6 +1094,8 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(GPS); } + ifds++; // skip CONTROL MODE, already handled + /* --- SENSOR COMBINED --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor); @@ -1063,6 +1161,9 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_ATT.roll_rate = buf.att.rollspeed; log_msg.body.log_ATT.pitch_rate = buf.att.pitchspeed; log_msg.body.log_ATT.yaw_rate = buf.att.yawspeed; + log_msg.body.log_ATT.gx = buf.att.g_comp[0]; + log_msg.body.log_ATT.gy = buf.att.g_comp[1]; + log_msg.body.log_ATT.gz = buf.att.g_comp[2]; LOGBUFFER_WRITE_AND_COUNT(ATT); } @@ -1123,6 +1224,17 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_LPOS.z_flags = (buf.local_pos.z_valid ? 1 : 0) | (buf.local_pos.v_z_valid ? 2 : 0) | (buf.local_pos.z_global ? 8 : 0); log_msg.body.log_LPOS.landed = buf.local_pos.landed; LOGBUFFER_WRITE_AND_COUNT(LPOS); + + if (buf.local_pos.dist_bottom_valid) { + dist_bottom_present = true; + } + if (dist_bottom_present) { + log_msg.msg_type = LOG_DIST_MSG; + log_msg.body.log_DIST.bottom = buf.local_pos.dist_bottom; + log_msg.body.log_DIST.bottom_rate = buf.local_pos.dist_bottom_rate; + log_msg.body.log_DIST.flags = (buf.local_pos.dist_bottom_valid ? 1 : 0); + LOGBUFFER_WRITE_AND_COUNT(DIST); + } } /* --- LOCAL POSITION SETPOINT --- */ @@ -1140,29 +1252,26 @@ int sdlog2_thread_main(int argc, char *argv[]) if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos); log_msg.msg_type = LOG_GPOS_MSG; - log_msg.body.log_GPOS.lat = buf.global_pos.lat; - log_msg.body.log_GPOS.lon = buf.global_pos.lon; + log_msg.body.log_GPOS.lat = buf.global_pos.lat * 1e7; + log_msg.body.log_GPOS.lon = buf.global_pos.lon * 1e7; log_msg.body.log_GPOS.alt = buf.global_pos.alt; - log_msg.body.log_GPOS.vel_n = buf.global_pos.vx; - log_msg.body.log_GPOS.vel_e = buf.global_pos.vy; - log_msg.body.log_GPOS.vel_d = buf.global_pos.vz; + log_msg.body.log_GPOS.vel_n = buf.global_pos.vel_n; + log_msg.body.log_GPOS.vel_e = buf.global_pos.vel_e; + log_msg.body.log_GPOS.vel_d = buf.global_pos.vel_d; LOGBUFFER_WRITE_AND_COUNT(GPOS); } /* --- GLOBAL POSITION SETPOINT --- */ if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(mission_item_triplet), subs.triplet_sub, &buf.triplet); + orb_copy(ORB_ID(position_setpoint_triplet), subs.triplet_sub, &buf.triplet); log_msg.msg_type = LOG_GPSP_MSG; - log_msg.body.log_GPSP.altitude_is_relative = buf.triplet.current.altitude_is_relative; log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7d); log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7d); - log_msg.body.log_GPSP.altitude = buf.triplet.current.altitude; + log_msg.body.log_GPSP.alt = buf.triplet.current.alt; log_msg.body.log_GPSP.yaw = buf.triplet.current.yaw; - log_msg.body.log_GPSP.nav_cmd = buf.triplet.current.nav_cmd; + log_msg.body.log_GPSP.type = buf.triplet.current.type; log_msg.body.log_GPSP.loiter_radius = buf.triplet.current.loiter_radius; log_msg.body.log_GPSP.loiter_direction = buf.triplet.current.loiter_direction; - log_msg.body.log_GPSP.acceptance_radius = buf.triplet.current.acceptance_radius; - log_msg.body.log_GPSP.time_inside = buf.triplet.current.time_inside; log_msg.body.log_GPSP.pitch_min = buf.triplet.current.pitch_min; LOGBUFFER_WRITE_AND_COUNT(GPSP); } @@ -1238,6 +1347,17 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(GVSP); } + /* --- BATTERY --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(battery_status), subs.battery_sub, &buf.battery); + log_msg.msg_type = LOG_BATT_MSG; + log_msg.body.log_BATT.voltage = buf.battery.voltage_v; + log_msg.body.log_BATT.voltage_filtered = buf.battery.voltage_filtered_v; + log_msg.body.log_BATT.current = buf.battery.current_a; + log_msg.body.log_BATT.discharged = buf.battery.discharged_mah; + LOGBUFFER_WRITE_AND_COUNT(BATT); + } + /* 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 */ @@ -1261,7 +1381,7 @@ int sdlog2_thread_main(int argc, char *argv[]) free(lb.data); - warnx("exiting."); + warnx("exiting"); thread_running = false; @@ -1274,8 +1394,8 @@ void sdlog2_status() float mebibytes = kibibytes / 1024.0f; float seconds = ((float)(hrt_absolute_time() - start_time)) / 1000000.0f; - warnx("wrote %lu msgs, %4.2f MiB (average %5.3f KiB/s), skipped %lu msgs.", log_msgs_written, (double)mebibytes, (double)(kibibytes / seconds), log_msgs_skipped); - mavlink_log_info(mavlink_fd, "[sdlog2] wrote %lu msgs, skipped %lu msgs.", log_msgs_written, log_msgs_skipped); + warnx("wrote %lu msgs, %4.2f MiB (average %5.3f KiB/s), skipped %lu msgs", log_msgs_written, (double)mebibytes, (double)(kibibytes / seconds), log_msgs_skipped); + mavlink_log_info(mavlink_fd, "[sdlog2] wrote %lu msgs, skipped %lu msgs", log_msgs_written, log_msgs_skipped); } /** @@ -1294,7 +1414,7 @@ int file_copy(const char *file_old, const char *file_new) int ret = 0; if (source == NULL) { - warnx("failed opening input file to copy."); + warnx("failed opening input file to copy"); return 1; } @@ -1302,7 +1422,7 @@ int file_copy(const char *file_old, const char *file_new) if (target == NULL) { fclose(source); - warnx("failed to open output file to copy."); + warnx("failed to open output file to copy"); return 1; } @@ -1313,7 +1433,7 @@ int file_copy(const char *file_old, const char *file_new) ret = fwrite(buf, 1, nread, target); if (ret <= 0) { - warnx("error writing file."); + warnx("error writing file"); ret = 1; break; } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 3afaaa2ada..98736dd211 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -57,6 +57,9 @@ struct log_ATT_s { float roll_rate; float pitch_rate; float yaw_rate; + float gx; + float gy; + float gz; }; /* --- ATSP - ATTITUDE SET POINT --- */ @@ -148,8 +151,6 @@ struct log_STAT_s { uint8_t main_state; uint8_t navigation_state; uint8_t arming_state; - float battery_voltage; - float battery_current; float battery_remaining; uint8_t battery_warning; uint8_t landed; @@ -209,16 +210,13 @@ struct log_GPOS_s { /* --- GPSP - GLOBAL POSITION SETPOINT --- */ #define LOG_GPSP_MSG 17 struct log_GPSP_s { - uint8_t altitude_is_relative; int32_t lat; int32_t lon; - float altitude; + float alt; float yaw; - uint8_t nav_cmd; + uint8_t type; float loiter_radius; int8_t loiter_direction; - float acceptance_radius; - float time_inside; float pitch_min; }; @@ -247,6 +245,25 @@ struct log_GVSP_s { float vz; }; +/* --- BATT - BATTERY --- */ +#define LOG_BATT_MSG 20 +struct log_BATT_s { + float voltage; + float voltage_filtered; + float current; + float discharged; +}; + +/* --- DIST - DISTANCE TO SURFACE --- */ +#define LOG_DIST_MSG 21 +struct log_DIST_s { + float bottom; + float bottom_rate; + uint8_t flags; +}; + +/********** SYSTEM MESSAGES, ID > 0x80 **********/ + /* --- TIME - TIME STAMP --- */ #define LOG_TIME_MSG 129 struct log_TIME_s { @@ -272,7 +289,7 @@ struct log_PARM_s { /* construct list of all message formats */ static const struct log_format_s log_formats[] = { /* business-level messages, ID < 0x80 */ - LOG_FORMAT(ATT, "ffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate"), + LOG_FORMAT(ATT, "fffffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"), LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), LOG_FORMAT(SENS, "ffff", "BaroPres,BaroAlt,BaroTemp,DiffPres"), @@ -280,16 +297,18 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), - LOG_FORMAT(STAT, "BBBfffBB", "MainState,NavState,ArmState,BatV,BatC,BatRem,BatWarn,Landed"), + LOG_FORMAT(STAT, "BBBfBB", "MainState,NavState,ArmState,BatRem,BatWarn,Landed"), LOG_FORMAT(RC, "ffffffffB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"), LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"), LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"), - LOG_FORMAT(GPSP, "BLLffBfbfff", "AltRel,Lat,Lon,Alt,Yaw,NavCmd,LoitR,LoitDir,AccR,TimeIn,PitMin"), + LOG_FORMAT(GPSP, "LLffBfbf", "Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"), LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), + LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"), + LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"), /* system-level messages, ID >= 0x80 */ // FMT: don't write format of format message, it's useless LOG_FORMAT(TIME, "Q", "StartTime"), diff --git a/src/modules/sensors/module.mk b/src/modules/sensors/module.mk index ebbc580e10..aa538fd6b1 100644 --- a/src/modules/sensors/module.mk +++ b/src/modules/sensors/module.mk @@ -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 diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 7637235548..30659fd3a0 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -1,9 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * @author Thomas Gubler - * @author Julian Oes + * 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 @@ -38,6 +35,10 @@ * @file sensor_params.c * * Parameters defined by the sensors task. + * + * @author Lorenz Meier + * @author Julian Oes + * @author Thomas Gubler */ #include @@ -45,41 +46,98 @@ #include /** - * Gyro X offset FIXME + * Gyro X offset * - * This is an X-axis offset for the gyro. - * Adjust it according to the calibration data. + * This is an X-axis offset for the gyro. Adjust it according to the calibration data. * * @min -10.0 * @max 10.0 - * @group Gyro Config + * @group Sensor Calibration */ PARAM_DEFINE_FLOAT(SENS_GYRO_XOFF, 0.0f); /** - * Gyro Y offset FIXME with dot. + * Gyro Y offset * * @min -10.0 * @max 10.0 - * @group Gyro Config + * @group Sensor Calibration */ PARAM_DEFINE_FLOAT(SENS_GYRO_YOFF, 0.0f); /** - * Gyro Z offset FIXME + * Gyro Z offset * * @min -5.0 * @max 5.0 - * @group Gyro Config + * @group Sensor Calibration */ PARAM_DEFINE_FLOAT(SENS_GYRO_ZOFF, 0.0f); +/** + * Gyro X scaling + * + * X-axis scaling. + * + * @min -1.5 + * @max 1.5 + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_GYRO_XSCALE, 1.0f); + +/** + * Gyro Y scaling + * + * Y-axis scaling. + * + * @min -1.5 + * @max 1.5 + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_GYRO_YSCALE, 1.0f); + +/** + * Gyro Z scaling + * + * Z-axis scaling. + * + * @min -1.5 + * @max 1.5 + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_GYRO_ZSCALE, 1.0f); +/** + * Magnetometer X offset + * + * This is an X-axis offset for the magnetometer. + * + * @min -500.0 + * @max 500.0 + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_MAG_XOFF, 0.0f); + +/** + * Magnetometer Y offset + * + * This is an Y-axis offset for the magnetometer. + * + * @min -500.0 + * @max 500.0 + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_MAG_YOFF, 0.0f); + +/** + * Magnetometer Z offset + * + * This is an Z-axis offset for the magnetometer. + * + * @min -500.0 + * @max 500.0 + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_MAG_ZOFF, 0.0f); PARAM_DEFINE_FLOAT(SENS_MAG_XSCALE, 1.0f); @@ -100,16 +158,114 @@ PARAM_DEFINE_INT32(SENS_DPRES_ANA, 0); PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0); PARAM_DEFINE_INT32(SENS_EXT_MAG_ROT, 0); +/** + * RC Channel 1 Minimum + * + * Minimum value for RC channel 1 + * + * @min 800.0 + * @max 1500.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f); + +/** + * RC Channel 1 Trim + * + * Mid point value (same as min for throttle) + * + * @min 800.0 + * @max 2200.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f); + +/** + * RC Channel 1 Maximum + * + * Maximum value for RC channel 1 + * + * @min 1500.0 + * @max 2200.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f); + +/** + * RC Channel 1 Reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC1_REV, 1.0f); + +/** + * RC Channel 1 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC1_DZ, 10.0f); -PARAM_DEFINE_FLOAT(RC2_MIN, 1000); -PARAM_DEFINE_FLOAT(RC2_TRIM, 1500); -PARAM_DEFINE_FLOAT(RC2_MAX, 2000); +/** + * RC Channel 2 Minimum + * + * Minimum value for RC channel 2 + * + * @min 800.0 + * @max 1500.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC2_MIN, 1000.0f); + +/** + * RC Channel 2 Trim + * + * Mid point value (same as min for throttle) + * + * @min 800.0 + * @max 2200.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC2_TRIM, 1500.0f); + +/** + * RC Channel 2 Maximum + * + * Maximum value for RC channel 2 + * + * @min 1500.0 + * @max 2200.0 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC2_MAX, 2000.0f); + +/** + * RC Channel 2 Reverse + * + * Set to -1 to reverse channel. + * + * @min -1.0 + * @max 1.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC2_REV, 1.0f); + +/** + * RC Channel 2 dead zone + * + * The +- range of this value around the trim value will be considered as zero. + * + * @min 0.0 + * @max 100.0 + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC2_DZ, 10.0f); PARAM_DEFINE_FLOAT(RC3_MIN, 1000); @@ -223,15 +379,75 @@ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f); /* FMU with PX4IOAR: (3.3f * 52.0f / 5.0f / 4095.0f) */ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.00459340659f); #endif +PARAM_DEFINE_FLOAT(BAT_C_SCALING, 0.0124); /* scaling for 3DR power brick */ +/** + * Roll control channel mapping. + * + * The channel index (starting from 1 for channel 1) indicates + * which channel should be used for reading roll inputs from. + * A value of zero indicates the switch is not assigned. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ PARAM_DEFINE_INT32(RC_MAP_ROLL, 1); + +/** + * Pitch control channel mapping. + * + * The channel index (starting from 1 for channel 1) indicates + * which channel should be used for reading pitch inputs from. + * A value of zero indicates the switch is not assigned. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ PARAM_DEFINE_INT32(RC_MAP_PITCH, 2); + +/** + * Throttle control channel mapping. + * + * The channel index (starting from 1 for channel 1) indicates + * which channel should be used for reading throttle inputs from. + * A value of zero indicates the switch is not assigned. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3); + +/** + * Yaw control channel mapping. + * + * The channel index (starting from 1 for channel 1) indicates + * which channel should be used for reading yaw inputs from. + * A value of zero indicates the switch is not assigned. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ PARAM_DEFINE_INT32(RC_MAP_YAW, 4); -PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5); +/** + * Mode switch channel mapping. + * + * This is the main flight mode selector. + * The channel index (starting from 1 for channel 1) indicates + * which channel should be used for deciding about the main mode. + * A value of zero indicates the switch is not assigned. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ +PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0); PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); -PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 6); +PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0); PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0); //PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 9baf1a6aff..ea864390da 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -114,6 +114,7 @@ #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 #define ADC_BATTERY_VOLTAGE_CHANNEL 10 +#define ADC_BATTERY_CURRENT_CHANNEL -1 #define ADC_AIRSPEED_VOLTAGE_CHANNEL 11 #endif @@ -124,10 +125,8 @@ #define ADC_AIRSPEED_VOLTAGE_CHANNEL 15 #endif -#define BAT_VOL_INITIAL 0.f -#define BAT_VOL_LOWPASS_1 0.99f -#define BAT_VOL_LOWPASS_2 0.01f -#define VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS 3.5f +#define BATT_V_LOWPASS 0.001f +#define BATT_V_IGNORE_THRESHOLD 3.5f /** * HACK - true temperature is much less than indicated temperature in baro, @@ -211,10 +210,13 @@ private: struct differential_pressure_s _diff_pres; struct airspeed_s _airspeed; - math::Matrix _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ - math::Matrix _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */ + math::Matrix<3,3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ + math::Matrix<3,3> _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */ bool _mag_is_external; /**< true if the active mag is on an external board */ + uint64_t _battery_discharged; /**< battery discharged current in mA*ms */ + hrt_abstime _battery_current_timestamp; /**< timestamp of last battery current reading */ + struct { float min[_rc_max_chan_count]; float trim[_rc_max_chan_count]; @@ -265,6 +267,7 @@ private: float rc_fs_thr; float battery_voltage_scaling; + float battery_current_scaling; } _parameters; /**< local copies of interesting parameters */ @@ -314,6 +317,7 @@ private: param_t rc_fs_thr; param_t battery_voltage_scaling; + param_t battery_current_scaling; param_t board_rotation; param_t external_mag_rotation; @@ -465,9 +469,9 @@ Sensors::Sensors() : /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")), - _board_rotation(3, 3), - _external_mag_rotation(3, 3), - _mag_is_external(false) + _mag_is_external(false), + _battery_discharged(0), + _battery_current_timestamp(0) { /* basic r/c parameters */ @@ -560,6 +564,7 @@ Sensors::Sensors() : _parameter_handles.diff_pres_analog_enabled = param_find("SENS_DPRES_ANA"); _parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING"); + _parameter_handles.battery_current_scaling = param_find("BAT_C_SCALING"); /* rotations */ _parameter_handles.board_rotation = param_find("SENS_BOARD_ROT"); @@ -740,6 +745,11 @@ Sensors::parameters_update() warnx("Failed updating voltage scaling param"); } + /* scaling of ADC ticks to battery current */ + if (param_get(_parameter_handles.battery_current_scaling, &(_parameters.battery_current_scaling)) != OK) { + warnx("Failed updating current scaling param"); + } + param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation)); param_get(_parameter_handles.external_mag_rotation, &(_parameters.external_mag_rotation)); @@ -785,7 +795,6 @@ Sensors::accel_init() #endif - warnx("using system accel"); close(fd); } } @@ -825,7 +834,6 @@ Sensors::gyro_init() #endif - warnx("using system gyro"); close(fd); } } @@ -920,7 +928,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report); - math::Vector3 vect = {accel_report.x, accel_report.y, accel_report.z}; + math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); vect = _board_rotation * vect; raw.accelerometer_m_s2[0] = vect(0); @@ -946,7 +954,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report); - math::Vector3 vect = {gyro_report.x, gyro_report.y, gyro_report.z}; + math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); vect = _board_rotation * vect; raw.gyro_rad_s[0] = vect(0); @@ -972,7 +980,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report); - math::Vector3 vect = {mag_report.x, mag_report.y, mag_report.z}; + math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z); if (_mag_is_external) vect = _external_mag_rotation * vect; @@ -1157,17 +1165,16 @@ Sensors::adc_poll(struct sensor_combined_s &raw) if (!_publishing) return; + hrt_abstime t = hrt_absolute_time(); /* rate limit to 100 Hz */ - if (hrt_absolute_time() - _last_adc >= 10000) { + if (t - _last_adc >= 10000) { /* make space for a maximum of eight channels */ struct adc_msg_s buf_adc[8]; /* read all channels available */ int ret = read(_fd_adc, &buf_adc, sizeof(buf_adc)); - for (unsigned i = 0; i < sizeof(buf_adc) / sizeof(buf_adc[0]); i++) { - - if (ret >= (int)sizeof(buf_adc[0])) { - + if (ret >= (int)sizeof(buf_adc[0])) { + for (unsigned i = 0; i < sizeof(buf_adc) / sizeof(buf_adc[0]); i++) { /* Save raw voltage values */ if (i < (sizeof(raw.adc_voltage_v)) / sizeof(raw.adc_voltage_v[0])) { raw.adc_voltage_v[i] = buf_adc[i].am_data / (4096.0f / 3.3f); @@ -1178,27 +1185,40 @@ Sensors::adc_poll(struct sensor_combined_s &raw) /* Voltage in volts */ float voltage = (buf_adc[i].am_data * _parameters.battery_voltage_scaling); - if (voltage > VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS) { - + if (voltage > BATT_V_IGNORE_THRESHOLD) { + _battery_status.voltage_v = voltage; /* one-time initialization of low-pass value to avoid long init delays */ - if (_battery_status.voltage_v < 3.0f) { - _battery_status.voltage_v = voltage; + if (_battery_status.voltage_filtered_v < BATT_V_IGNORE_THRESHOLD) { + _battery_status.voltage_filtered_v = voltage; } - _battery_status.timestamp = hrt_absolute_time(); - _battery_status.voltage_v = (BAT_VOL_LOWPASS_1 * (_battery_status.voltage_v + BAT_VOL_LOWPASS_2 * voltage));; - /* current and discharge are unknown */ - _battery_status.current_a = -1.0f; - _battery_status.discharged_mah = -1.0f; + _battery_status.timestamp = t; + _battery_status.voltage_filtered_v += (voltage - _battery_status.voltage_filtered_v) * BATT_V_LOWPASS; - /* announce the battery voltage if needed, just publish else */ - if (_battery_pub > 0) { - orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status); + } else { + /* mark status as invalid */ + _battery_status.voltage_v = -1.0f; + _battery_status.voltage_filtered_v = -1.0f; + } - } else { - _battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status); + } else if (ADC_BATTERY_CURRENT_CHANNEL == buf_adc[i].am_channel) { + /* handle current only if voltage is valid */ + if (_battery_status.voltage_v > 0.0f) { + float current = (buf_adc[i].am_data * _parameters.battery_current_scaling); + /* check measured current value */ + if (current >= 0.0f) { + _battery_status.timestamp = t; + _battery_status.current_a = current; + if (_battery_current_timestamp != 0) { + /* initialize discharged value */ + if (_battery_status.discharged_mah < 0.0f) + _battery_status.discharged_mah = 0.0f; + _battery_discharged += current * (t - _battery_current_timestamp); + _battery_status.discharged_mah = ((float) _battery_discharged) / 3600000.0f; + } } } + _battery_current_timestamp = t; } else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) { @@ -1214,7 +1234,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw) float diff_pres_pa = voltage * 1000.0f - _parameters.diff_pres_offset_pa; //for MPXV7002DP sensor - _diff_pres.timestamp = hrt_absolute_time(); + _diff_pres.timestamp = t; _diff_pres.differential_pressure_pa = diff_pres_pa; _diff_pres.voltage = voltage; @@ -1227,8 +1247,16 @@ Sensors::adc_poll(struct sensor_combined_s &raw) } } } + } + _last_adc = t; + if (_battery_status.voltage_v > 0.0f) { + /* announce the battery status if needed, just publish else */ + if (_battery_pub > 0) { + orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status); - _last_adc = hrt_absolute_time(); + } else { + _battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status); + } } } } @@ -1475,9 +1503,6 @@ void Sensors::task_main() { - /* inform about start */ - warnx("Initializing.."); - /* start individual sensors */ accel_init(); gyro_init(); @@ -1516,7 +1541,10 @@ Sensors::task_main() raw.adc_voltage_v[3] = 0.0f; memset(&_battery_status, 0, sizeof(_battery_status)); - _battery_status.voltage_v = BAT_VOL_INITIAL; + _battery_status.voltage_v = -1.0f; + _battery_status.voltage_filtered_v = -1.0f; + _battery_status.current_a = -1.0f; + _battery_status.discharged_mah = -1.0f; /* get a set of initial values */ accel_poll(raw); diff --git a/src/lib/mathlib/math/Vector3.hpp b/src/modules/systemlib/board_serial.c similarity index 65% rename from src/lib/mathlib/math/Vector3.hpp rename to src/modules/systemlib/board_serial.c index 568d9669ab..ad8c2bf83e 100644 --- a/src/lib/mathlib/math/Vector3.hpp +++ b/src/modules/systemlib/board_serial.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 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 @@ -32,45 +32,29 @@ ****************************************************************************/ /** - * @file Vector3.hpp + * @file board_serial.h + * Read off the board serial + * + * @author Lorenz Meier + * @author David "Buzz" Bussenschutt * - * math 3 vector */ -#pragma once +#include "otp.h" +#include "board_config.h" +#include "board_serial.h" -#include "Vector.hpp" - -namespace math +int get_board_serial(char *serialid) { + const volatile unsigned *udid_ptr = (const unsigned *)UDID_START; + union udid id; + val_read((unsigned *)&id, udid_ptr, sizeof(id)); -class __EXPORT Vector3 : - public Vector -{ -public: - Vector3(); - Vector3(const Vector &right); - Vector3(float x, float y, float z); - Vector3(const float *data); - virtual ~Vector3(); - Vector3 cross(const Vector3 &b) const; - /** - * accessors - */ - void setX(float x) { (*this)(0) = x; } - void setY(float y) { (*this)(1) = y; } - void setZ(float z) { (*this)(2) = z; } - const float &getX() const { return (*this)(0); } - const float &getY() const { return (*this)(1); } - const float &getZ() const { return (*this)(2); } -}; - -class __EXPORT Vector3f : - public Vector3 -{ -}; - -int __EXPORT vector3Test(); -} // math + /* Copy the serial from the chips non-write memory and swap endianess */ + serialid[0] = id.data[3]; serialid[1] = id.data[2]; serialid[2] = id.data[1]; serialid[3] = id.data[0]; + serialid[4] = id.data[7]; serialid[5] = id.data[6]; serialid[6] = id.data[5]; serialid[7] = id.data[4]; + serialid[8] = id.data[11]; serialid[9] = id.data[10]; serialid[10] = id.data[9]; serialid[11] = id.data[8]; + return 0; +} \ No newline at end of file diff --git a/src/lib/mathlib/math/arm/Matrix.cpp b/src/modules/systemlib/board_serial.h similarity index 84% rename from src/lib/mathlib/math/arm/Matrix.cpp rename to src/modules/systemlib/board_serial.h index 21661622ab..b14bb4376b 100644 --- a/src/lib/mathlib/math/arm/Matrix.cpp +++ b/src/modules/systemlib/board_serial.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 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 @@ -32,9 +32,18 @@ ****************************************************************************/ /** - * @file Matrix.cpp + * @file board_serial.h + * Read off the board serial + * + * @author Lorenz Meier + * @author David "Buzz" Bussenschutt * - * matrix code */ -#include "Matrix.hpp" +#pragma once + +__BEGIN_DECLS + +__EXPORT int get_board_serial(char *serialid); + +__END_DECLS diff --git a/src/modules/systemlib/bson/tinybson.c b/src/modules/systemlib/bson/tinybson.c index 8aca6a25dd..49403c98bd 100644 --- a/src/modules/systemlib/bson/tinybson.c +++ b/src/modules/systemlib/bson/tinybson.c @@ -407,6 +407,9 @@ bson_encoder_fini(bson_encoder_t encoder) memcpy(encoder->buf, &len, sizeof(len)); } + /* sync file */ + fsync(encoder->fd); + return 0; } diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index 843cda7225..3953b757d2 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -49,4 +49,8 @@ SRCS = err.c \ airspeed.c \ system_params.c \ mavlink_log.c \ - rc_check.c + rc_check.c \ + otp.c \ + board_serial.c \ + pwm_limit/pwm_limit.c + diff --git a/src/modules/systemlib/otp.c b/src/modules/systemlib/otp.c new file mode 100644 index 0000000000..695574fdcb --- /dev/null +++ b/src/modules/systemlib/otp.c @@ -0,0 +1,224 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. + * Authors: + * Lorenz Meier + * David "Buzz" Bussenschutt + * + * 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 otp.c + * otp estimation + * + * @author Lorenz Meier + * @author David "Buzz" Bussenschutt + * + */ + +#include +#include +#include +#include +#include +#include // memset +#include "conversions.h" +#include "otp.h" +#include "err.h" // warnx +#include + + +int val_read(void *dest, volatile const void *src, int bytes) +{ + + int i; + + for (i = 0; i < bytes / 4; i++) { + *(((volatile unsigned *)dest) + i) = *(((volatile unsigned *)src) + i); + } + + return i * 4; +} + + +int write_otp(uint8_t id_type, uint32_t vid, uint32_t pid, char *signature) +{ + + warnx("write_otp: PX4 / %02X / %02X / %02X / ... etc \n", id_type, vid, pid); + + int errors = 0; + + // descriptor + if (F_write_byte(ADDR_OTP_START, 'P')) + errors++; + // write the 'P' from PX4. to first byte in OTP + if (F_write_byte(ADDR_OTP_START + 1, 'X')) + errors++; // write the 'P' from PX4. to first byte in OTP + if (F_write_byte(ADDR_OTP_START + 2, '4')) + errors++; + if (F_write_byte(ADDR_OTP_START + 3, '\0')) + errors++; + //id_type + if (F_write_byte(ADDR_OTP_START + 4, id_type)) + errors++; + // vid and pid are 4 bytes each + if (F_write_word(ADDR_OTP_START + 5, vid)) + errors++; + if (F_write_word(ADDR_OTP_START + 9, pid)) + errors++; + + // leave some 19 bytes of space, and go to the next block... + // then the auth sig starts + for (int i = 0 ; i < 128 ; i++) { + if (F_write_byte(ADDR_OTP_START + 32 + i, signature[i])) + errors++; + } + + return errors; +} + +int lock_otp(void) +{ + //determine the required locking size - can only write full lock bytes */ +// int size = sizeof(struct otp) / 32; +// +// struct otp_lock otp_lock_mem; +// +// memset(&otp_lock_mem, OTP_LOCK_UNLOCKED, sizeof(otp_lock_mem)); +// for (int i = 0; i < sizeof(otp_lock_mem) / sizeof(otp_lock_mem.lock_bytes[0]); i++) +// otp_lock_mem.lock_bytes[i] = OTP_LOCK_LOCKED; + //XXX add the actual call here to write the OTP_LOCK bytes only at final stage + // val_copy(lock_ptr, &otp_lock_mem, sizeof(otp_lock_mem)); + + int locksize = 5; + + int errors = 0; + + // or just realise it's exctly 5x 32byte blocks we need to lock. 1 block for ID,type,vid,pid, and 4 blocks for certificate, which is 128 bytes. + for (int i = 0 ; i < locksize ; i++) { + if (F_write_byte(ADDR_OTP_LOCK_START + i, OTP_LOCK_LOCKED)) + errors++; + } + + return errors; +} + + + +// COMPLETE, BUSY, or other flash error? +int F_GetStatus(void) +{ + int fs = F_COMPLETE; + + if ((FLASH->status & F_BSY) == F_BSY) { fs = F_BUSY; } else { + + if ((FLASH->status & F_WRPERR) != (uint32_t)0x00) { fs = F_ERROR_WRP; } else { + + if ((FLASH->status & (uint32_t)0xEF) != (uint32_t)0x00) { fs = F_ERROR_PROGRAM; } else { + + if ((FLASH->status & F_OPERR) != (uint32_t)0x00) { fs = F_ERROR_OPERATION; } else { + fs = F_COMPLETE; + } + } + } + } + + return fs; +} + + +// enable FLASH Registers +void F_unlock(void) +{ + if ((FLASH->control & F_CR_LOCK) != 0) { + FLASH->key = F_KEY1; + FLASH->key = F_KEY2; + } +} + +// lock the FLASH Registers +void F_lock(void) +{ + FLASH->control |= F_CR_LOCK; +} + +// flash write word. +int F_write_word(uint32_t Address, uint32_t Data) +{ + unsigned char octet[4] = {0, 0, 0, 0}; + + int ret = 0; + + for (int i = 0; i < 4; i++) { + octet[i] = (Data >> (i * 8)) & 0xFF; + ret = F_write_byte(Address + i, octet[i]); + } + + return ret; +} + +// flash write byte +int F_write_byte(uint32_t Address, uint8_t Data) +{ + volatile int status = F_COMPLETE; + + //warnx("F_write_byte: %08X %02d", Address , Data ) ; + + //Check the parameters + assert(IS_F_ADDRESS(Address)); + + //Wait for FLASH operation to complete by polling on BUSY flag. + status = F_GetStatus(); + + while (status == F_BUSY) { status = F_GetStatus();} + + if (status == F_COMPLETE) { + //if the previous operation is completed, proceed to program the new data + FLASH->control &= CR_PSIZE_MASK; + FLASH->control |= F_PSIZE_BYTE; + FLASH->control |= F_CR_PG; + + *(volatile uint8_t *)Address = Data; + + //Wait for FLASH operation to complete by polling on BUSY flag. + status = F_GetStatus(); + + while (status == F_BUSY) { status = F_GetStatus();} + + //if the program operation is completed, disable the PG Bit + FLASH->control &= (~F_CR_PG); + } + + //Return the Program Status + return !(status == F_COMPLETE); +} + + + diff --git a/src/modules/systemlib/otp.h b/src/modules/systemlib/otp.h new file mode 100644 index 0000000000..f10e129d8c --- /dev/null +++ b/src/modules/systemlib/otp.h @@ -0,0 +1,151 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file otp.h + * One TIme Programmable ( OTP ) Flash routine/s. + * + * @author Lorenz Meier + * @author David "Buzz" Bussenschutt + * + */ + +#ifndef OTP_H_ +#define OTP_H_ + +__BEGIN_DECLS + +#define ADDR_OTP_START 0x1FFF7800 +#define ADDR_OTP_LOCK_START 0x1FFF7A00 + +#define OTP_LOCK_LOCKED 0x00 +#define OTP_LOCK_UNLOCKED 0xFF + + + +#include +#include + +// possible flash statuses +#define F_BUSY 1 +#define F_ERROR_WRP 2 +#define F_ERROR_PROGRAM 3 +#define F_ERROR_OPERATION 4 +#define F_COMPLETE 5 + +typedef struct { + volatile uint32_t accesscontrol; // 0x00 + volatile uint32_t key; // 0x04 + volatile uint32_t optionkey; // 0x08 + volatile uint32_t status; // 0x0C + volatile uint32_t control; // 0x10 + volatile uint32_t optioncontrol; //0x14 +} flash_registers; + +#define PERIPH_BASE ((uint32_t)0x40000000) //Peripheral base address +#define AHB1PERIPH_BASE (PERIPH_BASE + 0x00020000) +#define F_R_BASE (AHB1PERIPH_BASE + 0x3C00) +#define FLASH ((flash_registers *) F_R_BASE) + +#define F_BSY ((uint32_t)0x00010000) //FLASH Busy flag bit +#define F_OPERR ((uint32_t)0x00000002) //FLASH operation Error flag bit +#define F_WRPERR ((uint32_t)0x00000010) //FLASH Write protected error flag bit +#define CR_PSIZE_MASK ((uint32_t)0xFFFFFCFF) +#define F_PSIZE_WORD ((uint32_t)0x00000200) +#define F_PSIZE_BYTE ((uint32_t)0x00000000) +#define F_CR_PG ((uint32_t)0x00000001) // a bit in the F_CR register +#define F_CR_LOCK ((uint32_t)0x80000000) // also another bit. + +#define F_KEY1 ((uint32_t)0x45670123) +#define F_KEY2 ((uint32_t)0xCDEF89AB) +#define IS_F_ADDRESS(ADDRESS) ((((ADDRESS) >= 0x08000000) && ((ADDRESS) < 0x080FFFFF)) || (((ADDRESS) >= 0x1FFF7800) && ((ADDRESS) < 0x1FFF7A0F))) + + + +#pragma pack(push, 1) + +/* + * The OTP area is divided into 16 OTP data blocks of 32 bytes and one lock OTP block of 16 bytes. + * The OTP data and lock blocks cannot be erased. The lock block contains 16 bytes LOCKBi (0 ≤ i ≤ 15) + * to lock the corresponding OTP data block (blocks 0 to 15). Each OTP data block can be programmed + * until the value 0x00 is programmed in the corresponding OTP lock byte. The lock bytes must only + * contain 0x00 and 0xFF values, otherwise the OTP bytes might not be taken into account correctly. + */ + +struct otp { + // first 32 bytes = the '0' Block + char id[4]; ///4 bytes < 'P' 'X' '4' '\n' + uint8_t id_type; ///1 byte < 0 for USB VID, 1 for generic VID + uint32_t vid; ///4 bytes + uint32_t pid; ///4 bytes + char unused[19]; ///19 bytes + // Cert-of-Auth is next 4 blocks ie 1-4 ( where zero is first block ) + char signature[128]; + // insert extras here + uint32_t lock_bytes[4]; +}; + +struct otp_lock { + uint8_t lock_bytes[16]; +}; +#pragma pack(pop) + +#define ADDR_F_SIZE 0x1FFF7A22 + +#pragma pack(push, 1) +union udid { + uint32_t serial[3]; + char data[12]; +}; +#pragma pack(pop) + + +/** + * s + */ +//__EXPORT float calc_indicated_airspeed(float differential_pressure); + +__EXPORT void F_unlock(void); +__EXPORT void F_lock(void); +__EXPORT int val_read(void *dest, volatile const void *src, int bytes); +__EXPORT int val_write(volatile void *dest, const void *src, int bytes); +__EXPORT int write_otp(uint8_t id_type, uint32_t vid, uint32_t pid, char *signature); +__EXPORT int lock_otp(void); + + +__EXPORT int F_write_byte(uint32_t Address, uint8_t Data); +__EXPORT int F_write_word(uint32_t Address, uint32_t Data); + +__END_DECLS + +#endif diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index 398657dd79..2d773fd251 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 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 @@ -61,7 +61,7 @@ #include "uORB/uORB.h" #include "uORB/topics/parameter_update.h" -#if 1 +#if 0 # define debug(fmt, args...) do { warnx(fmt, ##args); } while(0) #else # define debug(fmt, args...) do { } while(0) @@ -512,6 +512,28 @@ param_save_default(void) int fd; const char *filename = param_get_default_file(); + + /* write parameters to temp file */ + fd = open(filename, O_WRONLY | O_CREAT); + + if (fd < 0) { + warn("failed to open param file: %s", filename); + return ERROR; + } + + if (res == OK) { + res = param_export(fd, false); + + if (res != OK) { + warnx("failed to write parameters to file: %s", filename); + } + } + + close(fd); + + return res; + +#if 0 const char *filename_tmp = malloc(strlen(filename) + 5); sprintf(filename_tmp, "%s.tmp", filename); @@ -565,6 +587,7 @@ param_save_default(void) free(filename_tmp); return res; +#endif } /** @@ -573,9 +596,9 @@ param_save_default(void) int param_load_default(void) { - int fd = open(param_get_default_file(), O_RDONLY); + int fd_load = open(param_get_default_file(), O_RDONLY); - if (fd < 0) { + if (fd_load < 0) { /* no parameter file is OK, otherwise this is an error */ if (errno != ENOENT) { warn("open '%s' for reading failed", param_get_default_file()); @@ -584,8 +607,8 @@ param_load_default(void) return 1; } - int result = param_load(fd); - close(fd); + int result = param_load(fd_load); + close(fd_load); if (result != 0) { warn("error reading parameters from '%s'", param_get_default_file()); diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index 77c952f52c..6a4e9392a2 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -39,7 +39,7 @@ /** * @file pid.c * - * Implementation of generic PID control interface. + * Implementation of generic PID controller. * * @author Laurens Mackay * @author Tobias Naegeli @@ -53,24 +53,21 @@ #define SIGMA 0.000001f -__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, - float limit, uint8_t mode, float dt_min) +__EXPORT void pid_init(PID_t *pid, uint8_t mode, float dt_min) { - pid->kp = kp; - pid->ki = ki; - pid->kd = kd; - pid->intmax = intmax; - pid->limit = limit; pid->mode = mode; pid->dt_min = dt_min; - pid->count = 0.0f; - pid->saturated = 0.0f; - pid->last_output = 0.0f; - pid->sp = 0.0f; - pid->error_previous = 0.0f; + pid->kp = 0.0f; + pid->ki = 0.0f; + pid->kd = 0.0f; pid->integral = 0.0f; + pid->integral_limit = 0.0f; + pid->output_limit = 0.0f; + pid->error_previous = 0.0f; + pid->last_output = 0.0f; } -__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit) + +__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float integral_limit, float output_limit) { int ret = 0; @@ -95,15 +92,15 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float ret = 1; } - if (isfinite(intmax)) { - pid->intmax = intmax; + if (isfinite(integral_limit)) { + pid->integral_limit = integral_limit; } else { ret = 1; } - if (isfinite(limit)) { - pid->limit = limit; + if (isfinite(output_limit)) { + pid->output_limit = output_limit; } else { ret = 1; @@ -112,42 +109,18 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float return ret; } -//void pid_set(PID_t *pid, float sp) -//{ -// pid->sp = sp; -// pid->error_previous = 0; -// pid->integral = 0; -//} - -/** - * - * @param pid - * @param val - * @param dt - * @return - */ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt) { - /* error = setpoint - actual_position - integral = integral + (error*dt) - derivative = (error - previous_error)/dt - output = (Kp*error) + (Ki*integral) + (Kd*derivative) - previous_error = error - wait(dt) - goto start - */ - if (!isfinite(sp) || !isfinite(val) || !isfinite(val_dot) || !isfinite(dt)) { return pid->last_output; } float i, d; - pid->sp = sp; - // Calculated current error value - float error = pid->sp - val; + /* current error value */ + float error = sp - val; - // Calculate or measured current error derivative + /* current error derivative */ if (pid->mode == PID_MODE_DERIVATIV_CALC) { d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min); pid->error_previous = error; @@ -167,39 +140,34 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo d = 0.0f; } - if (pid->ki > 0.0f) { + /* calculate PD output */ + float output = (error * pid->kp) + (d * pid->kd); + + if (pid->ki > SIGMA) { // Calculate the error integral and check for saturation i = pid->integral + (error * dt); - if ((pid->limit > SIGMA && (fabsf((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit)) || - fabsf(i) > pid->intmax) { - i = pid->integral; // If saturated then do not update integral value - pid->saturated = 1; - - } else { - if (!isfinite(i)) { - i = 0.0f; + /* check for saturation */ + if (isfinite(i)) { + if ((pid->output_limit < SIGMA || (fabsf(output + (i * pid->ki)) <= pid->output_limit)) && + fabsf(i) <= pid->integral_limit) { + /* not saturated, use new integral value */ + pid->integral = i; } - - pid->integral = i; - pid->saturated = 0; } - } else { - i = 0.0f; - pid->saturated = 0; + /* add I component to output */ + output += pid->integral * pid->ki; } - // Calculate the output. Limit output magnitude to pid->limit - float output = (error * pid->kp) + (i * pid->ki) + (d * pid->kd); - + /* limit output */ if (isfinite(output)) { - if (pid->limit > SIGMA) { - if (output > pid->limit) { - output = pid->limit; + if (pid->output_limit > SIGMA) { + if (output > pid->output_limit) { + output = pid->output_limit; - } else if (output < -pid->limit) { - output = -pid->limit; + } else if (output < -pid->output_limit) { + output = -pid->output_limit; } } @@ -212,5 +180,5 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo __EXPORT void pid_reset_integral(PID_t *pid) { - pid->integral = 0; + pid->integral = 0.0f; } diff --git a/src/modules/systemlib/pid/pid.h b/src/modules/systemlib/pid/pid.h index eca228464f..e8b1aac4fd 100644 --- a/src/modules/systemlib/pid/pid.h +++ b/src/modules/systemlib/pid/pid.h @@ -39,7 +39,7 @@ /** * @file pid.h * - * Definition of generic PID control interface. + * Definition of generic PID controller. * * @author Laurens Mackay * @author Tobias Naegeli @@ -55,38 +55,35 @@ __BEGIN_DECLS -/* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error - * val_dot in pid_calculate() will be ignored */ -#define PID_MODE_DERIVATIV_CALC 0 -/* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value, setpoint derivative is ignored - * val_dot in pid_calculate() will be ignored */ -#define PID_MODE_DERIVATIV_CALC_NO_SP 1 -/* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */ -#define PID_MODE_DERIVATIV_SET 2 -// Use PID_MODE_DERIVATIV_NONE for a PI controller (vs PID) -#define PID_MODE_DERIVATIV_NONE 9 +typedef enum PID_MODE { + /* Use PID_MODE_DERIVATIV_NONE for a PI controller (vs PID) */ + PID_MODE_DERIVATIV_NONE = 0, + /* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error, + * val_dot in pid_calculate() will be ignored */ + PID_MODE_DERIVATIV_CALC, + /* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value, + * setpoint derivative will be ignored, val_dot in pid_calculate() will be ignored */ + PID_MODE_DERIVATIV_CALC_NO_SP, + /* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */ + PID_MODE_DERIVATIV_SET +} pid_mode_t; typedef struct { + pid_mode_t mode; + float dt_min; float kp; float ki; float kd; - float intmax; - float sp; float integral; + float integral_limit; + float output_limit; float error_previous; float last_output; - float limit; - float dt_min; - uint8_t mode; - uint8_t count; - uint8_t saturated; } PID_t; -__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, uint8_t mode, float dt_min); -__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit); -//void pid_set(PID_t *pid, float sp); +__EXPORT void pid_init(PID_t *pid, pid_mode_t mode, float dt_min); +__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float integral_limit, float output_limit); __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt); - __EXPORT void pid_reset_integral(PID_t *pid); __END_DECLS diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c index cac3dc82a3..190b315f1f 100644 --- a/src/modules/systemlib/pwm_limit/pwm_limit.c +++ b/src/modules/systemlib/pwm_limit/pwm_limit.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. * Author: Julian Oes * * Redistribution and use in source and binary forms, with or without @@ -44,38 +44,53 @@ #include #include #include +#include void pwm_limit_init(pwm_limit_t *limit) { - limit->state = LIMIT_STATE_OFF; + limit->state = PWM_LIMIT_STATE_INIT; limit->time_armed = 0; return; } -void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, float *output, uint16_t *effective_pwm, pwm_limit_t *limit) +void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, const float *output, uint16_t *effective_pwm, pwm_limit_t *limit) { + /* first evaluate state changes */ switch (limit->state) { - case LIMIT_STATE_OFF: - if (armed) - limit->state = LIMIT_STATE_RAMP; - limit->time_armed = hrt_absolute_time(); + case PWM_LIMIT_STATE_INIT: + + if (armed) { + + /* set arming time for the first call */ + if (limit->time_armed == 0) { + limit->time_armed = hrt_absolute_time(); + } + + if (hrt_elapsed_time(&limit->time_armed) >= INIT_TIME_US) { + limit->state = PWM_LIMIT_STATE_OFF; + } + } break; - case LIMIT_STATE_INIT: - if (!armed) - limit->state = LIMIT_STATE_OFF; - else if (hrt_absolute_time() - limit->time_armed >= INIT_TIME_US) - limit->state = LIMIT_STATE_RAMP; + case PWM_LIMIT_STATE_OFF: + if (armed) { + limit->state = PWM_LIMIT_STATE_RAMP; + + /* reset arming time, used for ramp timing */ + limit->time_armed = hrt_absolute_time(); + } break; - case LIMIT_STATE_RAMP: - if (!armed) - limit->state = LIMIT_STATE_OFF; - else if (hrt_absolute_time() - limit->time_armed >= INIT_TIME_US + RAMP_TIME_US) - limit->state = LIMIT_STATE_ON; + case PWM_LIMIT_STATE_RAMP: + if (!armed) { + limit->state = PWM_LIMIT_STATE_OFF; + } else if (hrt_elapsed_time(&limit->time_armed) >= RAMP_TIME_US) { + limit->state = PWM_LIMIT_STATE_ON; + } break; - case LIMIT_STATE_ON: - if (!armed) - limit->state = LIMIT_STATE_OFF; + case PWM_LIMIT_STATE_ON: + if (!armed) { + limit->state = PWM_LIMIT_STATE_OFF; + } break; default: break; @@ -86,44 +101,47 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_ /* then set effective_pwm based on state */ switch (limit->state) { - case LIMIT_STATE_OFF: - case LIMIT_STATE_INIT: + case PWM_LIMIT_STATE_OFF: + case PWM_LIMIT_STATE_INIT: for (unsigned i=0; itime_armed); - progress = (hrt_absolute_time() - INIT_TIME_US - limit->time_armed)*10000 / RAMP_TIME_US; - for (unsigned i=0; i 0) { + progress = diff * 10000 / RAMP_TIME_US; - /* safeguard against overflows */ - uint16_t disarmed = disarmed_pwm[i]; - if (disarmed > min_pwm[i]) - disarmed = min_pwm[i]; + for (unsigned i=0; i 0) { - uint16_t disarmed_min_diff = min_pwm[i] - disarmed; - ramp_min_pwm = disarmed + (disarmed_min_diff * progress) / 10000; - } else { - - /* no disarmed pwm value set, choose min pwm */ - ramp_min_pwm = min_pwm[i]; + /* safeguard against overflows */ + unsigned disarmed = disarmed_pwm[i]; + if (disarmed > min_pwm[i]) { + disarmed = min_pwm[i]; + } + + unsigned disarmed_min_diff = min_pwm[i] - disarmed; + ramp_min_pwm = disarmed + (disarmed_min_diff * progress) / 10000; + + } else { + + /* no disarmed pwm value set, choose min pwm */ + ramp_min_pwm = min_pwm[i]; + } + + effective_pwm[i] = output[i] * (max_pwm[i] - ramp_min_pwm)/2 + (max_pwm[i] + ramp_min_pwm)/2; } - - effective_pwm[i] = output[i] * (max_pwm[i] - ramp_min_pwm)/2 + (max_pwm[i] + ramp_min_pwm)/2; - output[i] = (float)progress/10000.0f * output[i]; } break; - case LIMIT_STATE_ON: + case PWM_LIMIT_STATE_ON: for (unsigned i=0; i #include +__BEGIN_DECLS + /* * time for the ESCs to initialize * (this is not actually needed if PWM is sent right after boot) @@ -56,21 +58,21 @@ */ #define RAMP_TIME_US 2500000 +enum pwm_limit_state { + PWM_LIMIT_STATE_OFF = 0, + PWM_LIMIT_STATE_INIT, + PWM_LIMIT_STATE_RAMP, + PWM_LIMIT_STATE_ON +}; + typedef struct { - enum { - LIMIT_STATE_OFF = 0, - LIMIT_STATE_INIT, - LIMIT_STATE_RAMP, - LIMIT_STATE_ON - } state; + enum pwm_limit_state state; uint64_t time_armed; } pwm_limit_t; -__BEGIN_DECLS - __EXPORT void pwm_limit_init(pwm_limit_t *limit); -__EXPORT void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, float *output, uint16_t *effective_pwm, pwm_limit_t *limit); +__EXPORT void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, const float *output, uint16_t *effective_pwm, pwm_limit_t *limit); __END_DECLS diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 79a820c066..4c84c1f25e 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -117,8 +117,8 @@ ORB_DEFINE(vehicle_local_position_setpoint, struct vehicle_local_position_setpoi #include "topics/vehicle_bodyframe_speed_setpoint.h" ORB_DEFINE(vehicle_bodyframe_speed_setpoint, struct vehicle_bodyframe_speed_setpoint_s); -#include "topics/mission_item_triplet.h" -ORB_DEFINE(mission_item_triplet, struct mission_item_triplet_s); +#include "topics/position_setpoint_triplet.h" +ORB_DEFINE(position_setpoint_triplet, struct position_setpoint_triplet_s); #include "topics/vehicle_global_velocity_setpoint.h" ORB_DEFINE(vehicle_global_velocity_setpoint, struct vehicle_global_velocity_setpoint_s); diff --git a/src/modules/uORB/topics/battery_status.h b/src/modules/uORB/topics/battery_status.h index c40d0d4e56..d473dff3f4 100644 --- a/src/modules/uORB/topics/battery_status.h +++ b/src/modules/uORB/topics/battery_status.h @@ -53,9 +53,10 @@ */ struct battery_status_s { uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ - float voltage_v; /**< Battery voltage in volts, filtered */ - float current_a; /**< Battery current in amperes, filtered, -1 if unknown */ - float discharged_mah; /**< Discharged amount in mAh, filtered, -1 if unknown */ + float voltage_v; /**< Battery voltage in volts, 0 if unknown */ + float voltage_filtered_v; /**< Battery voltage in volts, filtered, 0 if unknown */ + float current_a; /**< Battery current in amperes, -1 if unknown */ + float discharged_mah; /**< Discharged amount in mAh, -1 if unknown */ }; /** @@ -65,4 +66,4 @@ struct battery_status_s { /* register this as object request broker structure */ ORB_DECLARE(battery_status); -#endif \ No newline at end of file +#endif diff --git a/src/modules/uORB/topics/home_position.h b/src/modules/uORB/topics/home_position.h index 3e2fee84e9..08d11abaea 100644 --- a/src/modules/uORB/topics/home_position.h +++ b/src/modules/uORB/topics/home_position.h @@ -62,7 +62,7 @@ struct home_position_s //bool altitude_is_relative; // TODO what means home relative altitude? we need clear definition of reference altitude then double lat; /**< Latitude in degrees */ double lon; /**< Longitude in degrees */ - float altitude; /**< Altitude in meters */ + float alt; /**< Altitude in meters */ }; /** diff --git a/src/modules/uORB/topics/mission_item_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h similarity index 72% rename from src/modules/uORB/topics/mission_item_triplet.h rename to src/modules/uORB/topics/position_setpoint_triplet.h index b35eae6070..4b57833b63 100644 --- a/src/modules/uORB/topics/mission_item_triplet.h +++ b/src/modules/uORB/topics/position_setpoint_triplet.h @@ -46,31 +46,42 @@ #include #include "../uORB.h" -#include "mission.h" - /** * @addtogroup topics * @{ */ +enum SETPOINT_TYPE +{ + SETPOINT_TYPE_NORMAL = 0, + SETPOINT_TYPE_LOITER, + SETPOINT_TYPE_TAKEOFF, + SETPOINT_TYPE_LAND, +}; + +struct position_setpoint_s +{ + bool valid; /**< true if setpoint is valid */ + enum SETPOINT_TYPE type; /**< setpoint type to adjust behavior of position controller */ + double lat; /**< latitude, in deg */ + double lon; /**< longitude, in deg */ + float alt; /**< altitude AMSL, in m */ + float yaw; /**< yaw (only for multirotors), in rad [-PI..PI), NaN = hold current yaw */ + float loiter_radius; /**< loiter radius (only for fixed wing), in m */ + int8_t loiter_direction; /**< loiter direction: 1 = CW, -1 = CCW */ + float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */ +}; + /** * Global position setpoint triplet in WGS84 coordinates. * * This are the three next waypoints (or just the next two or one). */ -struct mission_item_triplet_s +struct position_setpoint_triplet_s { - bool previous_valid; - bool current_valid; /**< flag indicating previous mission item is valid */ - bool next_valid; /**< flag indicating next mission item is valid */ - - struct mission_item_s previous; - struct mission_item_s current; - struct mission_item_s next; - - int previous_index; - int current_index; - int next_index; + struct position_setpoint_s previous; + struct position_setpoint_s current; + struct position_setpoint_s next; }; /** @@ -78,6 +89,6 @@ struct mission_item_triplet_s */ /* register this as object request broker structure */ -ORB_DECLARE(mission_item_triplet); +ORB_DECLARE(position_setpoint_triplet); #endif diff --git a/src/modules/uORB/topics/vehicle_attitude.h b/src/modules/uORB/topics/vehicle_attitude.h index 4380a5ee77..e5a35ff9bc 100755 --- a/src/modules/uORB/topics/vehicle_attitude.h +++ b/src/modules/uORB/topics/vehicle_attitude.h @@ -76,6 +76,7 @@ struct vehicle_attitude_s { float rate_offsets[3]; /**< Offsets of the body angular rates from zero */ float R[3][3]; /**< Rotation matrix body to world, (Tait-Bryan, NED) */ float q[4]; /**< Quaternion (NED) */ + float g_comp[3]; /**< Compensated gravity vector */ bool R_valid; /**< Rotation matrix valid */ bool q_valid; /**< Quaternion valid */ diff --git a/src/modules/uORB/topics/vehicle_attitude_setpoint.h b/src/modules/uORB/topics/vehicle_attitude_setpoint.h index 1a245132a8..7596f944f3 100644 --- a/src/modules/uORB/topics/vehicle_attitude_setpoint.h +++ b/src/modules/uORB/topics/vehicle_attitude_setpoint.h @@ -61,7 +61,7 @@ struct vehicle_attitude_setpoint_s float yaw_body; /**< body angle in NED frame */ //float body_valid; /**< Set to true if body angles are valid */ - float R_body[9]; /**< Rotation matrix describing the setpoint as rotation from the current body frame */ + float R_body[3][3]; /**< Rotation matrix describing the setpoint as rotation from the current body frame */ bool R_valid; /**< Set to true if rotation matrix is valid */ //! For quaternion-based attitude control diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 26dcbd985c..5aecac8987 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -67,6 +67,7 @@ typedef enum { NAV_STATE_LOITER, NAV_STATE_MISSION, NAV_STATE_RTL, + NAV_STATE_LAND, NAV_STATE_MAX } nav_state_t; @@ -92,7 +93,7 @@ struct vehicle_control_mode_s bool flag_control_position_enabled; /**< true if position is controlled */ bool flag_control_altitude_enabled; /**< true if altitude is controlled */ bool flag_control_climb_rate_enabled; /**< true if climb rate is controlled */ - bool flag_control_flighttermination_enabled; /**< true if flighttermination is enabled */ + bool flag_control_termination_enabled; /**< true if flighttermination is enabled */ }; /** diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h index 143734e372..ae771ca008 100644 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ b/src/modules/uORB/topics/vehicle_global_position.h @@ -54,7 +54,7 @@ /** * Fused global position in WGS84. * - * This struct contains the system's believ about its position. It is not the raw GPS + * This struct contains global position estimation. It is not the raw GPS * measurement (@see vehicle_gps_position). This topic is usually published by the position * estimator, which will take more sources of information into account than just GPS, * e.g. control inputs of the vehicle in a Kalman-filter implementation. @@ -65,14 +65,13 @@ struct vehicle_global_position_s uint64_t time_gps_usec; /**< GPS timestamp in microseconds */ bool valid; /**< true if position satisfies validity criteria of estimator */ - int32_t lat; /**< Latitude in 1E7 degrees */ - int32_t lon; /**< Longitude in 1E7 degrees */ - float alt; /**< Altitude in meters */ - float relative_alt; /**< Altitude above home position in meters, */ - float vx; /**< Ground X velocity, m/s in NED */ - float vy; /**< Ground Y velocity, m/s in NED */ - float vz; /**< Ground Z velocity, m/s in NED */ - float yaw; /**< Compass heading in radians -PI..+PI. */ + double lat; /**< Latitude in degrees */ + double lon; /**< Longitude in degrees */ + float alt; /**< Altitude in meters */ + float vel_n; /**< Ground north velocity, m/s */ + float vel_e; /**< Ground east velocity, m/s */ + float vel_d; /**< Ground downside velocity, m/s */ + float yaw; /**< Yaw in radians -PI..+PI. */ }; /** diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h index 4271537829..d567f2e020 100644 --- a/src/modules/uORB/topics/vehicle_local_position.h +++ b/src/modules/uORB/topics/vehicle_local_position.h @@ -77,6 +77,11 @@ struct vehicle_local_position_s int32_t ref_lon; /**< Reference point longitude in 1E7 degrees */ float ref_alt; /**< Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level */ bool landed; /**< true if vehicle is landed */ + /* Distance to surface */ + float dist_bottom; /**< Distance to bottom surface (ground) */ + float dist_bottom_rate; /**< Distance to bottom surface (ground) change rate */ + uint64_t surface_bottom_timestamp; /**< Time when new bottom surface found */ + bool dist_bottom_valid; /**< true if distance to bottom surface is valid */ }; /** diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 1a9dec5f51..a5988d3ba4 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -64,6 +64,7 @@ typedef enum { MAIN_STATE_SEATBELT, MAIN_STATE_EASY, MAIN_STATE_AUTO, + MAIN_STATE_MAX } main_state_t; typedef enum { @@ -73,7 +74,8 @@ typedef enum { ARMING_STATE_ARMED_ERROR, ARMING_STATE_STANDBY_ERROR, ARMING_STATE_REBOOT, - ARMING_STATE_IN_AIR_RESTORE + ARMING_STATE_IN_AIR_RESTORE, + ARMING_STATE_MAX } arming_state_t; typedef enum { @@ -82,9 +84,12 @@ typedef enum { } hil_state_t; typedef enum { - FLIGHTTERMINATION_STATE_OFF = 0, - FLIGHTTERMINATION_STATE_ON -} flighttermination_state_t; + FAILSAFE_STATE_NORMAL = 0, /**< Normal operation */ + FAILSAFE_STATE_RTL, /**< Return To Launch */ + FAILSAFE_STATE_LAND, /**< Land without position control */ + FAILSAFE_STATE_TERMINATION, /**< Disable motors and use parachute, can't be recovered */ + FAILSAFE_STATE_MAX +} failsafe_state_t; typedef enum { MODE_SWITCH_MANUAL = 0, @@ -173,6 +178,7 @@ struct vehicle_status_s uint64_t set_nav_state_timestamp; /**< timestamp of latest change of set_nav_state */ arming_state_t arming_state; /**< current arming state */ hil_state_t hil_state; /**< current hil state */ + failsafe_state_t failsafe_state; /**< current failsafe state */ int32_t system_type; /**< system type, inspired by MAVLink's VEHICLE_TYPE enum */ int32_t system_id; /**< system id, inspired by MAVLink's system ID field */ @@ -223,8 +229,6 @@ struct vehicle_status_s uint16_t errors_count2; uint16_t errors_count3; uint16_t errors_count4; - - flighttermination_state_t flighttermination_state; }; /** diff --git a/src/systemcmds/eeprom/eeprom.c b/src/systemcmds/eeprom/eeprom.c deleted file mode 100644 index 2aed80e011..0000000000 --- a/src/systemcmds/eeprom/eeprom.c +++ /dev/null @@ -1,265 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file eeprom.c - * - * EEPROM service and utility app. - */ - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include - -#include "systemlib/systemlib.h" -#include "systemlib/param/param.h" -#include "systemlib/err.h" - -#ifndef PX4_I2C_BUS_ONBOARD -# error PX4_I2C_BUS_ONBOARD not defined, cannot locate onboard EEPROM -#endif - -__EXPORT int eeprom_main(int argc, char *argv[]); - -static void eeprom_attach(void); -static void eeprom_start(void); -static void eeprom_erase(void); -static void eeprom_ioctl(unsigned operation); -static void eeprom_save(const char *name); -static void eeprom_load(const char *name); -static void eeprom_test(void); - -static bool attached = false; -static bool started = false; -static struct mtd_dev_s *eeprom_mtd; - -int eeprom_main(int argc, char *argv[]) -{ - if (argc >= 2) { - if (!strcmp(argv[1], "start")) - eeprom_start(); - - if (!strcmp(argv[1], "save_param")) - eeprom_save(argv[2]); - - if (!strcmp(argv[1], "load_param")) - eeprom_load(argv[2]); - - if (!strcmp(argv[1], "erase")) - eeprom_erase(); - - if (!strcmp(argv[1], "test")) - eeprom_test(); - - if (0) { /* these actually require a file on the filesystem... */ - - if (!strcmp(argv[1], "reformat")) - eeprom_ioctl(FIOC_REFORMAT); - - if (!strcmp(argv[1], "repack")) - eeprom_ioctl(FIOC_OPTIMIZE); - } - } - - errx(1, "expected a command, try 'start'\n\t'save_param /eeprom/parameters'\n\t'load_param /eeprom/parameters'\n\t'erase'\n"); -} - - -static void -eeprom_attach(void) -{ - /* find the right I2C */ - struct i2c_dev_s *i2c = up_i2cinitialize(PX4_I2C_BUS_ONBOARD); - /* this resets the I2C bus, set correct bus speed again */ - I2C_SETFREQUENCY(i2c, 400000); - - if (i2c == NULL) - errx(1, "failed to locate I2C bus"); - - /* start the MTD driver, attempt 5 times */ - for (int i = 0; i < 5; i++) { - eeprom_mtd = at24c_initialize(i2c); - if (eeprom_mtd) { - /* abort on first valid result */ - if (i > 0) { - warnx("warning: EEPROM needed %d attempts to attach", i+1); - } - break; - } - } - - /* if last attempt is still unsuccessful, abort */ - if (eeprom_mtd == NULL) - errx(1, "failed to initialize EEPROM driver"); - - attached = true; -} - -static void -eeprom_start(void) -{ - int ret; - - if (started) - errx(1, "EEPROM already mounted"); - - if (!attached) - eeprom_attach(); - - /* start NXFFS */ - ret = nxffs_initialize(eeprom_mtd); - - if (ret < 0) - errx(1, "failed to initialize NXFFS - erase EEPROM to reformat"); - - /* mount the EEPROM */ - ret = mount(NULL, "/eeprom", "nxffs", 0, NULL); - - if (ret < 0) - errx(1, "failed to mount /eeprom - erase EEPROM to reformat"); - - started = true; - warnx("mounted EEPROM at /eeprom"); - exit(0); -} - -extern int at24c_nuke(void); - -static void -eeprom_erase(void) -{ - if (!attached) - eeprom_attach(); - - if (at24c_nuke()) - errx(1, "erase failed"); - - errx(0, "erase done, reboot now"); -} - -static void -eeprom_ioctl(unsigned operation) -{ - int fd; - - fd = open("/eeprom/.", 0); - - if (fd < 0) - err(1, "open /eeprom"); - - if (ioctl(fd, operation, 0) < 0) - err(1, "ioctl"); - - exit(0); -} - -static void -eeprom_save(const char *name) -{ - if (!started) - errx(1, "must be started first"); - - if (!name) - err(1, "missing argument for device name, try '/eeprom/parameters'"); - - warnx("WARNING: 'eeprom save_param' deprecated - use 'param save' instead"); - - /* delete the file in case it exists */ - unlink(name); - - /* create the file */ - int fd = open(name, O_WRONLY | O_CREAT | O_EXCL); - - if (fd < 0) - err(1, "opening '%s' failed", name); - - int result = param_export(fd, false); - close(fd); - - if (result < 0) { - unlink(name); - errx(1, "error exporting to '%s'", name); - } - - exit(0); -} - -static void -eeprom_load(const char *name) -{ - if (!started) - errx(1, "must be started first"); - - if (!name) - err(1, "missing argument for device name, try '/eeprom/parameters'"); - - warnx("WARNING: 'eeprom load_param' deprecated - use 'param load' instead"); - - int fd = open(name, O_RDONLY); - - if (fd < 0) - err(1, "open '%s'", name); - - int result = param_load(fd); - close(fd); - - if (result < 0) - errx(1, "error importing from '%s'", name); - - exit(0); -} - -extern void at24c_test(void); - -static void -eeprom_test(void) -{ - at24c_test(); - exit(0); -} diff --git a/src/lib/mathlib/math/generic/Matrix.cpp b/src/systemcmds/hw_ver/hw_ver.c similarity index 69% rename from src/lib/mathlib/math/generic/Matrix.cpp rename to src/systemcmds/hw_ver/hw_ver.c index 21661622ab..4b84523cc1 100644 --- a/src/lib/mathlib/math/generic/Matrix.cpp +++ b/src/systemcmds/hw_ver/hw_ver.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * 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 @@ -32,9 +32,42 @@ ****************************************************************************/ /** - * @file Matrix.cpp + * @file hw_ver.c * - * matrix code + * Show and test hardware version. */ -#include "Matrix.hpp" +#include + +#include +#include +#include +#include + +__EXPORT int hw_ver_main(int argc, char *argv[]); + +int +hw_ver_main(int argc, char *argv[]) +{ + if (argc >= 2) { + if (!strcmp(argv[1], "show")) { + printf(HW_ARCH "\n"); + exit(0); + } + + if (!strcmp(argv[1], "compare")) { + if (argc >= 3) { + int ret = strcmp(HW_ARCH, argv[2]) != 0; + if (ret == 0) { + printf("hw_ver match: %s\n", HW_ARCH); + } + exit(ret); + + } else { + errx(1, "not enough arguments, try 'compare PX4FMU_1'"); + } + } + } + + errx(1, "expected a command, try 'show' or 'compare'"); +} diff --git a/src/systemcmds/hw_ver/module.mk b/src/systemcmds/hw_ver/module.mk new file mode 100644 index 0000000000..3cc08b6a1a --- /dev/null +++ b/src/systemcmds/hw_ver/module.mk @@ -0,0 +1,43 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# Show and test hardware version +# + +MODULE_COMMAND = hw_ver +SRCS = hw_ver.c + +MODULE_STACKSIZE = 1024 + +MAXOPTIMIZATION = -Os diff --git a/src/systemcmds/eeprom/24xxxx_mtd.c b/src/systemcmds/mtd/24xxxx_mtd.c similarity index 100% rename from src/systemcmds/eeprom/24xxxx_mtd.c rename to src/systemcmds/mtd/24xxxx_mtd.c diff --git a/src/systemcmds/mtd/module.mk b/src/systemcmds/mtd/module.mk new file mode 100644 index 0000000000..b3fceceb5c --- /dev/null +++ b/src/systemcmds/mtd/module.mk @@ -0,0 +1,6 @@ +# +# RAMTRON file system driver +# + +MODULE_COMMAND = mtd +SRCS = mtd.c 24xxxx_mtd.c diff --git a/src/systemcmds/mtd/mtd.c b/src/systemcmds/mtd/mtd.c new file mode 100644 index 0000000000..a2a0c109c6 --- /dev/null +++ b/src/systemcmds/mtd/mtd.c @@ -0,0 +1,493 @@ +/**************************************************************************** + * + * 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 mtd.c + * + * mtd service and utility app. + * + * @author Lorenz Meier + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include "systemlib/systemlib.h" +#include "systemlib/param/param.h" +#include "systemlib/err.h" + +#include + +__EXPORT int mtd_main(int argc, char *argv[]); + +#ifndef CONFIG_MTD + +/* create a fake command with decent warnx to not confuse users */ +int mtd_main(int argc, char *argv[]) +{ + errx(1, "MTD not enabled, skipping."); +} + +#else + +#ifdef CONFIG_MTD_RAMTRON +static void ramtron_attach(void); +#else + +#ifndef PX4_I2C_BUS_ONBOARD +# error PX4_I2C_BUS_ONBOARD not defined, cannot locate onboard EEPROM +#endif + +static void at24xxx_attach(void); +#endif +static void mtd_start(char *partition_names[], unsigned n_partitions); +static void mtd_test(void); +static void mtd_erase(char *partition_names[], unsigned n_partitions); +static void mtd_readtest(char *partition_names[], unsigned n_partitions); +static void mtd_rwtest(char *partition_names[], unsigned n_partitions); +static void mtd_print_info(); +static int mtd_get_geometry(unsigned long *blocksize, unsigned long *erasesize, unsigned long *neraseblocks, + unsigned *blkpererase, unsigned *nblocks, unsigned *partsize, unsigned n_partitions); + +static bool attached = false; +static bool started = false; +static struct mtd_dev_s *mtd_dev; +static unsigned n_partitions_current = 0; + +/* note, these will be equally sized */ +static char *partition_names_default[] = {"/fs/mtd_params", "/fs/mtd_waypoints"}; +static const int n_partitions_default = sizeof(partition_names_default) / sizeof(partition_names_default[0]); + +int mtd_main(int argc, char *argv[]) +{ + if (argc >= 2) { + if (!strcmp(argv[1], "start")) { + + /* start mapping according to user request */ + if (argc >= 3) { + mtd_start(argv + 2, argc - 2); + } else { + mtd_start(partition_names_default, n_partitions_default); + } + } + + if (!strcmp(argv[1], "test")) + mtd_test(); + + if (!strcmp(argv[1], "readtest")) { + if (argc >= 3) { + mtd_readtest(argv + 2, argc - 2); + } else { + mtd_readtest(partition_names_default, n_partitions_default); + } + } + + if (!strcmp(argv[1], "rwtest")) { + if (argc >= 3) { + mtd_rwtest(argv + 2, argc - 2); + } else { + mtd_rwtest(partition_names_default, n_partitions_default); + } + } + + if (!strcmp(argv[1], "status")) + mtd_status(); + + if (!strcmp(argv[1], "erase")) { + if (argc >= 3) { + mtd_erase(argv + 2, argc - 2); + } else { + mtd_erase(partition_names_default, n_partitions_default); + } + } + } + + errx(1, "expected a command, try 'start', 'erase', 'status', 'readtest', 'rwtest' or 'test'"); +} + +struct mtd_dev_s *ramtron_initialize(FAR struct spi_dev_s *dev); +struct mtd_dev_s *mtd_partition(FAR struct mtd_dev_s *mtd, + off_t firstblock, off_t nblocks); + +#ifdef CONFIG_MTD_RAMTRON +static void +ramtron_attach(void) +{ + /* find the right spi */ + struct spi_dev_s *spi = up_spiinitialize(2); + /* this resets the spi bus, set correct bus speed again */ + SPI_SETFREQUENCY(spi, 10 * 1000 * 1000); + SPI_SETBITS(spi, 8); + SPI_SETMODE(spi, SPIDEV_MODE3); + SPI_SELECT(spi, SPIDEV_FLASH, false); + + if (spi == NULL) + errx(1, "failed to locate spi bus"); + + /* start the RAMTRON driver, attempt 5 times */ + for (int i = 0; i < 5; i++) { + mtd_dev = ramtron_initialize(spi); + + if (mtd_dev) { + /* abort on first valid result */ + if (i > 0) { + warnx("warning: mtd needed %d attempts to attach", i + 1); + } + + break; + } + } + + /* if last attempt is still unsuccessful, abort */ + if (mtd_dev == NULL) + errx(1, "failed to initialize mtd driver"); + + int ret = mtd_dev->ioctl(mtd_dev, MTDIOC_SETSPEED, (unsigned long)10*1000*1000); + if (ret != OK) + warnx(1, "failed to set bus speed"); + + attached = true; +} +#else + +static void +at24xxx_attach(void) +{ + /* find the right I2C */ + struct i2c_dev_s *i2c = up_i2cinitialize(PX4_I2C_BUS_ONBOARD); + /* this resets the I2C bus, set correct bus speed again */ + I2C_SETFREQUENCY(i2c, 400000); + + if (i2c == NULL) + errx(1, "failed to locate I2C bus"); + + /* start the MTD driver, attempt 5 times */ + for (int i = 0; i < 5; i++) { + mtd_dev = at24c_initialize(i2c); + if (mtd_dev) { + /* abort on first valid result */ + if (i > 0) { + warnx("warning: EEPROM needed %d attempts to attach", i+1); + } + break; + } + } + + /* if last attempt is still unsuccessful, abort */ + if (mtd_dev == NULL) + errx(1, "failed to initialize EEPROM driver"); + + attached = true; +} +#endif + +static void +mtd_start(char *partition_names[], unsigned n_partitions) +{ + int ret; + + if (started) + errx(1, "mtd already mounted"); + + if (!attached) { + #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + at24xxx_attach(); + #else + ramtron_attach(); + #endif + } + + if (!mtd_dev) { + warnx("ERROR: Failed to create RAMTRON FRAM MTD instance"); + exit(1); + } + + unsigned long blocksize, erasesize, neraseblocks; + unsigned blkpererase, nblocks, partsize; + + ret = mtd_get_geometry(&blocksize, &erasesize, &neraseblocks, &blkpererase, &nblocks, &partsize, n_partitions); + if (ret) + exit(3); + + /* Now create MTD FLASH partitions */ + + warnx("Creating partitions"); + FAR struct mtd_dev_s *part[n_partitions]; + char blockname[32]; + + unsigned offset; + unsigned i; + + for (offset = 0, i = 0; i < n_partitions; offset += nblocks, i++) { + + warnx(" Partition %d. Block offset=%lu, size=%lu", + i, (unsigned long)offset, (unsigned long)nblocks); + + /* Create the partition */ + + part[i] = mtd_partition(mtd_dev, offset, nblocks); + + if (!part[i]) { + warnx("ERROR: mtd_partition failed. offset=%lu nblocks=%lu", + (unsigned long)offset, (unsigned long)nblocks); + exit(4); + } + + /* Initialize to provide an FTL block driver on the MTD FLASH interface */ + + snprintf(blockname, sizeof(blockname), "/dev/mtdblock%d", i); + + ret = ftl_initialize(i, part[i]); + + if (ret < 0) { + warnx("ERROR: ftl_initialize %s failed: %d", blockname, ret); + exit(5); + } + + /* Now create a character device on the block device */ + + ret = bchdev_register(blockname, partition_names[i], false); + + if (ret < 0) { + warnx("ERROR: bchdev_register %s failed: %d", partition_names[i], ret); + exit(6); + } + } + + n_partitions_current = n_partitions; + + started = true; + exit(0); +} + +int mtd_get_geometry(unsigned long *blocksize, unsigned long *erasesize, unsigned long *neraseblocks, + unsigned *blkpererase, unsigned *nblocks, unsigned *partsize, unsigned n_partitions) +{ + /* Get the geometry of the FLASH device */ + + FAR struct mtd_geometry_s geo; + + int ret = mtd_dev->ioctl(mtd_dev, MTDIOC_GEOMETRY, (unsigned long)((uintptr_t)&geo)); + + if (ret < 0) { + warnx("ERROR: mtd->ioctl failed: %d", ret); + return ret; + } + + *blocksize = geo.blocksize; + *erasesize = geo.blocksize; + *neraseblocks = geo.neraseblocks; + + /* Determine the size of each partition. Make each partition an even + * multiple of the erase block size (perhaps not using some space at the + * end of the FLASH). + */ + + *blkpererase = geo.erasesize / geo.blocksize; + *nblocks = (geo.neraseblocks / n_partitions) * *blkpererase; + *partsize = *nblocks * geo.blocksize; + + return ret; +} + +/* + get partition size in bytes + */ +static ssize_t mtd_get_partition_size(void) +{ + unsigned long blocksize, erasesize, neraseblocks; + unsigned blkpererase, nblocks, partsize = 0; + + int ret = mtd_get_geometry(&blocksize, &erasesize, &neraseblocks, &blkpererase, &nblocks, &partsize, n_partitions_current); + if (ret != OK) { + errx(1, "Failed to get geometry"); + } + return partsize; +} + +void mtd_print_info() +{ + if (!attached) + exit(1); + + unsigned long blocksize, erasesize, neraseblocks; + unsigned blkpererase, nblocks, partsize; + + int ret = mtd_get_geometry(&blocksize, &erasesize, &neraseblocks, &blkpererase, &nblocks, &partsize, n_partitions_current); + if (ret) + exit(3); + + warnx("Flash Geometry:"); + + printf(" blocksize: %lu\n", blocksize); + printf(" erasesize: %lu\n", erasesize); + printf(" neraseblocks: %lu\n", neraseblocks); + printf(" No. partitions: %u\n", n_partitions_current); + printf(" Partition size: %u Blocks (%u bytes)\n", nblocks, partsize); + printf(" TOTAL SIZE: %u KiB\n", neraseblocks * erasesize / 1024); + +} + +void +mtd_test(void) +{ + warnx("This test routine does not test anything yet!"); + exit(1); +} + +void +mtd_status(void) +{ + if (!attached) + errx(1, "MTD driver not started"); + + mtd_print_info(); + exit(0); +} + +void +mtd_erase(char *partition_names[], unsigned n_partitions) +{ + uint8_t v[64]; + memset(v, 0xFF, sizeof(v)); + for (uint8_t i = 0; i < n_partitions; i++) { + uint32_t count = 0; + printf("Erasing %s\n", partition_names[i]); + int fd = open(partition_names[i], O_WRONLY); + if (fd == -1) { + errx(1, "Failed to open partition"); + } + while (write(fd, v, sizeof(v)) == sizeof(v)) { + count += sizeof(v); + } + printf("Erased %lu bytes\n", (unsigned long)count); + close(fd); + } + exit(0); +} + +/* + readtest is useful during startup to validate the device is + responding on the bus. It relies on the driver returning an error on + bad reads (the ramtron driver does return an error) + */ +void +mtd_readtest(char *partition_names[], unsigned n_partitions) +{ + ssize_t expected_size = mtd_get_partition_size(); + + uint8_t v[128]; + for (uint8_t i = 0; i < n_partitions; i++) { + uint32_t count = 0; + printf("reading %s expecting %u bytes\n", partition_names[i], expected_size); + int fd = open(partition_names[i], O_RDONLY); + if (fd == -1) { + errx(1, "Failed to open partition"); + } + while (read(fd, v, sizeof(v)) == sizeof(v)) { + count += sizeof(v); + } + if (count != expected_size) { + errx(1,"Failed to read partition - got %u/%u bytes", count, expected_size); + } + close(fd); + } + printf("readtest OK\n"); + exit(0); +} + +/* + rwtest is useful during startup to validate the device is + responding on the bus for both reads and writes. It reads data in + blocks and writes the data back, then reads it again, failing if the + data isn't the same + */ +void +mtd_rwtest(char *partition_names[], unsigned n_partitions) +{ + ssize_t expected_size = mtd_get_partition_size(); + + uint8_t v[128], v2[128]; + for (uint8_t i = 0; i < n_partitions; i++) { + uint32_t count = 0; + off_t offset = 0; + printf("rwtest %s testing %u bytes\n", partition_names[i], expected_size); + int fd = open(partition_names[i], O_RDWR); + if (fd == -1) { + errx(1, "Failed to open partition"); + } + while (read(fd, v, sizeof(v)) == sizeof(v)) { + count += sizeof(v); + if (lseek(fd, offset, SEEK_SET) != offset) { + errx(1, "seek failed"); + } + if (write(fd, v, sizeof(v)) != sizeof(v)) { + errx(1, "write failed"); + } + if (lseek(fd, offset, SEEK_SET) != offset) { + errx(1, "seek failed"); + } + if (read(fd, v2, sizeof(v2)) != sizeof(v2)) { + errx(1, "read failed"); + } + if (memcmp(v, v2, sizeof(v2)) != 0) { + errx(1, "memcmp failed"); + } + offset += sizeof(v); + } + if (count != expected_size) { + errx(1,"Failed to read partition - got %u/%u bytes", count, expected_size); + } + close(fd); + } + printf("rwtest OK\n"); + exit(0); +} + +#endif diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c index 458bb2259e..7d9484d3eb 100644 --- a/src/systemcmds/nshterm/nshterm.c +++ b/src/systemcmds/nshterm/nshterm.c @@ -62,7 +62,9 @@ nshterm_main(int argc, char *argv[]) } uint8_t retries = 0; int fd = -1; - while (retries < 50) { + + /* 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); diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index 65f291f401..580fdc62f4 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -72,7 +72,12 @@ param_main(int argc, char *argv[]) if (argc >= 3) { do_save(argv[2]); } else { - do_save(param_get_default_file()); + if (param_save_default()) { + warnx("Param export failed."); + exit(1); + } else { + exit(0); + } } } @@ -133,11 +138,8 @@ param_main(int argc, char *argv[]) static void do_save(const char* param_file_name) { - /* delete the parameter file in case it exists */ - unlink(param_file_name); - /* create the file */ - int fd = open(param_file_name, O_WRONLY | O_CREAT | O_EXCL); + int fd = open(param_file_name, O_WRONLY | O_CREAT); if (fd < 0) err(1, "opening '%s' failed", param_file_name); @@ -146,7 +148,7 @@ do_save(const char* param_file_name) close(fd); if (result < 0) { - unlink(param_file_name); + (void)unlink(param_file_name); errx(1, "error exporting to '%s'", param_file_name); } @@ -203,11 +205,38 @@ do_show_print(void *arg, param_t param) int32_t i; float f; const char *search_string = (const char*)arg; + const char *p_name = (const char*)param_name(param); /* print nothing if search string is invalid and not matching */ - if (!(arg == NULL || (!strcmp(search_string, param_name(param))))) { - /* param not found */ - return; + if (!(arg == NULL)) { + + /* start search */ + char *ss = search_string; + char *pp = p_name; + bool mismatch = false; + + /* XXX this comparison is only ok for trailing wildcards */ + while (*ss != '\0' && *pp != '\0') { + + if (*ss == *pp) { + ss++; + pp++; + } else if (*ss == '*') { + if (*(ss + 1) != '\0') { + warnx("* symbol only allowed at end of search string."); + exit(1); + } + + pp++; + } else { + /* param not found */ + return; + } + } + + /* the search string must have been consumed */ + if (!(*ss == '\0' || *ss == '*')) + return; } printf("%c %s: ", diff --git a/src/systemcmds/ramtron/module.mk b/src/systemcmds/ramtron/module.mk deleted file mode 100644 index e4eb1d143c..0000000000 --- a/src/systemcmds/ramtron/module.mk +++ /dev/null @@ -1,6 +0,0 @@ -# -# RAMTRON file system driver -# - -MODULE_COMMAND = ramtron -SRCS = ramtron.c diff --git a/src/systemcmds/ramtron/ramtron.c b/src/systemcmds/ramtron/ramtron.c deleted file mode 100644 index 03c713987a..0000000000 --- a/src/systemcmds/ramtron/ramtron.c +++ /dev/null @@ -1,279 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file ramtron.c - * - * ramtron service and utility app. - */ - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include - -#include "systemlib/systemlib.h" -#include "systemlib/param/param.h" -#include "systemlib/err.h" - -__EXPORT int ramtron_main(int argc, char *argv[]); - -#ifndef CONFIG_MTD_RAMTRON - -/* create a fake command with decent message to not confuse users */ -int ramtron_main(int argc, char *argv[]) -{ - errx(1, "RAMTRON not enabled, skipping."); -} -#else - -static void ramtron_attach(void); -static void ramtron_start(void); -static void ramtron_erase(void); -static void ramtron_ioctl(unsigned operation); -static void ramtron_save(const char *name); -static void ramtron_load(const char *name); -static void ramtron_test(void); - -static bool attached = false; -static bool started = false; -static struct mtd_dev_s *ramtron_mtd; - -int ramtron_main(int argc, char *argv[]) -{ - if (argc >= 2) { - if (!strcmp(argv[1], "start")) - ramtron_start(); - - if (!strcmp(argv[1], "save_param")) - ramtron_save(argv[2]); - - if (!strcmp(argv[1], "load_param")) - ramtron_load(argv[2]); - - if (!strcmp(argv[1], "erase")) - ramtron_erase(); - - if (!strcmp(argv[1], "test")) - ramtron_test(); - - if (0) { /* these actually require a file on the filesystem... */ - - if (!strcmp(argv[1], "reformat")) - ramtron_ioctl(FIOC_REFORMAT); - - if (!strcmp(argv[1], "repack")) - ramtron_ioctl(FIOC_OPTIMIZE); - } - } - - errx(1, "expected a command, try 'start'\n\t'save_param /ramtron/parameters'\n\t'load_param /ramtron/parameters'\n\t'erase'\n"); -} - -struct mtd_dev_s *ramtron_initialize(FAR struct spi_dev_s *dev); - - -static void -ramtron_attach(void) -{ - /* find the right spi */ - struct spi_dev_s *spi = up_spiinitialize(2); - /* this resets the spi bus, set correct bus speed again */ - // xxx set in ramtron driver, leave this out -// SPI_SETFREQUENCY(spi, 4000000); - SPI_SETFREQUENCY(spi, 375000000); - SPI_SETBITS(spi, 8); - SPI_SETMODE(spi, SPIDEV_MODE3); - SPI_SELECT(spi, SPIDEV_FLASH, false); - - if (spi == NULL) - errx(1, "failed to locate spi bus"); - - /* start the MTD driver, attempt 5 times */ - for (int i = 0; i < 5; i++) { - ramtron_mtd = ramtron_initialize(spi); - if (ramtron_mtd) { - /* abort on first valid result */ - if (i > 0) { - warnx("warning: ramtron needed %d attempts to attach", i+1); - } - break; - } - } - - /* if last attempt is still unsuccessful, abort */ - if (ramtron_mtd == NULL) - errx(1, "failed to initialize ramtron driver"); - - attached = true; -} - -static void -ramtron_start(void) -{ - int ret; - - if (started) - errx(1, "ramtron already mounted"); - - if (!attached) - ramtron_attach(); - - /* start NXFFS */ - ret = nxffs_initialize(ramtron_mtd); - - if (ret < 0) - errx(1, "failed to initialize NXFFS - erase ramtron to reformat"); - - /* mount the ramtron */ - ret = mount(NULL, "/ramtron", "nxffs", 0, NULL); - - if (ret < 0) - errx(1, "failed to mount /ramtron - erase ramtron to reformat"); - - started = true; - warnx("mounted ramtron at /ramtron"); - exit(0); -} - -//extern int at24c_nuke(void); - -static void -ramtron_erase(void) -{ - if (!attached) - ramtron_attach(); - -// if (at24c_nuke()) - errx(1, "erase failed"); - - errx(0, "erase done, reboot now"); -} - -static void -ramtron_ioctl(unsigned operation) -{ - int fd; - - fd = open("/ramtron/.", 0); - - if (fd < 0) - err(1, "open /ramtron"); - - if (ioctl(fd, operation, 0) < 0) - err(1, "ioctl"); - - exit(0); -} - -static void -ramtron_save(const char *name) -{ - if (!started) - errx(1, "must be started first"); - - if (!name) - err(1, "missing argument for device name, try '/ramtron/parameters'"); - - warnx("WARNING: 'ramtron save_param' deprecated - use 'param save' instead"); - - /* delete the file in case it exists */ - unlink(name); - - /* create the file */ - int fd = open(name, O_WRONLY | O_CREAT | O_EXCL); - - if (fd < 0) - err(1, "opening '%s' failed", name); - - int result = param_export(fd, false); - close(fd); - - if (result < 0) { - unlink(name); - errx(1, "error exporting to '%s'", name); - } - - exit(0); -} - -static void -ramtron_load(const char *name) -{ - if (!started) - errx(1, "must be started first"); - - if (!name) - err(1, "missing argument for device name, try '/ramtron/parameters'"); - - warnx("WARNING: 'ramtron load_param' deprecated - use 'param load' instead"); - - int fd = open(name, O_RDONLY); - - if (fd < 0) - err(1, "open '%s'", name); - - int result = param_load(fd); - close(fd); - - if (result < 0) - errx(1, "error importing from '%s'", name); - - exit(0); -} - -//extern void at24c_test(void); - -static void -ramtron_test(void) -{ -// at24c_test(); - exit(0); -} - -#endif diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk index 68a080c77c..acf28c35b2 100644 --- a/src/systemcmds/tests/module.mk +++ b/src/systemcmds/tests/module.mk @@ -24,8 +24,12 @@ SRCS = test_adc.c \ test_uart_loopback.c \ test_uart_send.c \ test_mixer.cpp \ + test_mathlib.cpp \ test_file.c \ tests_main.c \ test_param.c \ test_ppm_loopback.c \ - test_rc.c + test_rc.c \ + test_conv.cpp \ + test_mount.c \ + test_mtd.c diff --git a/src/lib/mathlib/math/EulerAngles.hpp b/src/systemcmds/tests/test_conv.cpp similarity index 66% rename from src/lib/mathlib/math/EulerAngles.hpp rename to src/systemcmds/tests/test_conv.cpp index 399eecfa7d..50dece816b 100644 --- a/src/lib/mathlib/math/EulerAngles.hpp +++ b/src/systemcmds/tests/test_conv.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * 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 @@ -32,43 +32,45 @@ ****************************************************************************/ /** - * @file Vector.h + * @file test_conv.cpp + * Tests conversions used across the system. * - * math vector */ -#pragma once +#include -#include "Vector.hpp" +#include -namespace math +#include +#include +#include +#include +#include +#include + +#include "tests.h" + +#include +#include + +#include +#include +#include + +int test_conv(int argc, char *argv[]) { + warnx("Testing system conversions"); -class Quaternion; -class Dcm; + for (int i = -10000; i <= 10000; i+=1) { + float f = i/10000.0f; + float fres = REG_TO_FLOAT(FLOAT_TO_REG(f)); + if (fabsf(f - fres) > 0.0001f) { + warnx("conversion fail: input: %8.4f, intermediate: %d, result: %8.4f", f, REG_TO_SIGNED(FLOAT_TO_REG(f)), fres); + return 1; + } + } -class __EXPORT EulerAngles : public Vector -{ -public: - EulerAngles(); - EulerAngles(float phi, float theta, float psi); - EulerAngles(const Quaternion &q); - EulerAngles(const Dcm &dcm); - virtual ~EulerAngles(); - - // alias - void setPhi(float phi) { (*this)(0) = phi; } - void setTheta(float theta) { (*this)(1) = theta; } - void setPsi(float psi) { (*this)(2) = psi; } - - // const accessors - const float &getPhi() const { return (*this)(0); } - const float &getTheta() const { return (*this)(1); } - const float &getPsi() const { return (*this)(2); } - -}; - -int __EXPORT eulerAnglesTest(); - -} // math + warnx("All conversions clean"); + return 0; +} diff --git a/src/systemcmds/tests/test_file.c b/src/systemcmds/tests/test_file.c index 798724cf1a..96be1e8df5 100644 --- a/src/systemcmds/tests/test_file.c +++ b/src/systemcmds/tests/test_file.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 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 @@ -35,9 +35,12 @@ * @file test_file.c * * File write test. + * + * @author Lorenz Meier */ #include +#include #include #include #include @@ -51,6 +54,40 @@ #include "tests.h" +static int check_user_abort(int fd); + +int check_user_abort(int fd) { + /* check if user wants to abort */ + char c; + + struct pollfd fds; + int ret; + fds.fd = 0; /* stdin */ + fds.events = POLLIN; + ret = poll(&fds, 1, 0); + + if (ret > 0) { + + read(0, &c, 1); + + switch (c) { + case 0x03: // ctrl-c + case 0x1b: // esc + case 'c': + case 'q': + { + warnx("Test aborted."); + fsync(fd); + close(fd); + return OK; + /* not reached */ + } + } + } + + return 1; +} + int test_file(int argc, char *argv[]) { @@ -86,7 +123,6 @@ test_file(int argc, char *argv[]) uint8_t read_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64))); hrt_abstime start, end; - //perf_counter_t wperf = perf_alloc(PC_ELAPSED, "SD writes (aligned)"); int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); @@ -94,28 +130,25 @@ test_file(int argc, char *argv[]) start = hrt_absolute_time(); for (unsigned i = 0; i < iterations; i++) { - //perf_begin(wperf); int wret = write(fd, write_buf + a, chunk_sizes[c]); if (wret != chunk_sizes[c]) { warn("WRITE ERROR!"); if ((0x3 & (uintptr_t)(write_buf + a))) - errx(1, "memory is unaligned, align shift: %d", a); + warnx("memory is unaligned, align shift: %d", a); + return 1; } fsync(fd); - //perf_end(wperf); + + if (!check_user_abort(fd)) + return OK; } end = hrt_absolute_time(); - //warnx("%dKiB in %llu microseconds", iterations / 2, end - start); - - //perf_print_counter(wperf); - //perf_free(wperf); - close(fd); fd = open("/fs/microsd/testfile", O_RDONLY); @@ -124,7 +157,8 @@ test_file(int argc, char *argv[]) int rret = read(fd, read_buf, chunk_sizes[c]); if (rret != chunk_sizes[c]) { - errx(1, "READ ERROR!"); + warnx("READ ERROR!"); + return 1; } /* compare value */ @@ -139,9 +173,13 @@ test_file(int argc, char *argv[]) } if (!compare_ok) { - errx(1, "ABORTING FURTHER COMPARISON DUE TO ERROR"); + warnx("ABORTING FURTHER COMPARISON DUE TO ERROR"); + return 1; } + if (!check_user_abort(fd)) + return OK; + } /* @@ -152,16 +190,20 @@ test_file(int argc, char *argv[]) int ret = unlink("/fs/microsd/testfile"); fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); - warnx("testing aligned writes - please wait.."); + warnx("testing aligned writes - please wait.. (CTRL^C to abort)"); start = hrt_absolute_time(); for (unsigned i = 0; i < iterations; i++) { int wret = write(fd, write_buf, chunk_sizes[c]); if (wret != chunk_sizes[c]) { - err(1, "WRITE ERROR!"); + warnx("WRITE ERROR!"); + return 1; } + if (!check_user_abort(fd)) + return OK; + } fsync(fd); @@ -178,7 +220,8 @@ test_file(int argc, char *argv[]) int rret = read(fd, read_buf, chunk_sizes[c]); if (rret != chunk_sizes[c]) { - err(1, "READ ERROR!"); + warnx("READ ERROR!"); + return 1; } /* compare value */ @@ -190,10 +233,14 @@ test_file(int argc, char *argv[]) align_read_ok = false; break; } + + if (!check_user_abort(fd)) + return OK; } if (!align_read_ok) { - errx(1, "ABORTING FURTHER COMPARISON DUE TO ERROR"); + warnx("ABORTING FURTHER COMPARISON DUE TO ERROR"); + return 1; } } @@ -215,7 +262,8 @@ test_file(int argc, char *argv[]) int rret = read(fd, read_buf + a, chunk_sizes[c]); if (rret != chunk_sizes[c]) { - err(1, "READ ERROR!"); + warnx("READ ERROR!"); + return 1; } for (int j = 0; j < chunk_sizes[c]; j++) { @@ -228,10 +276,14 @@ test_file(int argc, char *argv[]) if (unalign_read_err_count > 10) break; } + + if (!check_user_abort(fd)) + return OK; } if (!unalign_read_ok) { - errx(1, "ABORTING FURTHER COMPARISON DUE TO ERROR"); + warnx("ABORTING FURTHER COMPARISON DUE TO ERROR"); + return 1; } } @@ -239,9 +291,10 @@ test_file(int argc, char *argv[]) ret = unlink("/fs/microsd/testfile"); close(fd); - if (ret) - err(1, "UNLINKING FILE FAILED"); - + if (ret) { + warnx("UNLINKING FILE FAILED"); + return 1; + } } } @@ -261,75 +314,9 @@ test_file(int argc, char *argv[]) } else { /* failed opening dir */ - err(1, "FAILED LISTING MICROSD ROOT DIRECTORY"); - } - - return 0; -} -#if 0 -int -test_file(int argc, char *argv[]) -{ - const iterations = 1024; - - /* check if microSD card is mounted */ - struct stat buffer; - if (stat("/fs/microsd/", &buffer)) { - warnx("no microSD card mounted, aborting file test"); + warnx("FAILED LISTING MICROSD ROOT DIRECTORY"); return 1; } - uint8_t buf[512]; - hrt_abstime start, end; - perf_counter_t wperf = perf_alloc(PC_ELAPSED, "SD writes"); - - int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); - memset(buf, 0, sizeof(buf)); - - start = hrt_absolute_time(); - for (unsigned i = 0; i < iterations; i++) { - perf_begin(wperf); - write(fd, buf, sizeof(buf)); - perf_end(wperf); - } - end = hrt_absolute_time(); - - close(fd); - - unlink("/fs/microsd/testfile"); - - warnx("%dKiB in %llu microseconds", iterations / 2, end - start); - perf_print_counter(wperf); - perf_free(wperf); - - warnx("running unlink test"); - - /* ensure that common commands do not run against file count limits */ - for (unsigned i = 0; i < 64; i++) { - - warnx("unlink iteration #%u", i); - - int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); - if (fd < 0) - errx(1, "failed opening test file before unlink()"); - int ret = write(fd, buf, sizeof(buf)); - if (ret < 0) - errx(1, "failed writing test file before unlink()"); - close(fd); - - ret = unlink("/fs/microsd/testfile"); - if (ret != OK) - errx(1, "failed unlinking test file"); - - fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); - if (fd < 0) - errx(1, "failed opening test file after unlink()"); - ret = write(fd, buf, sizeof(buf)); - if (ret < 0) - errx(1, "failed writing test file after unlink()"); - close(fd); - } - return 0; } -#endif diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp new file mode 100644 index 0000000000..693a208ba7 --- /dev/null +++ b/src/systemcmds/tests/test_mathlib.cpp @@ -0,0 +1,159 @@ +/**************************************************************************** + * + * 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 test_mathlib.cpp + * + * Mathlib test + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "tests.h" + +#define TEST_OP(_title, _op) { unsigned int n = 60000; hrt_abstime t0, t1; t0 = hrt_absolute_time(); for (unsigned int j = 0; j < n; j++) { _op; }; t1 = hrt_absolute_time(); warnx(_title ": %.6fus", (double)(t1 - t0) / n); } + +using namespace math; + +const char* formatResult(bool res) { + return res ? "OK" : "ERROR"; +} + +int test_mathlib(int argc, char *argv[]) +{ + warnx("testing mathlib"); + + { + Vector<2> v; + Vector<2> v1(1.0f, 2.0f); + Vector<2> v2(1.0f, -1.0f); + float data[2] = {1.0f, 2.0f}; + TEST_OP("Constructor Vector<2>()", Vector<2> v3); + TEST_OP("Constructor Vector<2>(Vector<2>)", Vector<2> v3(v1)); + TEST_OP("Constructor Vector<2>(float[])", Vector<2> v3(data)); + TEST_OP("Constructor Vector<2>(float, float)", Vector<2> v3(1.0f, 2.0f)); + TEST_OP("Vector<2> = Vector<2>", v = v1); + TEST_OP("Vector<2> + Vector<2>", v + v1); + TEST_OP("Vector<2> - Vector<2>", v - v1); + TEST_OP("Vector<2> += Vector<2>", v += v1); + TEST_OP("Vector<2> -= Vector<2>", v -= v1); + TEST_OP("Vector<2> * Vector<2>", v * v1); + TEST_OP("Vector<2> %% Vector<2>", v1 % v2); + } + + { + Vector<3> v; + Vector<3> v1(1.0f, 2.0f, 0.0f); + Vector<3> v2(1.0f, -1.0f, 2.0f); + float data[3] = {1.0f, 2.0f, 3.0f}; + TEST_OP("Constructor Vector<3>()", Vector<3> v3); + TEST_OP("Constructor Vector<3>(Vector<3>)", Vector<3> v3(v1)); + TEST_OP("Constructor Vector<3>(float[])", Vector<3> v3(data)); + TEST_OP("Constructor Vector<3>(float, float, float)", Vector<3> v3(1.0f, 2.0f, 3.0f)); + TEST_OP("Vector<3> = Vector<3>", v = v1); + TEST_OP("Vector<3> + Vector<3>", v + v1); + TEST_OP("Vector<3> - Vector<3>", v - v1); + TEST_OP("Vector<3> += Vector<3>", v += v1); + TEST_OP("Vector<3> -= Vector<3>", v -= v1); + TEST_OP("Vector<3> * float", v1 * 2.0f); + TEST_OP("Vector<3> / float", v1 / 2.0f); + TEST_OP("Vector<3> *= float", v1 *= 2.0f); + TEST_OP("Vector<3> /= float", v1 /= 2.0f); + TEST_OP("Vector<3> * Vector<3>", v * v1); + TEST_OP("Vector<3> %% Vector<3>", v1 % v2); + TEST_OP("Vector<3> length", v1.length()); + TEST_OP("Vector<3> length squared", v1.length_squared()); + TEST_OP("Vector<3> element read", volatile float a = v1(0)); + TEST_OP("Vector<3> element read direct", volatile float a = v1.data[0]); + TEST_OP("Vector<3> element write", v1(0) = 1.0f); + TEST_OP("Vector<3> element write direct", v1.data[0] = 1.0f); + } + + { + Vector<4> v; + Vector<4> v1(1.0f, 2.0f, 0.0f, -1.0f); + Vector<4> v2(1.0f, -1.0f, 2.0f, 0.0f); + float data[4] = {1.0f, 2.0f, 3.0f, 4.0f}; + TEST_OP("Constructor Vector<4>()", Vector<4> v3); + TEST_OP("Constructor Vector<4>(Vector<4>)", Vector<4> v3(v1)); + TEST_OP("Constructor Vector<4>(float[])", Vector<4> v3(data)); + TEST_OP("Constructor Vector<4>(float, float, float, float)", Vector<4> v3(1.0f, 2.0f, 3.0f, 4.0f)); + TEST_OP("Vector<4> = Vector<4>", v = v1); + TEST_OP("Vector<4> + Vector<4>", v + v1); + TEST_OP("Vector<4> - Vector<4>", v - v1); + TEST_OP("Vector<4> += Vector<4>", v += v1); + TEST_OP("Vector<4> -= Vector<4>", v -= v1); + TEST_OP("Vector<4> * Vector<4>", v * v1); + } + + { + Vector<10> v1; + v1.zero(); + float data[10]; + TEST_OP("Constructor Vector<10>()", Vector<10> v3); + TEST_OP("Constructor Vector<10>(Vector<10>)", Vector<10> v3(v1)); + TEST_OP("Constructor Vector<10>(float[])", Vector<10> v3(data)); + } + + { + Matrix<3, 3> m1; + m1.identity(); + Matrix<3, 3> m2; + m2.identity(); + Vector<3> v1(1.0f, 2.0f, 0.0f); + TEST_OP("Matrix<3, 3> * Vector<3>", m1 * v1); + TEST_OP("Matrix<3, 3> + Matrix<3, 3>", m1 + m2); + TEST_OP("Matrix<3, 3> * Matrix<3, 3>", m1 * m2); + } + + { + Matrix<10, 10> m1; + m1.identity(); + Matrix<10, 10> m2; + m2.identity(); + Vector<10> v1; + v1.zero(); + TEST_OP("Matrix<10, 10> * Vector<10>", m1 * v1); + TEST_OP("Matrix<10, 10> + Matrix<10, 10>", m1 + m2); + TEST_OP("Matrix<10, 10> * Matrix<10, 10>", m1 * m2); + } + + return 0; +} diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index 4da86042d2..2a47551ee6 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -48,9 +48,13 @@ #include #include #include +#include #include #include +#include +#include +#include #include "tests.h" @@ -59,6 +63,20 @@ static int mixer_callback(uintptr_t handle, uint8_t control_index, float &control); +const unsigned output_max = 8; +static float actuator_controls[output_max]; +static bool should_arm = false; +uint16_t r_page_servo_disarmed[output_max]; +uint16_t r_page_servo_control_min[output_max]; +uint16_t r_page_servo_control_max[output_max]; +uint16_t r_page_servos[output_max]; +uint16_t servo_predicted[output_max]; + +/* + * PWM limit structure + */ +pwm_limit_t pwm_limit; + int test_mixer(int argc, char *argv[]) { warnx("testing mixer"); @@ -164,6 +182,174 @@ int test_mixer(int argc, char *argv[]) if (mixer_group.count() != 8) return 1; + /* execute the mixer */ + + float outputs[output_max]; + unsigned mixed; + const int jmax = 5; + + pwm_limit_init(&pwm_limit); + should_arm = true; + + /* run through arming phase */ + for (int i = 0; i < output_max; i++) { + actuator_controls[i] = 0.1f; + r_page_servo_disarmed[i] = PWM_LOWEST_MIN; + r_page_servo_control_min[i] = PWM_DEFAULT_MIN; + r_page_servo_control_max[i] = PWM_DEFAULT_MAX; + } + + warnx("ARMING TEST: STARTING RAMP"); + unsigned sleep_quantum_us = 10000; + + hrt_abstime starttime = hrt_absolute_time(); + unsigned sleepcount = 0; + + while (hrt_elapsed_time(&starttime) < INIT_TIME_US + RAMP_TIME_US) { + + /* mix */ + mixed = mixer_group.mix(&outputs[0], output_max); + + pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); + + //warnx("mixed %d outputs (max %d), values:", mixed, output_max); + for (int i = 0; i < mixed; i++) + { + /* check mixed outputs to be zero during init phase */ + if (hrt_elapsed_time(&starttime) < INIT_TIME_US && + r_page_servos[i] != r_page_servo_disarmed[i]) { + warnx("disarmed servo value mismatch"); + return 1; + } + + if (hrt_elapsed_time(&starttime) >= INIT_TIME_US && + r_page_servos[i] + 1 <= r_page_servo_disarmed[i]) { + warnx("ramp servo value mismatch"); + return 1; + } + + //printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, outputs_unlimited[i], outputs[i], (int)r_page_servos[i]); + } + usleep(sleep_quantum_us); + sleepcount++; + + if (sleepcount % 10 == 0) { + printf("."); + fflush(stdout); + } + } + printf("\n"); + + warnx("ARMING TEST: NORMAL OPERATION"); + + for (int j = -jmax; j <= jmax; j++) { + + for (int i = 0; i < output_max; i++) { + actuator_controls[i] = j/10.0f + 0.1f * i; + r_page_servo_disarmed[i] = PWM_LOWEST_MIN; + r_page_servo_control_min[i] = PWM_DEFAULT_MIN; + r_page_servo_control_max[i] = PWM_DEFAULT_MAX; + } + + /* mix */ + mixed = mixer_group.mix(&outputs[0], output_max); + + pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); + + warnx("mixed %d outputs (max %d)", mixed, output_max); + for (int i = 0; i < mixed; i++) + { + servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f; + if (fabsf(servo_predicted[i] - r_page_servos[i]) > 2) { + printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, outputs[i], servo_predicted[i], (int)r_page_servos[i]); + warnx("mixer violated predicted value"); + return 1; + } + } + } + + warnx("ARMING TEST: DISARMING"); + + starttime = hrt_absolute_time(); + sleepcount = 0; + should_arm = false; + + while (hrt_elapsed_time(&starttime) < 600000) { + + /* mix */ + mixed = mixer_group.mix(&outputs[0], output_max); + + pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); + + //warnx("mixed %d outputs (max %d), values:", mixed, output_max); + for (int i = 0; i < mixed; i++) + { + /* check mixed outputs to be zero during init phase */ + if (r_page_servos[i] != r_page_servo_disarmed[i]) { + warnx("disarmed servo value mismatch"); + return 1; + } + + //printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, outputs_unlimited[i], outputs[i], (int)r_page_servos[i]); + } + usleep(sleep_quantum_us); + sleepcount++; + + if (sleepcount % 10 == 0) { + printf("."); + fflush(stdout); + } + } + printf("\n"); + + warnx("ARMING TEST: REARMING: STARTING RAMP"); + + starttime = hrt_absolute_time(); + sleepcount = 0; + should_arm = true; + + while (hrt_elapsed_time(&starttime) < 600000 + RAMP_TIME_US) { + + /* mix */ + mixed = mixer_group.mix(&outputs[0], output_max); + + pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); + + //warnx("mixed %d outputs (max %d), values:", mixed, output_max); + for (int i = 0; i < mixed; i++) + { + /* predict value */ + servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f; + + /* check ramp */ + + if (hrt_elapsed_time(&starttime) < RAMP_TIME_US && + (r_page_servos[i] + 1 <= r_page_servo_disarmed[i] || + r_page_servos[i] > servo_predicted[i])) { + warnx("ramp servo value mismatch"); + return 1; + } + + /* check post ramp phase */ + if (hrt_elapsed_time(&starttime) > RAMP_TIME_US && + fabsf(servo_predicted[i] - r_page_servos[i]) > 2) { + printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, outputs[i], servo_predicted[i], (int)r_page_servos[i]); + warnx("mixer violated predicted value"); + return 1; + } + + //printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, outputs_unlimited[i], outputs[i], (int)r_page_servos[i]); + } + usleep(sleep_quantum_us); + sleepcount++; + + if (sleepcount % 10 == 0) { + printf("."); + fflush(stdout); + } + } + printf("\n"); + /* load multirotor at once test */ mixer_group.reset(); @@ -180,8 +366,12 @@ int test_mixer(int argc, char *argv[]) unsigned mc_loaded = loaded; mixer_group.load_from_buf(&buf[0], mc_loaded); warnx("complete buffer load: loaded %u mixers", mixer_group.count()); - if (mixer_group.count() != 8) + if (mixer_group.count() != 5) { + warnx("FAIL: Quad W mixer load failed"); return 1; + } + + warnx("SUCCESS: No errors in mixer test"); } static int @@ -193,7 +383,10 @@ mixer_callback(uintptr_t handle, if (control_group != 0) return -1; - control = 0.0f; + if (control_index > (sizeof(actuator_controls) / sizeof(actuator_controls[0]))) + return -1; + + control = actuator_controls[control_index]; return 0; } diff --git a/src/systemcmds/tests/test_mount.c b/src/systemcmds/tests/test_mount.c new file mode 100644 index 0000000000..44e34d9ef3 --- /dev/null +++ b/src/systemcmds/tests/test_mount.c @@ -0,0 +1,289 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file test_mount.c + * + * Device mount / unmount stress test + * + * @author Lorenz Meier + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "tests.h" + +const int fsync_tries = 1; +const int abort_tries = 10; + +int +test_mount(int argc, char *argv[]) +{ + const unsigned iterations = 2000; + const unsigned alignments = 10; + + const char* cmd_filename = "/fs/microsd/mount_test_cmds.txt"; + + + /* check if microSD card is mounted */ + struct stat buffer; + if (stat("/fs/microsd/", &buffer)) { + warnx("no microSD card mounted, aborting file test"); + return 1; + } + + /* list directory */ + DIR *d; + struct dirent *dir; + d = opendir("/fs/microsd"); + if (d) { + + while ((dir = readdir(d)) != NULL) { + //printf("%s\n", dir->d_name); + } + + closedir(d); + + warnx("directory listing ok (FS mounted and readable)"); + + } else { + /* failed opening dir */ + warnx("FAILED LISTING MICROSD ROOT DIRECTORY"); + + if (stat(cmd_filename, &buffer) == OK) { + (void)unlink(cmd_filename); + } + + return 1; + } + + /* read current test status from file, write test instructions for next round */ + + /* initial values */ + int it_left_fsync = fsync_tries; + int it_left_abort = abort_tries; + + int cmd_fd; + if (stat(cmd_filename, &buffer) == OK) { + + /* command file exists, read off state */ + cmd_fd = open(cmd_filename, O_RDWR | O_NONBLOCK); + char buf[64]; + int ret = read(cmd_fd, buf, sizeof(buf)); + + if (ret > 0) { + int count = 0; + ret = sscanf(buf, "TEST: %u %u %n", &it_left_fsync, &it_left_abort, &count); + } else { + buf[0] = '\0'; + } + + if (it_left_fsync > fsync_tries) + it_left_fsync = fsync_tries; + + if (it_left_abort > abort_tries) + it_left_abort = abort_tries; + + warnx("Iterations left: #%d / #%d of %d / %d\n(%s)", it_left_fsync, it_left_abort, + fsync_tries, abort_tries, buf); + + int it_left_fsync_prev = it_left_fsync; + + /* now write again what to do next */ + if (it_left_fsync > 0) + it_left_fsync--; + + if (it_left_fsync == 0 && it_left_abort > 0) { + + it_left_abort--; + + /* announce mode switch */ + if (it_left_fsync_prev != it_left_fsync && it_left_fsync == 0) { + warnx("\n SUCCESSFULLY PASSED FSYNC'ED WRITES, CONTINUTING WITHOUT FSYNC"); + fsync(stdout); + fsync(stderr); + usleep(20000); + } + + } + + if (it_left_abort == 0) { + (void)unlink(cmd_filename); + return 0; + } + + } else { + + /* this must be the first iteration, do something */ + cmd_fd = open(cmd_filename, O_TRUNC | O_WRONLY | O_CREAT); + + warnx("First iteration of file test\n"); + } + + char buf[64]; + int wret = sprintf(buf, "TEST: %d %d ", it_left_fsync, it_left_abort); + lseek(cmd_fd, 0, SEEK_SET); + write(cmd_fd, buf, strlen(buf) + 1); + fsync(cmd_fd); + + /* perform tests for a range of chunk sizes */ + unsigned chunk_sizes[] = {32, 64, 128, 256, 512, 600, 1200}; + + for (unsigned c = 0; c < (sizeof(chunk_sizes) / sizeof(chunk_sizes[0])); c++) { + + printf("\n\n====== FILE TEST: %u bytes chunks (%s) ======\n", chunk_sizes[c], (it_left_fsync > 0) ? "FSYNC" : "NO FSYNC"); + printf("unpower the system immediately (within 0.5s) when the hash (#) sign appears\n"); + fsync(stdout); + fsync(stderr); + usleep(50000); + + for (unsigned a = 0; a < alignments; a++) { + + printf("."); + + uint8_t write_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64))); + + /* fill write buffer with known values */ + for (int i = 0; i < sizeof(write_buf); i++) { + /* this will wrap, but we just need a known value with spacing */ + write_buf[i] = i+11; + } + + uint8_t read_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64))); + hrt_abstime start, end; + + int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); + + start = hrt_absolute_time(); + for (unsigned i = 0; i < iterations; i++) { + + int wret = write(fd, write_buf + a, chunk_sizes[c]); + + if (wret != chunk_sizes[c]) { + warn("WRITE ERROR!"); + + if ((0x3 & (uintptr_t)(write_buf + a))) + warnx("memory is unaligned, align shift: %d", a); + + return 1; + + } + + if (it_left_fsync > 0) { + fsync(fd); + } else { + printf("#"); + fsync(stdout); + fsync(stderr); + } + } + + if (it_left_fsync > 0) { + printf("#"); + } + + printf("."); + fsync(stdout); + fsync(stderr); + usleep(200000); + + end = hrt_absolute_time(); + + close(fd); + fd = open("/fs/microsd/testfile", O_RDONLY); + + /* read back data for validation */ + for (unsigned i = 0; i < iterations; i++) { + int rret = read(fd, read_buf, chunk_sizes[c]); + + if (rret != chunk_sizes[c]) { + warnx("READ ERROR!"); + return 1; + } + + /* compare value */ + bool compare_ok = true; + + for (int j = 0; j < chunk_sizes[c]; j++) { + if (read_buf[j] != write_buf[j + a]) { + warnx("COMPARISON ERROR: byte %d, align shift: %d", j, a); + compare_ok = false; + break; + } + } + + if (!compare_ok) { + warnx("ABORTING FURTHER COMPARISON DUE TO ERROR"); + return 1; + } + + } + + int ret = unlink("/fs/microsd/testfile"); + close(fd); + + if (ret) { + warnx("UNLINKING FILE FAILED"); + return 1; + } + + } + } + + fsync(stdout); + fsync(stderr); + usleep(20000); + + + + /* we always reboot for the next test if we get here */ + warnx("Iteration done, rebooting.."); + fsync(stdout); + fsync(stderr); + usleep(50000); + systemreset(false); + + /* never going to get here */ + return 0; +} diff --git a/src/systemcmds/tests/test_mtd.c b/src/systemcmds/tests/test_mtd.c new file mode 100644 index 0000000000..bac9efbdb3 --- /dev/null +++ b/src/systemcmds/tests/test_mtd.c @@ -0,0 +1,229 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file test_mtd.c + * + * Param storage / file test. + * + * @author Lorenz Meier + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "tests.h" + +#define PARAM_FILE_NAME "/fs/mtd_params" + +static int check_user_abort(int fd); + +int check_user_abort(int fd) { + /* check if user wants to abort */ + char c; + + struct pollfd fds; + int ret; + fds.fd = 0; /* stdin */ + fds.events = POLLIN; + ret = poll(&fds, 1, 0); + + if (ret > 0) { + + read(0, &c, 1); + + switch (c) { + case 0x03: // ctrl-c + case 0x1b: // esc + case 'c': + case 'q': + { + warnx("Test aborted."); + fsync(fd); + close(fd); + return OK; + /* not reached */ + } + } + } + + return 1; +} + +void print_fail() +{ + printf("<[T]: MTD: FAIL>\n"); +} + +void print_success() +{ + printf("<[T]: MTD: OK>\n"); +} + +int +test_mtd(int argc, char *argv[]) +{ + unsigned iterations= 0; + + /* check if microSD card is mounted */ + struct stat buffer; + if (stat(PARAM_FILE_NAME, &buffer)) { + warnx("file %s not found, aborting MTD test", PARAM_FILE_NAME); + print_fail(); + return 1; + } + + // XXX get real storage space here + unsigned file_size = 4096; + + /* perform tests for a range of chunk sizes */ + unsigned chunk_sizes[] = {256, 512, 4096}; + + for (unsigned c = 0; c < (sizeof(chunk_sizes) / sizeof(chunk_sizes[0])); c++) { + + printf("\n====== FILE TEST: %u bytes chunks ======\n", chunk_sizes[c]); + + uint8_t write_buf[chunk_sizes[c]] __attribute__((aligned(64))); + + /* fill write buffer with known values */ + for (int i = 0; i < sizeof(write_buf); i++) { + /* this will wrap, but we just need a known value with spacing */ + write_buf[i] = i+11; + } + + uint8_t read_buf[chunk_sizes[c]] __attribute__((aligned(64))); + hrt_abstime start, end; + + int fd = open(PARAM_FILE_NAME, O_RDONLY); + int rret = read(fd, read_buf, chunk_sizes[c]); + close(fd); + + fd = open(PARAM_FILE_NAME, O_WRONLY); + + printf("printing 2 percent of the first chunk:\n"); + for (int i = 0; i < sizeof(read_buf) / 50; i++) { + printf("%02X", read_buf[i]); + } + printf("\n"); + + iterations = file_size / chunk_sizes[c]; + + start = hrt_absolute_time(); + for (unsigned i = 0; i < iterations; i++) { + int wret = write(fd, write_buf, chunk_sizes[c]); + + if (wret != (int)chunk_sizes[c]) { + warn("WRITE ERROR!"); + print_fail(); + return 1; + } + + fsync(fd); + + if (!check_user_abort(fd)) + return OK; + + } + end = hrt_absolute_time(); + + close(fd); + fd = open(PARAM_FILE_NAME, O_RDONLY); + + /* read back data for validation */ + for (unsigned i = 0; i < iterations; i++) { + int rret = read(fd, read_buf, chunk_sizes[c]); + + if (rret != chunk_sizes[c]) { + warnx("READ ERROR!"); + print_fail(); + return 1; + } + + /* compare value */ + bool compare_ok = true; + + for (int j = 0; j < chunk_sizes[c]; j++) { + if (read_buf[j] != write_buf[j]) { + warnx("COMPARISON ERROR: byte %d", j); + print_fail(); + compare_ok = false; + break; + } + } + + if (!compare_ok) { + warnx("ABORTING FURTHER COMPARISON DUE TO ERROR"); + print_fail(); + return 1; + } + + if (!check_user_abort(fd)) + return OK; + + } + + + close(fd); + + } + + /* fill the file with 0xFF to make it look new again */ + char ffbuf[64]; + memset(ffbuf, 0xFF, sizeof(ffbuf)); + int fd = open(PARAM_FILE_NAME, O_WRONLY); + for (int i = 0; i < file_size / sizeof(ffbuf); i++) { + int ret = write(fd, ffbuf, sizeof(ffbuf)); + + if (ret != sizeof(ffbuf)) { + warnx("ERROR! Could not fill file with 0xFF"); + close(fd); + print_fail(); + return 1; + } + } + + (void)close(fd); + print_success(); + + return 0; +} diff --git a/src/systemcmds/tests/test_rc.c b/src/systemcmds/tests/test_rc.c index 72619fc8ba..6a602ecfc9 100644 --- a/src/systemcmds/tests/test_rc.c +++ b/src/systemcmds/tests/test_rc.c @@ -32,8 +32,8 @@ ****************************************************************************/ /** - * @file test_ppm_loopback.c - * Tests the PWM outputs and PPM input + * @file test_rc.c + * Tests RC input. * */ diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h index a57d04be37..ac64ad33d3 100644 --- a/src/systemcmds/tests/tests.h +++ b/src/systemcmds/tests/tests.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 @@ -109,6 +109,10 @@ extern int test_bson(int argc, char *argv[]); extern int test_file(int argc, char *argv[]); extern int test_mixer(int argc, char *argv[]); extern int test_rc(int argc, char *argv[]); +extern int test_conv(int argc, char *argv[]); +extern int test_mount(int argc, char *argv[]); +extern int test_mtd(int argc, char *argv[]); +extern int test_mathlib(int argc, char *argv[]); __END_DECLS diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index 1088a44076..73827b7cf0 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -106,6 +106,10 @@ const struct { {"file", test_file, 0}, {"mixer", test_mixer, OPT_NOJIGTEST | OPT_NOALLTEST}, {"rc", test_rc, OPT_NOJIGTEST | OPT_NOALLTEST}, + {"conv", test_conv, OPT_NOJIGTEST | OPT_NOALLTEST}, + {"mount", test_mount, OPT_NOJIGTEST | OPT_NOALLTEST}, + {"mtd", test_mtd, 0}, + {"mathlib", test_mathlib, 0}, {"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST}, {NULL, NULL, 0} }; From 1e3d2acbf66b1101a9b17f97d2b786ffaa0e423a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 28 Jan 2014 19:30:23 +0100 Subject: [PATCH 031/381] Not building yet, things are coming together slowly on mavlink app --- src/modules/mavlink/mavlink_main.cpp | 18 +- src/modules/mavlink/mavlink_main.h | 50 +- src/modules/mavlink/mavlink_orb_listener.cpp | 545 ++++++++++--------- src/modules/mavlink/mavlink_orb_listener.h | 94 +++- src/modules/mavlink/mavlink_receiver.cpp | 26 +- src/modules/mavlink/mavlink_receiver.h | 4 +- 6 files changed, 421 insertions(+), 316 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 1c7986cbb8..cd37c5437c 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -56,7 +56,6 @@ #include #include #include -#include #include #include #include @@ -196,6 +195,11 @@ Mavlink::~Mavlink() } } +void Mavlink::set_mode(enum MAVLINK_MODE mode) +{ + _mode = mode; +} + int Mavlink::instance_count() { /* note: a local buffer count will help if this ever is called often */ @@ -1506,7 +1510,7 @@ Mavlink::task_main(int argc, char *argv[]) break; case 'o': - mavlink_link_mode = MAVLINK_INTERFACE_MODE_ONBOARD; + _mode = MODE_ONBOARD; break; default: @@ -1523,7 +1527,7 @@ Mavlink::task_main(int argc, char *argv[]) warnx("MAVLink v1.0 serial interface starting..."); /* inform about mode */ - warnx((mavlink_link_mode == MAVLINK_INTERFACE_MODE_ONBOARD) ? "ONBOARD MODE" : "DOWNLINK MODE"); + warnx((_mode == MODE_ONBOARD) ? "ONBOARD MODE" : "DOWNLINK MODE"); /* Flush stdout in case MAVLink is about to take it over */ fflush(stdout); @@ -1541,12 +1545,12 @@ Mavlink::task_main(int argc, char *argv[]) mavlink_update_system(); /* start the MAVLink receiver */ - MavlinkReceiver rcv(this); - receive_thread = rcv.receive_start(uart); + // MavlinkReceiver rcv(this); + receive_thread = MavlinkReceiver::receive_start(this); /* start the ORB receiver */ - MavlinkOrbListener listener(this); - uorb_receive_thread = listener.uorb_receive_start(); + //MavlinkOrbListener listener(this); + uorb_receive_thread = MavlinkOrbListener::uorb_receive_start(this); /* initialize waypoint manager */ mavlink_wpm_init(wpm); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 244af04a67..3b6714559f 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -54,9 +54,7 @@ #include #include #include -#include #include -#include #include #include #include @@ -72,7 +70,8 @@ #include #include #include - +#include +#include #include @@ -139,6 +138,7 @@ struct mavlink_wpm_storage { class Mavlink { + public: /** * Constructor @@ -170,6 +170,31 @@ public: static int get_uart_fd(unsigned index); + int get_uart_fd() { return _mavlink_fd; } + + enum MAVLINK_MODE { + MODE_TX_HEARTBEAT_ONLY=0, + MODE_OFFBOARD, + MODE_ONBOARD, + MODE_HIL + }; + + void set_mode(enum MAVLINK_MODE); + enum MAVLINK_MODE get_mode(); + + bool hil_enabled() { return _mavlink_hil_enabled; }; + + /** + * Handle waypoint related messages. + */ + void mavlink_wpm_message_handler(const mavlink_message_t *msg); + + /** + * Handle parameter related messages. + */ + void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg); + + struct mavlink_subscriptions { int sensor_sub; int att_sub; @@ -196,10 +221,15 @@ public: int airspeed_sub; int navigation_capabilities_sub; int control_mode_sub; + int rc_sub; + int status_sub; }; struct mavlink_subscriptions subs; + struct mavlink_subscriptions* get_subs() { return &subs; } + mavlink_channel_t get_chan() { return chan; } + /** Global position */ struct vehicle_global_position_s global_pos; /** Local position */ @@ -243,6 +273,7 @@ private: uint8_t missionlib_msg_buf[300]; //XXX MAGIC NUMBER bool thread_running; bool thread_should_exit; + MAVLINK_MODE _mode; uint8_t mavlink_wpm_comp_id; mavlink_channel_t chan; @@ -269,12 +300,6 @@ private: */ unsigned int mavlink_param_queue_index; - /* interface mode */ - enum { - MAVLINK_INTERFACE_MODE_OFFBOARD, - MAVLINK_INTERFACE_MODE_ONBOARD - } mavlink_link_mode; - struct mavlink_logbuffer lb; bool mavlink_link_termination_allowed; @@ -293,12 +318,6 @@ private: */ int set_hil_on_off(bool hil_enabled); - - /** - * Handle parameter related messages. - */ - void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg); - /** * Send all parameters at once. * @@ -352,7 +371,6 @@ private: void mavlink_update_system(); - void mavlink_wpm_message_handler(const mavlink_message_t *msg); void mavlink_waypoint_eventloop(uint64_t now); void mavlink_wpm_send_waypoint_reached(uint16_t seq); void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq); diff --git a/src/modules/mavlink/mavlink_orb_listener.cpp b/src/modules/mavlink/mavlink_orb_listener.cpp index 44bf77bb0d..fa1a6887ef 100644 --- a/src/modules/mavlink/mavlink_orb_listener.cpp +++ b/src/modules/mavlink/mavlink_orb_listener.cpp @@ -62,11 +62,7 @@ void *uorb_receive_thread(void *arg); -struct listener { - void (*callback)(const struct listener *l); - int *subp; - uintptr_t arg; -}; + uint16_t cm_uint16_from_m_float(float m); @@ -86,41 +82,58 @@ cm_uint16_from_m_float(float m) MavlinkOrbListener::MavlinkOrbListener(Mavlink* parent) : - _task_should_exit(false), + thread_should_exit(false), _loop_perf(perf_alloc(PC_ELAPSED, "mavlink orb")), _mavlink(parent), + _listeners(nullptr), _n_listeners(0) { - static const struct listener listeners[] = { - {l_sensor_combined, &mavlink_subs.sensor_sub, 0}, - {l_vehicle_attitude, &mavlink_subs.att_sub, 0}, - {l_vehicle_gps_position, &mavlink_subs.gps_sub, 0}, - {l_vehicle_status, &status_sub, 0}, - {l_rc_channels, &rc_sub, 0}, - {l_input_rc, &mavlink_subs.input_rc_sub, 0}, - {l_global_position, &mavlink_subs.global_pos_sub, 0}, - {l_local_position, &mavlink_subs.local_pos_sub, 0}, - {l_global_position_setpoint, &mavlink_subs.triplet_sub, 0}, - {l_local_position_setpoint, &mavlink_subs.spl_sub, 0}, - {l_attitude_setpoint, &mavlink_subs.spa_sub, 0}, - {l_actuator_outputs, &mavlink_subs.act_0_sub, 0}, - {l_actuator_outputs, &mavlink_subs.act_1_sub, 1}, - {l_actuator_outputs, &mavlink_subs.act_2_sub, 2}, - {l_actuator_outputs, &mavlink_subs.act_3_sub, 3}, - {l_actuator_armed, &mavlink_subs.armed_sub, 0}, - {l_manual_control_setpoint, &mavlink_subs.man_control_sp_sub, 0}, - {l_vehicle_attitude_controls, &mavlink_subs.actuators_sub, 0}, - {l_debug_key_value, &mavlink_subs.debug_key_value, 0}, - {l_optical_flow, &mavlink_subs.optical_flow, 0}, - {l_vehicle_rates_setpoint, &mavlink_subs.rates_setpoint_sub, 0}, - {l_home, &mavlink_subs.home_sub, 0}, - {l_airspeed, &mavlink_subs.airspeed_sub, 0}, - {l_nav_cap, &mavlink_subs.navigation_capabilities_sub, 0}, - {l_control_mode, &mavlink_subs.control_mode_sub, 0}, -}; + add_listener(MavlinkOrbListener::l_sensor_combined, &_mavlink->get_subs()->sensor_sub, 0); + add_listener(MavlinkOrbListener::l_vehicle_attitude, &_mavlink->get_subs()->att_sub, 0); + add_listener(MavlinkOrbListener::l_vehicle_gps_position, &_mavlink->get_subs()->gps_sub, 0); + add_listener(MavlinkOrbListener::l_vehicle_status, &status_sub, 0); + add_listener(MavlinkOrbListener::l_rc_channels, &rc_sub, 0); + add_listener(MavlinkOrbListener::l_input_rc, &_mavlink->get_subs()->input_rc_sub, 0); + add_listener(MavlinkOrbListener::l_global_position, &_mavlink->get_subs()->global_pos_sub, 0); + add_listener(MavlinkOrbListener::l_local_position, &_mavlink->get_subs()->local_pos_sub, 0); + add_listener(MavlinkOrbListener::l_global_position_setpoint, &_mavlink->get_subs()->triplet_sub, 0); + add_listener(MavlinkOrbListener::l_local_position_setpoint, &_mavlink->get_subs()->spl_sub, 0); + add_listener(MavlinkOrbListener::l_attitude_setpoint, &_mavlink->get_subs()->spa_sub, 0); + add_listener(MavlinkOrbListener::l_actuator_outputs, &_mavlink->get_subs()->act_0_sub, 0); + add_listener(MavlinkOrbListener::l_actuator_outputs, &_mavlink->get_subs()->act_1_sub, 1); + add_listener(MavlinkOrbListener::l_actuator_outputs, &_mavlink->get_subs()->act_2_sub, 2); + add_listener(MavlinkOrbListener::l_actuator_outputs, &_mavlink->get_subs()->act_3_sub, 3); + add_listener(MavlinkOrbListener::l_actuator_armed, &_mavlink->get_subs()->armed_sub, 0); + add_listener(MavlinkOrbListener::l_manual_control_setpoint, &_mavlink->get_subs()->man_control_sp_sub, 0); + add_listener(MavlinkOrbListener::l_vehicle_attitude_controls, &_mavlink->get_subs()->actuators_sub, 0); + add_listener(MavlinkOrbListener::l_debug_key_value, &_mavlink->get_subs()->debug_key_value, 0); + add_listener(MavlinkOrbListener::l_optical_flow, &_mavlink->get_subs()->optical_flow, 0); + add_listener(MavlinkOrbListener::l_vehicle_rates_setpoint, &_mavlink->get_subs()->rates_setpoint_sub, 0); + add_listener(MavlinkOrbListener::l_home, &_mavlink->get_subs()->home_sub, 0); + add_listener(MavlinkOrbListener::l_airspeed, &_mavlink->get_subs()->airspeed_sub, 0); + add_listener(MavlinkOrbListener::l_nav_cap, &_mavlink->get_subs()->navigation_capabilities_sub, 0); + add_listener(MavlinkOrbListener::l_control_mode, &_mavlink->get_subs()->control_mode_sub, 0); - _n_listeners = sizeof(listeners) / sizeof(listeners[0]); +} +void MavlinkOrbListener::add_listener(void (*callback)(const struct listener *l), int *subp, uintptr_t arg) +{ + struct listener *nl = new listener; + + nl->callback = callback; + nl->subp = subp; + nl->arg = arg; + nl->next = nullptr; + + // Register it + struct listener *next = _listeners; + while (next->next != nullptr) { + next = next->next; + } + + // Attach + next->next = nl; + _n_listeners++; } void @@ -129,43 +142,37 @@ MavlinkOrbListener::l_sensor_combined(const struct listener *l) struct sensor_combined_s raw; /* copy sensors raw data into local buffer */ - orb_copy(ORB_ID(sensor_combined), mavlink_subs.sensor_sub, &raw); + orb_copy(ORB_ID(sensor_combined), l->mavlink->get_subs()->sensor_sub, &raw); - last_sensor_timestamp = raw.timestamp; - - /* mark individual fields as changed */ + /* mark individual fields as _mavlink->get_chan()ged */ uint16_t fields_updated = 0; - static unsigned accel_counter = 0; - static unsigned gyro_counter = 0; - static unsigned mag_counter = 0; - static unsigned baro_counter = 0; - if (accel_counter != raw.accelerometer_counter) { - /* mark first three dimensions as changed */ - fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); - accel_counter = raw.accelerometer_counter; - } + // if (accel_counter != raw.accelerometer_counter) { + // /* mark first three dimensions as _mavlink->get_chan()ged */ + // fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); + // accel_counter = raw.accelerometer_counter; + // } - if (gyro_counter != raw.gyro_counter) { - /* mark second group dimensions as changed */ - fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); - gyro_counter = raw.gyro_counter; - } + // if (gyro_counter != raw.gyro_counter) { + // /* mark second group dimensions as _mavlink->get_chan()ged */ + // fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); + // gyro_counter = raw.gyro_counter; + // } - if (mag_counter != raw.magnetometer_counter) { - /* mark third group dimensions as changed */ - fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); - mag_counter = raw.magnetometer_counter; - } + // if (mag_counter != raw.magnetometer_counter) { + // /* mark third group dimensions as _mavlink->get_chan()ged */ + // fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); + // mag_counter = raw.magnetometer_counter; + // } - if (baro_counter != raw.baro_counter) { - /* mark last group dimensions as changed */ - fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); - baro_counter = raw.baro_counter; - } + // if (baro_counter != raw.baro_counter) { + // /* mark last group dimensions as _mavlink->get_chan()ged */ + // fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); + // baro_counter = raw.baro_counter; + // } - if (gcs_link) - mavlink_msg_highres_imu_send(_mavlink->get_mavlink_chan(), last_sensor_timestamp, + if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) + mavlink_msg_highres_imu_send(l->mavlink->get_chan(), l->listener->last_sensor_timestamp, raw.accelerometer_m_s2[0], raw.accelerometer_m_s2[1], raw.accelerometer_m_s2[2], raw.gyro_rad_s[0], raw.gyro_rad_s[1], raw.gyro_rad_s[2], @@ -175,51 +182,51 @@ MavlinkOrbListener::l_sensor_combined(const struct listener *l) raw.baro_alt_meter, raw.baro_temp_celcius, fields_updated); - sensors_raw_counter++; + l->listener->sensors_raw_counter++; } void MavlinkOrbListener::l_vehicle_attitude(const struct listener *l) { /* copy attitude data into local buffer */ - orb_copy(ORB_ID(vehicle_attitude), mavlink_subs.att_sub, &att); + orb_copy(ORB_ID(vehicle_attitude), l->mavlink->get_subs()->att_sub, &l->listener->att); - if (gcs_link) { + if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) { /* send sensor values */ - mavlink_msg_attitude_send(_mavlink->get_mavlink_chan(), - last_sensor_timestamp / 1000, - att.roll, - att.pitch, - att.yaw, - att.rollspeed, - att.pitchspeed, - att.yawspeed); + mavlink_msg_attitude_send(l->mavlink->get_chan(), + l->listener->last_sensor_timestamp / 1000, + l->listener->att.roll, + l->listener->att.pitch, + l->listener->att.yaw, + l->listener->att.rollspeed, + l->listener->att.pitchspeed, + l->listener->att.yawspeed); /* limit VFR message rate to 10Hz */ hrt_abstime t = hrt_absolute_time(); - if (t >= last_sent_vfr + 100000) { - last_sent_vfr = t; - float groundspeed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e); - uint16_t heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F; - float throttle = armed.armed ? actuators_0.control[3] * 100.0f : 0.0f; - mavlink_msg_vfr_hud_send(_mavlink->get_mavlink_chan(), airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vel_d); + if (t >= l->listener->last_sent_vfr + 100000) { + l->listener->last_sent_vfr = t; + float groundspeed = sqrtf(l->listener->global_pos.vel_n * l->listener->global_pos.vel_n + l->listener->global_pos.vel_e * l->listener->global_pos.vel_e); + uint16_t heading = _wrap_2pi(l->listener->att.yaw) * M_RAD_TO_DEG_F; + float throttle = l->listener->armed.armed ? l->listener->actuators_0.control[3] * 100.0f : 0.0f; + mavlink_msg_vfr_hud_send(l->mavlink->get_chan(), l->listener->airspeed.true_airspeed_m_s, groundspeed, heading, throttle, l->listener->global_pos.alt, -l->listener->global_pos.vel_d); } /* send quaternion values if it exists */ - if(att.q_valid) { - mavlink_msg_attitude_quaternion_send(_mavlink->get_mavlink_chan(), - last_sensor_timestamp / 1000, - att.q[0], - att.q[1], - att.q[2], - att.q[3], - att.rollspeed, - att.pitchspeed, - att.yawspeed); + if(l->listener->att.q_valid) { + mavlink_msg_attitude_quaternion_send(l->mavlink->get_chan(), + l->listener->last_sensor_timestamp / 1000, + l->listener->att.q[0], + l->listener->att.q[1], + l->listener->att.q[2], + l->listener->att.q[3], + l->listener->att.rollspeed, + l->listener->att.pitchspeed, + l->listener->att.yawspeed); } } - attitude_counter++; + l->listener->attitude_counter++; } void @@ -228,13 +235,13 @@ MavlinkOrbListener::l_vehicle_gps_position(const struct listener *l) struct vehicle_gps_position_s gps; /* copy gps data into local buffer */ - orb_copy(ORB_ID(vehicle_gps_position), mavlink_subs.gps_sub, &gps); + orb_copy(ORB_ID(vehicle_gps_position), l->mavlink->get_subs()->gps_sub, &gps); /* GPS COG is 0..2PI in degrees * 1e2 */ float cog_deg = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F; /* GPS position */ - mavlink_msg_gps_raw_int_send(_mavlink->get_mavlink_chan(), + mavlink_msg_gps_raw_int_send(l->mavlink->get_chan(), gps.timestamp_position, gps.fix_type, gps.lat, @@ -247,8 +254,8 @@ MavlinkOrbListener::l_vehicle_gps_position(const struct listener *l) gps.satellites_visible); /* update SAT info every 10 seconds */ - if (gps.satellite_info_available && (gps_counter % 50 == 0)) { - mavlink_msg_gps_status_send(_mavlink->get_mavlink_chan(), + if (gps.satellite_info_available && (l->listener->gps_counter % 50 == 0)) { + mavlink_msg_gps_status_send(l->mavlink->get_chan(), gps.satellites_visible, gps.satellite_prn, gps.satellite_used, @@ -257,30 +264,30 @@ MavlinkOrbListener::l_vehicle_gps_position(const struct listener *l) gps.satellite_snr); } - gps_counter++; + l->listener->gps_counter++; } void MavlinkOrbListener::l_vehicle_status(const struct listener *l) { - /* immediately communicate state changes back to user */ - orb_copy(ORB_ID(vehicle_status), status_sub, &v_status); - orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed); + /* immediately communicate state _mavlink->get_chan()ges back to user */ + orb_copy(ORB_ID(vehicle_status), l->listener->status_sub, &l->listener->v_status); + orb_copy(ORB_ID(actuator_armed), l->mavlink->get_subs()->armed_sub, &l->listener->armed); /* enable or disable HIL */ - if (v_status.hil_state == HIL_STATE_ON) - set_hil_on_off(true); - else if (v_status.hil_state == HIL_STATE_OFF) - set_hil_on_off(false); + if (l->listener->v_status.hil_state == HIL_STATE_ON) + l->mavlink->set_hil_on_off(true); + else if (l->listener->v_status.hil_state == HIL_STATE_OFF) + l->mavlink->set_hil_on_off(false); /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state = 0; uint8_t mavlink_base_mode = 0; uint32_t mavlink_custom_mode = 0; - get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); + l->mavlink->get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); /* send heartbeat */ - mavlink_msg_heartbeat_send(chan, + mavlink_msg_heartbeat_send(l->mavlink->get_chan(), mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_base_mode, @@ -289,37 +296,37 @@ MavlinkOrbListener::l_vehicle_status(const struct listener *l) } void -MavlinkOrbListener::l_rc_channels(const struct listener *l) +MavlinkOrbListener::l_rc__mavlink->get_chan()nels(const struct listener *l) { - /* copy rc channels into local buffer */ - orb_copy(ORB_ID(rc_channels), rc_sub, &rc); - // XXX Add RC channels scaled message here + /* copy rc _mavlink->get_chan()nels into local buffer */ + orb_copy(ORB_ID(rc__mavlink->get_chan()nels), rc_sub, &rc); + // XXX Add RC _mavlink->get_chan()nels scaled message here } void MavlinkOrbListener::l_input_rc(const struct listener *l) { - /* copy rc channels into local buffer */ - orb_copy(ORB_ID(input_rc), mavlink_subs.input_rc_sub, &rc_raw); + /* copy rc _mavlink->get_chan()nels into local buffer */ + orb_copy(ORB_ID(input_rc), l->mavlink->get_subs()->input_rc_sub, &l->listener->rc_raw); - if (gcs_link) { + if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) { const unsigned port_width = 8; - for (unsigned i = 0; (i * port_width) < (rc_raw.channel_count + port_width); i++) { + for (unsigned i = 0; (i * port_width) < (l->listener->rc_raw.channel_count + port_width); i++) { /* Channels are sent in MAVLink main loop at a fixed interval */ - mavlink_msg_rc_channels_raw_send(chan, - rc_raw.timestamp / 1000, + mavlink_msg_rc_channels_raw_send(_mavlink->get_chan(), + l->listener->rc_raw.timestamp / 1000, i, - (rc_raw.channel_count > (i * port_width) + 0) ? rc_raw.values[(i * port_width) + 0] : UINT16_MAX, - (rc_raw.channel_count > (i * port_width) + 1) ? rc_raw.values[(i * port_width) + 1] : UINT16_MAX, - (rc_raw.channel_count > (i * port_width) + 2) ? rc_raw.values[(i * port_width) + 2] : UINT16_MAX, - (rc_raw.channel_count > (i * port_width) + 3) ? rc_raw.values[(i * port_width) + 3] : UINT16_MAX, - (rc_raw.channel_count > (i * port_width) + 4) ? rc_raw.values[(i * port_width) + 4] : UINT16_MAX, - (rc_raw.channel_count > (i * port_width) + 5) ? rc_raw.values[(i * port_width) + 5] : UINT16_MAX, - (rc_raw.channel_count > (i * port_width) + 6) ? rc_raw.values[(i * port_width) + 6] : UINT16_MAX, - (rc_raw.channel_count > (i * port_width) + 7) ? rc_raw.values[(i * port_width) + 7] : UINT16_MAX, - rc_raw.rssi); + (l->listener->rc_raw.channel_count > (i * port_width) + 0) ? l->listener->rc_raw.values[(i * port_width) + 0] : UINT16_MAX, + (l->listener->rc_raw.channel_count > (i * port_width) + 1) ? l->listener->rc_raw.values[(i * port_width) + 1] : UINT16_MAX, + (l->listener->rc_raw.channel_count > (i * port_width) + 2) ? l->listener->rc_raw.values[(i * port_width) + 2] : UINT16_MAX, + (l->listener->rc_raw.channel_count > (i * port_width) + 3) ? l->listener->rc_raw.values[(i * port_width) + 3] : UINT16_MAX, + (l->listener->rc_raw.channel_count > (i * port_width) + 4) ? l->listener->rc_raw.values[(i * port_width) + 4] : UINT16_MAX, + (l->listener->rc_raw.channel_count > (i * port_width) + 5) ? l->listener->rc_raw.values[(i * port_width) + 5] : UINT16_MAX, + (l->listener->rc_raw.channel_count > (i * port_width) + 6) ? l->listener->rc_raw.values[(i * port_width) + 6] : UINT16_MAX, + (l->listener->rc_raw.channel_count > (i * port_width) + 7) ? l->listener->rc_raw.values[(i * port_width) + 7] : UINT16_MAX, + l->listener->rc_raw.rssi); } } } @@ -328,48 +335,48 @@ void MavlinkOrbListener::l_global_position(const struct listener *l) { /* copy global position data into local buffer */ - orb_copy(ORB_ID(vehicle_global_position), mavlink_subs.global_pos_sub, &global_pos); + orb_copy(ORB_ID(vehicle_global_position), _mavlink->get_subs()->global_pos_sub, l->listener->global_pos); - mavlink_msg_global_position_int_send(_mavlink->get_mavlink_chan(), - global_pos.timestamp / 1000, - global_pos.lat * 1e7, - global_pos.lon * 1e7, - global_pos.alt * 1000.0f, - (global_pos.alt - home.alt) * 1000.0f, - global_pos.vel_n * 100.0f, - global_pos.vel_e * 100.0f, - global_pos.vel_d * 100.0f, - _wrap_2pi(global_pos.yaw) * M_RAD_TO_DEG_F * 100.0f); + mavlink_msg_global_position_int_send(l->mavlink->get_chan(), + l->listener->global_pos.timestamp / 1000, + l->listener->global_pos.lat * 1e7, + l->listener->global_pos.lon * 1e7, + l->listener->global_pos.alt * 1000.0f, + (l->listener->global_pos.alt - l->listener->home.alt) * 1000.0f, + l->listener->global_pos.vel_n * 100.0f, + l->listener->global_pos.vel_e * 100.0f, + l->listener->global_pos.vel_d * 100.0f, + _wrap_2pi(l->listener->global_pos.yaw) * M_RAD_TO_DEG_F * 100.0f); } void MavlinkOrbListener::l_local_position(const struct listener *l) { /* copy local position data into local buffer */ - orb_copy(ORB_ID(vehicle_local_position), mavlink_subs.local_pos_sub, &local_pos); + orb_copy(ORB_ID(vehicle_local_position), _mavlink->get_subs()->local_pos_sub, l->listener->local_pos); - if (gcs_link) - mavlink_msg_local_position_ned_send(_mavlink->get_mavlink_chan(), - local_pos.timestamp / 1000, - local_pos.x, - local_pos.y, - local_pos.z, - local_pos.vx, - local_pos.vy, - local_pos.vz); + if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) + mavlink_msg_local_position_ned_send(l->mavlink->get_chan(), + l->listener->local_pos.timestamp / 1000, + l->listener->local_pos.x, + l->listener->local_pos.y, + l->listener->local_pos.z, + l->listener->local_pos.vx, + l->listener->local_pos.vy, + l->listener->local_pos.vz); } void MavlinkOrbListener::l_global_position_setpoint(const struct listener *l) { struct position_setpoint_triplet_s triplet; - orb_copy(ORB_ID(position_setpoint_triplet), mavlink_subs.triplet_sub, &triplet); + orb_copy(ORB_ID(position_setpoint_triplet), l->mavlink->get_subs()->triplet_sub, &triplet); if (!triplet.current.valid) return; - if (gcs_link) - mavlink_msg_global_position_setpoint_int_send(_mavlink->get_mavlink_chan(), + if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) + mavlink_msg_global_position_setpoint_int_send(l->mavlink->get_chan(), MAV_FRAME_GLOBAL, (int32_t)(triplet.current.lat * 1e7d), (int32_t)(triplet.current.lon * 1e7d), @@ -383,10 +390,10 @@ MavlinkOrbListener::l_local_position_setpoint(const struct listener *l) struct vehicle_local_position_setpoint_s local_sp; /* copy local position data into local buffer */ - orb_copy(ORB_ID(vehicle_local_position_setpoint), mavlink_subs.spl_sub, &local_sp); + orb_copy(ORB_ID(vehicle_local_position_setpoint), l->mavlink->get_subs()->spl_sub, &local_sp); - if (gcs_link) - mavlink_msg_local_position_setpoint_send(_mavlink->get_mavlink_chan(), + if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) + mavlink_msg_local_position_setpoint_send(l->mavlink->get_chan(), MAV_FRAME_LOCAL_NED, local_sp.x, local_sp.y, @@ -400,10 +407,10 @@ MavlinkOrbListener::l_attitude_setpoint(const struct listener *l) struct vehicle_attitude_setpoint_s att_sp; /* copy local position data into local buffer */ - orb_copy(ORB_ID(vehicle_attitude_setpoint), mavlink_subs.spa_sub, &att_sp); + orb_copy(ORB_ID(vehicle_attitude_setpoint), _mavlink->get_subs()->spa_sub, &att_sp); - if (gcs_link) - mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_mavlink->get_mavlink_chan(), + if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) + mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(l->mavlink->get_chan(), att_sp.timestamp / 1000, att_sp.roll_body, att_sp.pitch_body, @@ -417,10 +424,10 @@ MavlinkOrbListener::l_vehicle_rates_setpoint(const struct listener *l) struct vehicle_rates_setpoint_s rates_sp; /* copy local position data into local buffer */ - orb_copy(ORB_ID(vehicle_rates_setpoint), mavlink_subs.rates_setpoint_sub, &rates_sp); + orb_copy(ORB_ID(vehicle_rates_setpoint), l->mavlink->get_subs()->rates_setpoint_sub, &rates_sp); - if (gcs_link) - mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_mavlink->get_mavlink_chan(), + if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) + mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(l->mavlink->get_chan(), rates_sp.timestamp / 1000, rates_sp.roll, rates_sp.pitch, @@ -443,8 +450,8 @@ MavlinkOrbListener::l_actuator_outputs(const struct listener *l) /* copy actuator data into local buffer */ orb_copy(ids[l->arg], *l->subp, &act_outputs); - if (gcs_link) { - mavlink_msg_servo_output_raw_send(_mavlink->get_mavlink_chan(), last_sensor_timestamp / 1000, + if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) { + mavlink_msg_servo_output_raw_send(l->mavlink->get_chan(), l->listener->last_sensor_timestamp / 1000, l->arg /* port number - needs GCS support */, /* QGC has port number support already */ act_outputs.output[0], @@ -457,7 +464,7 @@ MavlinkOrbListener::l_actuator_outputs(const struct listener *l) act_outputs.output[7]); /* only send in HIL mode and only send first group for HIL */ - if (mavlink_hil_enabled && armed.armed && ids[l->arg] == ORB_ID(actuator_outputs_0)) { + if (l->listener->mavlink_hil_enabled && l->listener->armed.armed && ids[l->arg] == ORB_ID(actuator_outputs_0)) { /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state = 0; @@ -470,7 +477,7 @@ MavlinkOrbListener::l_actuator_outputs(const struct listener *l) /* scale / assign outputs depending on system type */ if (mavlink_system.type == MAV_TYPE_QUADROTOR) { - mavlink_msg_hil_controls_send(chan, + mavlink_msg_hil_controls_send(l->mavlink->get_chan(), hrt_absolute_time(), ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, @@ -484,7 +491,7 @@ MavlinkOrbListener::l_actuator_outputs(const struct listener *l) 0); } else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) { - mavlink_msg_hil_controls_send(chan, + mavlink_msg_hil_controls_send(l->mavlink->get_chan(), hrt_absolute_time(), ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, @@ -498,7 +505,7 @@ MavlinkOrbListener::l_actuator_outputs(const struct listener *l) 0); } else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) { - mavlink_msg_hil_controls_send(chan, + mavlink_msg_hil_controls_send(l->mavlink->get_chan(), hrt_absolute_time(), ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, @@ -512,7 +519,7 @@ MavlinkOrbListener::l_actuator_outputs(const struct listener *l) 0); } else { - mavlink_msg_hil_controls_send(chan, + mavlink_msg_hil_controls_send(l->mavlink->get_chan(), hrt_absolute_time(), (act_outputs.output[0] - 1500.0f) / 500.0f, (act_outputs.output[1] - 1500.0f) / 500.0f, @@ -532,7 +539,7 @@ MavlinkOrbListener::l_actuator_outputs(const struct listener *l) void MavlinkOrbListener::l_actuator_armed(const struct listener *l) { - orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed); + orb_copy(ORB_ID(actuator_armed), l->mavlink->get_subs()->armed_sub, &l->listener->armed); } void @@ -541,10 +548,10 @@ MavlinkOrbListener::l_manual_control_setpoint(const struct listener *l) struct manual_control_setpoint_s man_control; /* copy manual control data into local buffer */ - orb_copy(ORB_ID(manual_control_setpoint), mavlink_subs.man_control_sp_sub, &man_control); + orb_copy(ORB_ID(manual_control_setpoint), l->mavlink->get_subs()->man_control_sp_sub, &man_control); - if (gcs_link) - mavlink_msg_manual_control_send(_mavlink->get_mavlink_chan(), + if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) + mavlink_msg_manual_control_send(l->mavlink->get_chan(), mavlink_system.sysid, man_control.roll * 1000, man_control.pitch * 1000, @@ -556,26 +563,26 @@ MavlinkOrbListener::l_manual_control_setpoint(const struct listener *l) void MavlinkOrbListener::l_vehicle_attitude_controls(const struct listener *l) { - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, mavlink_subs.actuators_sub, &actuators_0); + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, l->mavlink->get_subs()->actuators_sub, &l->listener->actuators_0); - if (gcs_link) { + if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) { /* send, add spaces so that string buffer is at least 10 chars long */ - mavlink_msg_named_value_float_send(_mavlink->get_mavlink_chan(), - last_sensor_timestamp / 1000, + mavlink_msg_named_value_float_send(l->mavlink->get_chan(), + l->listener->last_sensor_timestamp / 1000, "ctrl0 ", - actuators_0.control[0]); - mavlink_msg_named_value_float_send(_mavlink->get_mavlink_chan(), - last_sensor_timestamp / 1000, + l->listener->actuators_0.control[0]); + mavlink_msg_named_value_float_send(l->mavlink->get_chan(), + l->listener->last_sensor_timestamp / 1000, "ctrl1 ", - actuators_0.control[1]); - mavlink_msg_named_value_float_send(_mavlink->get_mavlink_chan(), - last_sensor_timestamp / 1000, + l->listener->actuators_0.control[1]); + mavlink_msg_named_value_float_send(l->mavlink->get_chan(), + l->listener->last_sensor_timestamp / 1000, "ctrl2 ", - actuators_0.control[2]); - mavlink_msg_named_value_float_send(_mavlink->get_mavlink_chan(), - last_sensor_timestamp / 1000, + l->listener->actuators_0.control[2]); + mavlink_msg_named_value_float_send(l->mavlink->get_chan(), + l->listener->last_sensor_timestamp / 1000, "ctrl3 ", - actuators_0.control[3]); + l->listener->actuators_0.control[3]); } } @@ -584,13 +591,13 @@ MavlinkOrbListener::l_debug_key_value(const struct listener *l) { struct debug_key_value_s debug; - orb_copy(ORB_ID(debug_key_value), mavlink_subs.debug_key_value, &debug); + orb_copy(ORB_ID(debug_key_value), l->mavlink->get_subs()->debug_key_value, &debug); /* Enforce null termination */ debug.key[sizeof(debug.key) - 1] = '\0'; - mavlink_msg_named_value_float_send(_mavlink->get_mavlink_chan(), - last_sensor_timestamp / 1000, + mavlink_msg_named_value_float_send(l->mavlink->get_chan(), + l->listener->last_sensor_timestamp / 1000, debug.key, debug.value); } @@ -600,52 +607,52 @@ MavlinkOrbListener::l_optical_flow(const struct listener *l) { struct optical_flow_s flow; - orb_copy(ORB_ID(optical_flow), mavlink_subs.optical_flow, &flow); + orb_copy(ORB_ID(optical_flow), l->mavlink->get_subs()->optical_flow, &flow); - mavlink_msg_optical_flow_send(_mavlink->get_mavlink_chan(), flow.timestamp, flow.sensor_id, flow.flow_raw_x, flow.flow_raw_y, + mavlink_msg_optical_flow_send(l->mavlink->get_chan(), flow.timestamp, flow.sensor_id, flow.flow_raw_x, flow.flow_raw_y, flow.flow_comp_x_m, flow.flow_comp_y_m, flow.quality, flow.ground_distance_m); } void MavlinkOrbListener::l_home(const struct listener *l) { - orb_copy(ORB_ID(home_position), mavlink_subs.home_sub, &home); + orb_copy(ORB_ID(home_position), l->mavlink->get_subs()->home_sub, &home); - mavlink_msg_gps_global_origin_send(_mavlink->get_mavlink_chan(), (int32_t)(home.lat*1e7d), (int32_t)(home.lon*1e7d), (int32_t)(home.alt)*1e3f); + mavlink_msg_gps_global_origin_send(l->mavlink->get_chan(), (int32_t)(home.lat*1e7d), (int32_t)(home.lon*1e7d), (int32_t)(home.alt)*1e3f); } void MavlinkOrbListener::l_airspeed(const struct listener *l) { - orb_copy(ORB_ID(airspeed), mavlink_subs.airspeed_sub, &airspeed); + orb_copy(ORB_ID(airspeed), l->mavlink->get_subs()->airspeed_sub, &l->listener->airspeed); } void MavlinkOrbListener::l_nav_cap(const struct listener *l) { - orb_copy(ORB_ID(navigation_capabilities), mavlink_subs.navigation_capabilities_sub, &nav_cap); + orb_copy(ORB_ID(navigation_capabilities), l->mavlink->get_subs()->navigation_capabilities_sub, &l->listener->nav_cap); - mavlink_msg_named_value_float_send(_mavlink->get_mavlink_chan(), + mavlink_msg_named_value_float_send(l->mavlink->get_chan(), hrt_absolute_time() / 1000, "turn dist", - nav_cap.turn_distance); + l->listener->nav_cap.turn_distance); } void MavlinkOrbListener::l_control_mode(const struct listener *l) { - orb_copy(ORB_ID(vehicle_control_mode), mavlink_subs.control_mode_sub, &control_mode); + orb_copy(ORB_ID(vehicle_control_mode), l->mavlink->get_subs()->control_mode_sub, &l->listener->control_mode); /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state = 0; uint8_t mavlink_base_mode = 0; uint32_t mavlink_custom_mode = 0; - get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); + l->mavlink->get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); /* send heartbeat */ - mavlink_msg_heartbeat_send(chan, + mavlink_msg_heartbeat_send(l->mavlink->get_chan(), mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_base_mode, @@ -657,7 +664,9 @@ void * MavlinkOrbListener::uorb_receive_thread(void *arg) { /* Set thread name */ - prctl(PR_SET_NAME, "mavlink_orb_rcv", getpid()); + char buf[32]; + sprintf(buf, "mavlink rcv%d", Mavlink::instance_count()); + prctl(PR_SET_NAME, buf, getpid()); /* * set up poll to block for new data, @@ -670,15 +679,21 @@ MavlinkOrbListener::uorb_receive_thread(void *arg) * * Might want to invoke each listener once to set initial state. */ - struct pollfd fds[_n_listeners]; + struct pollfd fds[_max_listeners]; - for (unsigned i = 0; i < _n_listeners; i++) { - fds[i].fd = *listeners[i].subp; + struct listener* next = _listeners; + unsigned i = 0; + + while (next != nullptr) { + + fds[i].fd = *next->subp; fds[i].events = POLLIN; + next = next->next; + i++; + } /* Invoke callback to set initial state */ //listeners[i].callback(&listener[i]); - } while (!thread_should_exit) { @@ -689,13 +704,19 @@ MavlinkOrbListener::uorb_receive_thread(void *arg) /* silent */ } else if (poll_ret < 0) { - mavlink_missionlib_send_gcs_string("[mavlink] ERROR reading uORB data"); + //mavlink_missionlib_send_gcs_string("[mavlink] ERROR reading uORB data"); } else { - for (unsigned i = 0; i < _n_listeners; i++) { + unsigned i = 0; + struct listener* cb = _listeners; + while (cb != nullptr) { + if (fds[i].revents & POLLIN) - listeners[i].callback(&listeners[i]); + cb->callback(cb); + + cb = cb->next; + i++; } } } @@ -703,107 +724,113 @@ MavlinkOrbListener::uorb_receive_thread(void *arg) return NULL; } -pthread_t -MavlinkOrbListener::uorb_receive_start(void) +void * MavlinkOrbListener::uorb_start_helper(void *context) { + return ((MavlinkOrbListener *)context)->uorb_receive_thread(NULL); +} + +pthread_t +MavlinkOrbListener::uorb_receive_start(Mavlink* mavlink) +{ + MavlinkOrbListener* urcv = new MavlinkOrbListener(mavlink); + /* --- SENSORS RAW VALUE --- */ - mavlink_subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + mavlink->get_subs()->sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); /* rate limit set externally based on interface speed, set a basic default here */ - orb_set_interval(mavlink_subs.sensor_sub, 200); /* 5Hz updates */ + orb_set_interval(mavlink->get_subs()->sensor_sub, 200); /* 5Hz updates */ /* --- ATTITUDE VALUE --- */ - mavlink_subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + mavlink->get_subs()->att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); /* rate limit set externally based on interface speed, set a basic default here */ - orb_set_interval(mavlink_subs.att_sub, 200); /* 5Hz updates */ + orb_set_interval(mavlink->get_subs()->att_sub, 200); /* 5Hz updates */ /* --- GPS VALUE --- */ - mavlink_subs.gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); - orb_set_interval(mavlink_subs.gps_sub, 200); /* 5Hz updates */ + mavlink->get_subs()->gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + orb_set_interval(mavlink->get_subs()->gps_sub, 200); /* 5Hz updates */ /* --- HOME POSITION --- */ - mavlink_subs.home_sub = orb_subscribe(ORB_ID(home_position)); - orb_set_interval(mavlink_subs.home_sub, 1000); /* 1Hz updates */ + mavlink->get_subs()->home_sub = orb_subscribe(ORB_ID(home_position)); + orb_set_interval(mavlink->get_subs()->home_sub, 1000); /* 1Hz updates */ /* --- SYSTEM STATE --- */ - status_sub = orb_subscribe(ORB_ID(vehicle_status)); - orb_set_interval(status_sub, 300); /* max 3.33 Hz updates */ + mavlink->get_subs()->status_sub = orb_subscribe(ORB_ID(vehicle_status)); + orb_set_interval(mavlink->get_subs()->status_sub, 300); /* max 3.33 Hz updates */ /* --- CONTROL MODE --- */ - mavlink_subs.control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - orb_set_interval(mavlink_subs.control_mode_sub, 300); /* max 3.33 Hz updates */ + mavlink->get_subs()->control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + orb_set_interval(mavlink->get_subs()->control_mode_sub, 300); /* max 3.33 Hz updates */ /* --- RC CHANNELS VALUE --- */ - rc_sub = orb_subscribe(ORB_ID(rc_channels)); - orb_set_interval(rc_sub, 100); /* 10Hz updates */ + mavlink->get_subs()->rc_sub = orb_subscribe(ORB_ID(rc_channels)); + orb_set_interval(mavlink->get_subs()->rc_sub, 100); /* 10Hz updates */ /* --- RC RAW VALUE --- */ - mavlink_subs.input_rc_sub = orb_subscribe(ORB_ID(input_rc)); - orb_set_interval(mavlink_subs.input_rc_sub, 100); + mavlink->get_subs()->input_rc_sub = orb_subscribe(ORB_ID(input_rc)); + orb_set_interval(mavlink->get_subs()->input_rc_sub, 100); /* --- GLOBAL POS VALUE --- */ - mavlink_subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - orb_set_interval(mavlink_subs.global_pos_sub, 100); /* 10 Hz active updates */ + mavlink->get_subs()->global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + orb_set_interval(mavlink->get_subs()->global_pos_sub, 100); /* 10 Hz active updates */ /* --- LOCAL POS VALUE --- */ - mavlink_subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); - orb_set_interval(mavlink_subs.local_pos_sub, 1000); /* 1Hz active updates */ + mavlink->get_subs()->local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); + orb_set_interval(mavlink->get_subs()->local_pos_sub, 1000); /* 1Hz active updates */ /* --- GLOBAL SETPOINT VALUE --- */ - mavlink_subs.triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); - orb_set_interval(mavlink_subs.triplet_sub, 2000); /* 0.5 Hz updates */ + mavlink->get_subs()->triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); + orb_set_interval(mavlink->get_subs()->triplet_sub, 2000); /* 0.5 Hz updates */ /* --- LOCAL SETPOINT VALUE --- */ - mavlink_subs.spl_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); - orb_set_interval(mavlink_subs.spl_sub, 2000); /* 0.5 Hz updates */ + mavlink->get_subs()->spl_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); + orb_set_interval(mavlink->get_subs()->spl_sub, 2000); /* 0.5 Hz updates */ /* --- ATTITUDE SETPOINT VALUE --- */ - mavlink_subs.spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - orb_set_interval(mavlink_subs.spa_sub, 2000); /* 0.5 Hz updates */ + mavlink->get_subs()->spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + orb_set_interval(mavlink->get_subs()->spa_sub, 2000); /* 0.5 Hz updates */ /* --- RATES SETPOINT VALUE --- */ - mavlink_subs.rates_setpoint_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); - orb_set_interval(mavlink_subs.rates_setpoint_sub, 2000); /* 0.5 Hz updates */ + mavlink->get_subs()->rates_setpoint_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); + orb_set_interval(mavlink->get_subs()->rates_setpoint_sub, 2000); /* 0.5 Hz updates */ /* --- ACTUATOR OUTPUTS --- */ - mavlink_subs.act_0_sub = orb_subscribe(ORB_ID(actuator_outputs_0)); - mavlink_subs.act_1_sub = orb_subscribe(ORB_ID(actuator_outputs_1)); - mavlink_subs.act_2_sub = orb_subscribe(ORB_ID(actuator_outputs_2)); - mavlink_subs.act_3_sub = orb_subscribe(ORB_ID(actuator_outputs_3)); + mavlink->get_subs()->act_0_sub = orb_subscribe(ORB_ID(actuator_outputs_0)); + mavlink->get_subs()->act_1_sub = orb_subscribe(ORB_ID(actuator_outputs_1)); + mavlink->get_subs()->act_2_sub = orb_subscribe(ORB_ID(actuator_outputs_2)); + mavlink->get_subs()->act_3_sub = orb_subscribe(ORB_ID(actuator_outputs_3)); /* rate limits set externally based on interface speed, set a basic default here */ - orb_set_interval(mavlink_subs.act_0_sub, 100); /* 10Hz updates */ - orb_set_interval(mavlink_subs.act_1_sub, 100); /* 10Hz updates */ - orb_set_interval(mavlink_subs.act_2_sub, 100); /* 10Hz updates */ - orb_set_interval(mavlink_subs.act_3_sub, 100); /* 10Hz updates */ + orb_set_interval(mavlink->get_subs()->act_0_sub, 100); /* 10Hz updates */ + orb_set_interval(mavlink->get_subs()->act_1_sub, 100); /* 10Hz updates */ + orb_set_interval(mavlink->get_subs()->act_2_sub, 100); /* 10Hz updates */ + orb_set_interval(mavlink->get_subs()->act_3_sub, 100); /* 10Hz updates */ /* --- ACTUATOR ARMED VALUE --- */ - mavlink_subs.armed_sub = orb_subscribe(ORB_ID(actuator_armed)); - orb_set_interval(mavlink_subs.armed_sub, 100); /* 10Hz updates */ + mavlink->get_subs()->armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + orb_set_interval(mavlink->get_subs()->armed_sub, 100); /* 10Hz updates */ /* --- MAPPED MANUAL CONTROL INPUTS --- */ - mavlink_subs.man_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + mavlink->get_subs()->man_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); /* rate limits set externally based on interface speed, set a basic default here */ - orb_set_interval(mavlink_subs.man_control_sp_sub, 100); /* 10Hz updates */ + orb_set_interval(mavlink->get_subs()->man_control_sp_sub, 100); /* 10Hz updates */ /* --- ACTUATOR CONTROL VALUE --- */ - mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); - orb_set_interval(mavlink_subs.actuators_sub, 100); /* 10Hz updates */ + mavlink->get_subs()->actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + orb_set_interval(mavlink->get_subs()->actuators_sub, 100); /* 10Hz updates */ /* --- DEBUG VALUE OUTPUT --- */ - mavlink_subs.debug_key_value = orb_subscribe(ORB_ID(debug_key_value)); - orb_set_interval(mavlink_subs.debug_key_value, 100); /* 10Hz updates */ + mavlink->get_subs()->debug_key_value = orb_subscribe(ORB_ID(debug_key_value)); + orb_set_interval(mavlink->get_subs()->debug_key_value, 100); /* 10Hz updates */ /* --- FLOW SENSOR --- */ - mavlink_subs.optical_flow = orb_subscribe(ORB_ID(optical_flow)); - orb_set_interval(mavlink_subs.optical_flow, 200); /* 5Hz updates */ + mavlink->get_subs()->optical_flow = orb_subscribe(ORB_ID(optical_flow)); + orb_set_interval(mavlink->get_subs()->optical_flow, 200); /* 5Hz updates */ /* --- AIRSPEED --- */ - mavlink_subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed)); - orb_set_interval(mavlink_subs.airspeed_sub, 200); /* 5Hz updates */ + mavlink->get_subs()->airspeed_sub = orb_subscribe(ORB_ID(airspeed)); + orb_set_interval(mavlink->get_subs()->airspeed_sub, 200); /* 5Hz updates */ /* --- NAVIGATION CAPABILITIES --- */ - mavlink_subs.navigation_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); - orb_set_interval(mavlink_subs.navigation_capabilities_sub, 500); /* 2Hz updates */ - nav_cap.turn_distance = 0.0f; + mavlink->get_subs()->navigation_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); + orb_set_interval(mavlink->get_subs()->navigation_capabilities_sub, 500); /* 2Hz updates */ /* start the listener loop */ pthread_attr_t uorb_attr; @@ -813,7 +840,7 @@ MavlinkOrbListener::uorb_receive_start(void) pthread_attr_setstacksize(&uorb_attr, 2048); pthread_t thread; - pthread_create(&thread, &uorb_attr, uorb_receive_thread, NULL); + pthread_create(&thread, &uorb_attr, MavlinkOrbListener::uorb_start_helper, urcv); pthread_attr_destroy(&uorb_attr); return thread; diff --git a/src/modules/mavlink/mavlink_orb_listener.h b/src/modules/mavlink/mavlink_orb_listener.h index 29e081b360..3988103bcf 100644 --- a/src/modules/mavlink/mavlink_orb_listener.h +++ b/src/modules/mavlink/mavlink_orb_listener.h @@ -38,8 +38,40 @@ * @author Lorenz Meier */ +#include + #pragma once +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + class Mavlink; class MavlinkOrbListener @@ -67,18 +99,32 @@ public: */ void status(); - pthread_t uorb_receive_start(void); + static pthread_t uorb_receive_start(Mavlink *mavlink); void * uorb_receive_thread(void *arg); + struct listener { + void (*callback)(const struct listener *l); + int *subp; + uintptr_t arg; + struct listener* next; + Mavlink *mavlink; + MavlinkOrbListener* listener; + }; + + void add_listener(void (*callback)(const struct listener *l), int *subp, uintptr_t arg); + static void * uorb_start_helper(void *context); + private: - bool _task_should_exit; /**< if true, sensor task should exit */ + bool thread_should_exit; /**< if true, sensor task should exit */ perf_counter_t _loop_perf; /**< loop performance counter */ Mavlink* _mavlink; + struct listener *_listeners; unsigned _n_listeners; + static const unsigned _max_listeners = 32; /** * Shim for calling task_main from task_create. @@ -90,28 +136,28 @@ private: */ void task_main() __attribute__((noreturn)); - void l_sensor_combined(const struct listener *l); - void l_vehicle_attitude(const struct listener *l); - void l_vehicle_gps_position(const struct listener *l); - void l_vehicle_status(const struct listener *l); - void l_rc_channels(const struct listener *l); - void l_input_rc(const struct listener *l); - void l_global_position(const struct listener *l); - void l_local_position(const struct listener *l); - void l_global_position_setpoint(const struct listener *l); - void l_local_position_setpoint(const struct listener *l); - void l_attitude_setpoint(const struct listener *l); - void l_actuator_outputs(const struct listener *l); - void l_actuator_armed(const struct listener *l); - void l_manual_control_setpoint(const struct listener *l); - void l_vehicle_attitude_controls(const struct listener *l); - void l_debug_key_value(const struct listener *l); - void l_optical_flow(const struct listener *l); - void l_vehicle_rates_setpoint(const struct listener *l); - void l_home(const struct listener *l); - void l_airspeed(const struct listener *l); - void l_nav_cap(const struct listener *l); - void l_control_mode(const struct listener *l); + static void l_sensor_combined(const struct listener *l); + static void l_vehicle_attitude(const struct listener *l); + static void l_vehicle_gps_position(const struct listener *l); + static void l_vehicle_status(const struct listener *l); + static void l_rc_channels(const struct listener *l); + static void l_input_rc(const struct listener *l); + static void l_global_position(const struct listener *l); + static void l_local_position(const struct listener *l); + static void l_global_position_setpoint(const struct listener *l); + static void l_local_position_setpoint(const struct listener *l); + static void l_attitude_setpoint(const struct listener *l); + static void l_actuator_outputs(const struct listener *l); + static void l_actuator_armed(const struct listener *l); + static void l_manual_control_setpoint(const struct listener *l); + static void l_vehicle_attitude_controls(const struct listener *l); + static void l_debug_key_value(const struct listener *l); + static void l_optical_flow(const struct listener *l); + static void l_vehicle_rates_setpoint(const struct listener *l); + static void l_home(const struct listener *l); + static void l_airspeed(const struct listener *l); + static void l_nav_cap(const struct listener *l); + static void l_control_mode(const struct listener *l); struct vehicle_global_position_s global_pos; struct vehicle_local_position_s local_pos; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 4f763c3c68..982d6c1d82 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -83,7 +83,8 @@ __BEGIN_DECLS __END_DECLS -void MavlinkReceiver::MavlinkReceiver() : +MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : + _mavlink(parent), pub_hil_global_pos(-1), pub_hil_local_pos(-1), pub_hil_attitude(-1), @@ -790,7 +791,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) void * MavlinkReceiver::receive_thread(void *arg) { - int uart_fd = *((int *)arg); + int uart_fd = _mavlink->get_uart_fd(); const int timeout = 1000; uint8_t buf[32]; @@ -822,10 +823,10 @@ MavlinkReceiver::receive_thread(void *arg) handle_message(&msg); /* handle packet with waypoint component */ - mavlink_wpm_message_handler(&msg); + _mavlink->mavlink_wpm_message_handler(&msg); /* handle packet with parameter component */ - mavlink_pm_message_handler(MAVLINK_COMM_0, &msg); + _mavlink->mavlink_pm_message_handler(MAVLINK_COMM_0, &msg); } } } @@ -839,15 +840,22 @@ void MavlinkReceiver::print_status() } -pthread_t -MavlinkReceiver::receive_start(int uart) +void * MavlinkReceiver::start_helper(void *context) { + return ((MavlinkReceiver *)context)->receive_thread(NULL); +} + +pthread_t +MavlinkReceiver::receive_start(Mavlink *mavlink) +{ + MavlinkReceiver *rcv = new MavlinkReceiver(mavlink); + pthread_attr_t receiveloop_attr; pthread_attr_init(&receiveloop_attr); // set to non-blocking read - int flags = fcntl(uart, F_GETFL, 0); - fcntl(uart, F_SETFL, flags | O_NONBLOCK); + int flags = fcntl(mavlink->get_uart_fd(), F_GETFL, 0); + fcntl(mavlink->get_uart_fd(), F_SETFL, flags | O_NONBLOCK); struct sched_param param; (void)pthread_attr_getschedparam(&receiveloop_attr, ¶m); @@ -857,7 +865,7 @@ MavlinkReceiver::receive_start(int uart) pthread_attr_setstacksize(&receiveloop_attr, 3000); pthread_t thread; - pthread_create(&thread, &receiveloop_attr, receive_thread, &uart); + pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, rcv); pthread_attr_destroy(&receiveloop_attr); return thread; diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index ea57714d2d..483d91e725 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -96,7 +96,9 @@ public: */ void print_status(); - pthread_t receive_start(int uart); + static pthread_t receive_start(Mavlink* mavlink); + + static void * start_helper(void *context); private: From 63b18399c26acb1e3cf771376c3376b4d00a407a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 28 Jan 2014 21:05:00 +0100 Subject: [PATCH 032/381] Butchered MAVLink C++ app to compile and link - there is no hope it will work out of the box 8) --- src/modules/mavlink/mavlink_bridge_header.h | 6 +- src/modules/mavlink/mavlink_main.cpp | 124 ++++++++++--------- src/modules/mavlink/mavlink_main.h | 42 +++---- src/modules/mavlink/mavlink_orb_listener.cpp | 24 ++-- src/modules/mavlink/mavlink_orb_listener.h | 1 + src/modules/mavlink/mavlink_receiver.cpp | 5 +- src/modules/mavlink/mavlink_receiver.h | 2 +- src/modules/mavlink/module.mk | 3 +- 8 files changed, 106 insertions(+), 101 deletions(-) diff --git a/src/modules/mavlink/mavlink_bridge_header.h b/src/modules/mavlink/mavlink_bridge_header.h index 1fae3ee9d0..374d1511c1 100644 --- a/src/modules/mavlink/mavlink_bridge_header.h +++ b/src/modules/mavlink/mavlink_bridge_header.h @@ -42,6 +42,8 @@ #ifndef MAVLINK_BRIDGE_HEADER_H #define MAVLINK_BRIDGE_HEADER_H +__BEGIN_DECLS + #define MAVLINK_USE_CONVENIENCE_FUNCTIONS /* use efficient approach, see mavlink_helpers.h */ @@ -72,11 +74,13 @@ extern mavlink_system_t mavlink_system; * @param chan MAVLink channel to use, usually MAVLINK_COMM_0 = UART0 * @param ch Character to send */ -extern void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int length); +void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int length); extern mavlink_status_t *mavlink_get_channel_status(uint8_t chan); extern mavlink_message_t *mavlink_get_channel_buffer(uint8_t chan); #include +__END_DECLS + #endif /* MAVLINK_BRIDGE_HEADER_H */ diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index cd37c5437c..6c04d2aba0 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -97,6 +97,8 @@ #endif static const int ERROR = -1; +static Mavlink* _head = nullptr; + /** * mavlink app start / stop handling function * @@ -203,10 +205,10 @@ void Mavlink::set_mode(enum MAVLINK_MODE mode) int Mavlink::instance_count() { /* note: a local buffer count will help if this ever is called often */ - Mavlink* inst = _head; + Mavlink* inst = ::_head; unsigned inst_index = 0; - while (inst->_head != nullptr) { - inst = inst->_head; + while (inst->_next != nullptr) { + inst = inst->_next; inst_index++; } @@ -216,22 +218,22 @@ int Mavlink::instance_count() Mavlink* Mavlink::new_instance(const char *port, unsigned baud_rate) { Mavlink* inst = new Mavlink(port, baud_rate); - Mavlink* parent = _head; - while (parent->_head != nullptr) - parent = parent->_head; + Mavlink* parent = ::_head; + while (parent->_next != nullptr) + parent = parent->_next; /* now parent points to a null pointer, fill it */ - parent->_head = inst; + parent->_next = inst; return inst; } Mavlink* Mavlink::get_instance(unsigned instance) { - Mavlink* inst = _head; + Mavlink* inst = ::_head; unsigned inst_index = 0; - while (inst->_head != nullptr && inst_index < instance) { - inst = inst->_head; + while (inst->_next != nullptr && inst_index < instance) { + inst = inst->_next; inst_index++; } @@ -428,9 +430,9 @@ Mavlink::set_hil_on_off(bool hil_enabled) int ret = OK; /* Enable HIL */ - if (hil_enabled && !mavlink_hil_enabled) { + if (hil_enabled && !_mavlink_hil_enabled) { - mavlink_hil_enabled = true; + _mavlink_hil_enabled = true; /* ramp up some HIL-related subscriptions */ unsigned hil_rate_interval; @@ -456,8 +458,8 @@ Mavlink::set_hil_on_off(bool hil_enabled) set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, hil_rate_interval); } - if (!hil_enabled && mavlink_hil_enabled) { - mavlink_hil_enabled = false; + if (!hil_enabled && _mavlink_hil_enabled) { + _mavlink_hil_enabled = false; orb_set_interval(subs.spa_sub, 200); } else { @@ -1426,12 +1428,6 @@ Mavlink::mavlink_missionlib_send_gcs_string(const char *string) } } -void -Mavlink::task_main_trampoline(int argc, char *argv[]) -{ - mavlink::g_mavlink->task_main(argc, argv); -} - int Mavlink::task_main(int argc, char *argv[]) { @@ -1481,43 +1477,43 @@ Mavlink::task_main(int argc, char *argv[]) /* initialize mavlink text message buffering */ - mavlink_logbuffer_init(&lb, 10); + // mavlink_logbuffer_init(&lb, 10); int ch; char *device_name = "/dev/ttyS1"; baudrate = 57600; - /* work around some stupidity in task_create's argv handling */ - argc -= 2; - argv += 2; + // /* work around some stupidity in task_create's argv handling */ + // argc -= 2; + // argv += 2; - while ((ch = getopt(argc, argv, "b:d:eo")) != EOF) { - switch (ch) { - case 'b': - baudrate = strtoul(optarg, NULL, 10); + // while ((ch = getopt(argc, argv, "b:d:eo")) != EOF) { + // switch (ch) { + // case 'b': + // baudrate = strtoul(optarg, NULL, 10); - if (baudrate < 9600 || baudrate > 921600) - errx(1, "invalid baud rate '%s'", optarg); + // if (baudrate < 9600 || baudrate > 921600) + // errx(1, "invalid baud rate '%s'", optarg); - break; + // break; - case 'd': - device_name = optarg; - break; + // case 'd': + // device_name = optarg; + // break; - case 'e': - mavlink_link_termination_allowed = true; - break; + // case 'e': + // mavlink_link_termination_allowed = true; + // break; - case 'o': - _mode = MODE_ONBOARD; - break; + // case 'o': + // _mode = MODE_ONBOARD; + // break; - default: - usage(); - break; - } - } + // default: + // usage(); + // break; + // } + // } struct termios uart_config_original; @@ -1699,15 +1695,15 @@ Mavlink::task_main(int argc, char *argv[]) /* sleep 10 ms */ usleep(10000); - /* send one string at 10 Hz */ - if (!mavlink_logbuffer_is_empty(&lb)) { - struct mavlink_logmessage msg; - int lb_ret = mavlink_logbuffer_read(&lb, &msg); + // /* send one string at 10 Hz */ + // if (!mavlink_logbuffer_is_empty(&lb)) { + // struct mavlink_logmessage msg; + // int lb_ret = mavlink_logbuffer_read(&lb, &msg); - if (lb_ret == OK) { - mavlink_missionlib_send_gcs_string(msg.text); - } - } + // if (lb_ret == OK) { + // mavlink_missionlib_send_gcs_string(msg.text); + // } + // } /* sleep 15 ms */ usleep(15000); @@ -1742,27 +1738,32 @@ Mavlink::task_main(int argc, char *argv[]) _exit(0); } -int -Mavlink::start() +int Mavlink::start_helper(int argc, char *argv[]) +{ + // This is beyond evil.. and needs a lock to be safe + return Mavlink::get_instance(Mavlink::instance_count() - 1)->task_main(argc, argv); +} + +int +Mavlink::start(Mavlink* mavlink) { - ASSERT(_mavlink_task == -1); /* start the task */ char buf[32]; sprintf(buf, "mavlink if%d", Mavlink::instance_count()); - _mavlink_task = task_spawn_cmd(buf, + mavlink->_mavlink_task = task_spawn_cmd(buf, SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2048, - (main_t)&Mavlink::task_main_trampoline, - nullptr); + (main_t)&Mavlink::start_helper, + NULL); // while (!this->is_running()) { // usleep(200); // } - if (_mavlink_task < 0) { + if (mavlink->_mavlink_task < 0) { warn("task start failed"); return -errno; } @@ -1794,6 +1795,9 @@ int mavlink_main(int argc, char *argv[]) if (mavlink::g_mavlink == nullptr) mavlink::g_mavlink = instance; + // Instantiate thread + Mavlink::start(instance); + // if (mavlink::g_mavlink != nullptr) { // errx(1, "already running"); // } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 3b6714559f..e50b0f0c0f 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -155,7 +155,7 @@ public: * * @return OK on success. */ - int start(); + static int start(Mavlink* mavlink); /** * Display the mavlink status. @@ -170,7 +170,7 @@ public: static int get_uart_fd(unsigned index); - int get_uart_fd() { return _mavlink_fd; } + int get_uart_fd() { return uart; } enum MAVLINK_MODE { MODE_TX_HEARTBEAT_ONLY=0, @@ -180,7 +180,7 @@ public: }; void set_mode(enum MAVLINK_MODE); - enum MAVLINK_MODE get_mode(); + enum MAVLINK_MODE get_mode() { return _mode; } bool hil_enabled() { return _mavlink_hil_enabled; }; @@ -189,11 +189,24 @@ public: */ void mavlink_wpm_message_handler(const mavlink_message_t *msg); + static int start_helper(int argc, char *argv[]); + /** * Handle parameter related messages. */ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg); + void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode); + + /** + * Enable / disable Hardware in the Loop simulation mode. + * + * @param hil_enabled The new HIL enable/disable state. + * @return OK if the HIL state changed, ERROR if the + * requested change could not be made or was + * redundant. + */ + int set_hil_on_off(bool hil_enabled); struct mavlink_subscriptions { int sensor_sub; @@ -217,12 +230,13 @@ public: int input_rc_sub; int optical_flow; int rates_setpoint_sub; - int home_sub; + int get_sub; int airspeed_sub; int navigation_capabilities_sub; int control_mode_sub; int rc_sub; int status_sub; + int home_sub; }; struct mavlink_subscriptions subs; @@ -252,6 +266,7 @@ protected: */ struct file_operations fops; int _mavlink_fd; + Mavlink* _next; private: @@ -264,7 +279,6 @@ private: /* states */ bool _mavlink_hil_enabled; /**< Hardware in the loop mode */ - static Mavlink* _head; int _params_sub; @@ -289,7 +303,6 @@ private: mavlink_wpm_storage *wpm; bool verbose; - bool mavlink_hil_enabled; int uart; int baudrate; bool gcs_link; @@ -308,16 +321,6 @@ private: */ void parameters_update(); - /** - * Enable / disable Hardware in the Loop simulation mode. - * - * @param hil_enabled The new HIL enable/disable state. - * @return OK if the HIL state changed, ERROR if the - * requested change could not be made or was - * redundant. - */ - int set_hil_on_off(bool hil_enabled); - /** * Send all parameters at once. * @@ -390,8 +393,6 @@ private: int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval); - void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode); - /** * Callback for param interface. */ @@ -399,11 +400,6 @@ private: static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); - /** - * Shim for calling task_main from task_create. - */ - void task_main_trampoline(int argc, char *argv[]); - /** * Main mavlink task. */ diff --git a/src/modules/mavlink/mavlink_orb_listener.cpp b/src/modules/mavlink/mavlink_orb_listener.cpp index fa1a6887ef..a6bcd4c0a2 100644 --- a/src/modules/mavlink/mavlink_orb_listener.cpp +++ b/src/modules/mavlink/mavlink_orb_listener.cpp @@ -296,11 +296,11 @@ MavlinkOrbListener::l_vehicle_status(const struct listener *l) } void -MavlinkOrbListener::l_rc__mavlink->get_chan()nels(const struct listener *l) +MavlinkOrbListener::l_rc_channels(const struct listener *l) { - /* copy rc _mavlink->get_chan()nels into local buffer */ - orb_copy(ORB_ID(rc__mavlink->get_chan()nels), rc_sub, &rc); - // XXX Add RC _mavlink->get_chan()nels scaled message here + /* copy rc channels into local buffer */ + orb_copy(ORB_ID(rc_channels), l->mavlink->get_subs()->rc_sub, &l->listener->rc); + // XXX Add RC channels scaled message here } void @@ -315,7 +315,7 @@ MavlinkOrbListener::l_input_rc(const struct listener *l) for (unsigned i = 0; (i * port_width) < (l->listener->rc_raw.channel_count + port_width); i++) { /* Channels are sent in MAVLink main loop at a fixed interval */ - mavlink_msg_rc_channels_raw_send(_mavlink->get_chan(), + mavlink_msg_rc_channels_raw_send(l->mavlink->get_chan(), l->listener->rc_raw.timestamp / 1000, i, (l->listener->rc_raw.channel_count > (i * port_width) + 0) ? l->listener->rc_raw.values[(i * port_width) + 0] : UINT16_MAX, @@ -335,7 +335,7 @@ void MavlinkOrbListener::l_global_position(const struct listener *l) { /* copy global position data into local buffer */ - orb_copy(ORB_ID(vehicle_global_position), _mavlink->get_subs()->global_pos_sub, l->listener->global_pos); + orb_copy(ORB_ID(vehicle_global_position), l->mavlink->get_subs()->global_pos_sub, &l->listener->global_pos); mavlink_msg_global_position_int_send(l->mavlink->get_chan(), l->listener->global_pos.timestamp / 1000, @@ -353,7 +353,7 @@ void MavlinkOrbListener::l_local_position(const struct listener *l) { /* copy local position data into local buffer */ - orb_copy(ORB_ID(vehicle_local_position), _mavlink->get_subs()->local_pos_sub, l->listener->local_pos); + orb_copy(ORB_ID(vehicle_local_position), l->mavlink->get_subs()->local_pos_sub, &l->listener->local_pos); if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) mavlink_msg_local_position_ned_send(l->mavlink->get_chan(), @@ -407,7 +407,7 @@ MavlinkOrbListener::l_attitude_setpoint(const struct listener *l) struct vehicle_attitude_setpoint_s att_sp; /* copy local position data into local buffer */ - orb_copy(ORB_ID(vehicle_attitude_setpoint), _mavlink->get_subs()->spa_sub, &att_sp); + orb_copy(ORB_ID(vehicle_attitude_setpoint), l->mavlink->get_subs()->spa_sub, &att_sp); if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(l->mavlink->get_chan(), @@ -464,13 +464,13 @@ MavlinkOrbListener::l_actuator_outputs(const struct listener *l) act_outputs.output[7]); /* only send in HIL mode and only send first group for HIL */ - if (l->listener->mavlink_hil_enabled && l->listener->armed.armed && ids[l->arg] == ORB_ID(actuator_outputs_0)) { + if (l->mavlink->hil_enabled() && l->listener->armed.armed && ids[l->arg] == ORB_ID(actuator_outputs_0)) { /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state = 0; uint8_t mavlink_base_mode = 0; uint32_t mavlink_custom_mode = 0; - get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); + l->mavlink->get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); /* HIL message as per MAVLink spec */ @@ -616,9 +616,9 @@ MavlinkOrbListener::l_optical_flow(const struct listener *l) void MavlinkOrbListener::l_home(const struct listener *l) { - orb_copy(ORB_ID(home_position), l->mavlink->get_subs()->home_sub, &home); + orb_copy(ORB_ID(home_position), l->mavlink->get_subs()->home_sub, &l->listener->home); - mavlink_msg_gps_global_origin_send(l->mavlink->get_chan(), (int32_t)(home.lat*1e7d), (int32_t)(home.lon*1e7d), (int32_t)(home.alt)*1e3f); + mavlink_msg_gps_global_origin_send(l->mavlink->get_chan(), (int32_t)(l->listener->home.lat*1e7d), (int32_t)(l->listener->home.lon*1e7d), (int32_t)(l->listener->home.alt)*1e3f); } void diff --git a/src/modules/mavlink/mavlink_orb_listener.h b/src/modules/mavlink/mavlink_orb_listener.h index 3988103bcf..c9f35a1fbd 100644 --- a/src/modules/mavlink/mavlink_orb_listener.h +++ b/src/modules/mavlink/mavlink_orb_listener.h @@ -170,6 +170,7 @@ private: struct actuator_controls_s actuators_0; struct vehicle_attitude_s att; struct airspeed_s airspeed; + struct home_position_s home; int status_sub; int rc_sub; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 982d6c1d82..0f1d5293ef 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -106,7 +106,8 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : telemetry_status_pub(-1), lat0(0), lon0(0), - alt0(0) + alt0(0), + thread_should_exit(false) { } @@ -818,7 +819,7 @@ MavlinkReceiver::receive_thread(void *arg) /* if read failed, this loop won't execute */ for (ssize_t i = 0; i < nread; i++) { - if (mavlink_parse_char(chan, buf[i], &msg, &status)) { + if (mavlink_parse_char(_mavlink->get_chan(), buf[i], &msg, &status)) { /* handle generic messages and commands */ handle_message(&msg); diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 483d91e725..be32ce0f74 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -102,7 +102,7 @@ public: private: - bool _task_should_exit; /**< if true, sensor task should exit */ + bool thread_should_exit; /**< if true, sensor task should exit */ perf_counter_t _loop_perf; /**< loop performance counter */ diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index 2a005565e0..76798eb124 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -39,7 +39,6 @@ MODULE_COMMAND = mavlink SRCS += mavlink_main.cpp \ mavlink.c \ mavlink_receiver.cpp \ - mavlink_orb_listener.cpp \ - waypoints.cpp + mavlink_orb_listener.cpp INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink From a937e972b059bfc6f6ec99fd5bcf064d89d29acb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 31 Jan 2014 09:53:48 +0100 Subject: [PATCH 033/381] Updated to latest estimator version --- .../fw_att_pos_estimator/estimator.cpp | 218 +++++++++--------- src/modules/fw_att_pos_estimator/estimator.h | 3 + 2 files changed, 114 insertions(+), 107 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index 388cea8646..c7c9b6476d 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -20,16 +20,9 @@ Vector3f dVelIMU; Vector3f dAngIMU; float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec) float dt; // time lapsed since last covariance prediction -bool onGround = true; // boolean true when the flight vehicle is on the ground (not flying) -bool useAirspeed = true; // boolean true if airspeed data is being used -bool useCompass = true; // boolean true if magnetometer data is being used uint8_t fusionModeGPS = 0; // 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 -bool fuseVelData = false; // this boolean causes the posNE and velNED obs to be fused -bool fusePosData = false; // this boolean causes the posNE and velNED obs to be fused -bool fuseHgtData = false; // this boolean causes the hgtMea obs to be fused -bool fuseMagData = false; // boolean true when magnetometer data is to be fused float velNED[3]; // North, East, Down velocity obs (m/s) float posNE[2]; // North, East position obs (m) @@ -45,7 +38,6 @@ Vector3f magData; // magnetometer flux radings in X,Y,Z body axes float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time float innovVtas; // innovation output float varInnovVtas; // innovation variance output -bool fuseVtasData = false; // boolean true when airspeed data is to be fused float VtasMeas; // true airspeed measurement (m/s) float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time float latRef; // WGS-84 latitude of reference point (rad) @@ -64,9 +56,20 @@ float gpsLon; float gpsHgt; uint8_t GPSstatus; +// Baro input +float baroHgt; bool statesInitialised = false; +bool fuseVelData = false; // this boolean causes the posNE and velNED obs to be fused +bool fusePosData = false; // this boolean causes the posNE and velNED obs to be fused +bool fuseHgtData = false; // this boolean causes the hgtMea obs to be fused +bool fuseMagData = false; // boolean true when magnetometer data is to be fused +bool fuseVtasData = false; // boolean true when airspeed data is to be fused + +bool onGround = true; // boolean true when the flight vehicle is on the ground (not flying) +bool useAirspeed = true; // boolean true if airspeed data is being used +bool useCompass = true; // boolean true if magnetometer data is being used float Vector3f::length(void) const { @@ -182,23 +185,23 @@ void UpdateStrapdownEquationsNED() float deltaQuat[4]; static float lastVelocity[3]; const Vector3f gravityNED = {0.0,0.0,GRAVITY_MSS}; - - // Remove sensor bias errors + +// Remove sensor bias errors correctedDelAng.x = dAngIMU.x - states[10]; correctedDelAng.y = dAngIMU.y - states[11]; correctedDelAng.z = dAngIMU.z - states[12]; dVelIMU.x = dVelIMU.x; dVelIMU.y = dVelIMU.y; dVelIMU.z = dVelIMU.z; - - // Save current measurements + +// Save current measurements prevDelAng = correctedDelAng; - - // Apply corrections for earths rotation rate and coning errors - // * and + operators have been overloaded + +// Apply corrections for earths rotation rate and coning errors +// * and + operators have been overloaded correctedDelAng = correctedDelAng - Tnb*earthRateNED*dtIMU + 8.333333333333333e-2f*(prevDelAng % correctedDelAng); - - // Convert the rotation vector to its equivalent quaternion + +// Convert the rotation vector to its equivalent quaternion rotationMag = correctedDelAng.length(); if (rotationMag < 1e-12f) { @@ -215,15 +218,15 @@ void UpdateStrapdownEquationsNED() deltaQuat[2] = correctedDelAng.y*rotScaler; deltaQuat[3] = correctedDelAng.z*rotScaler; } - - // Update the quaternions by rotating from the previous attitude through - // the delta angle rotation quaternion + +// Update the quaternions by rotating from the previous attitude through +// the delta angle rotation quaternion qUpdated[0] = states[0]*deltaQuat[0] - states[1]*deltaQuat[1] - states[2]*deltaQuat[2] - states[3]*deltaQuat[3]; qUpdated[1] = states[0]*deltaQuat[1] + states[1]*deltaQuat[0] + states[2]*deltaQuat[3] - states[3]*deltaQuat[2]; qUpdated[2] = states[0]*deltaQuat[2] + states[2]*deltaQuat[0] + states[3]*deltaQuat[1] - states[1]*deltaQuat[3]; qUpdated[3] = states[0]*deltaQuat[3] + states[3]*deltaQuat[0] + states[1]*deltaQuat[2] - states[2]*deltaQuat[1]; - - // Normalise the quaternions and update the quaternion states + +// Normalise the quaternions and update the quaternion states quatMag = sqrt(sq(qUpdated[0]) + sq(qUpdated[1]) + sq(qUpdated[2]) + sq(qUpdated[3])); if (quatMag > 1e-16f) { @@ -233,8 +236,8 @@ void UpdateStrapdownEquationsNED() states[2] = quatMagInv*qUpdated[2]; states[3] = quatMagInv*qUpdated[3]; } - - // Calculate the body to nav cosine matrix + +// Calculate the body to nav cosine matrix q00 = sq(states[0]); q11 = sq(states[1]); q22 = sq(states[2]); @@ -245,7 +248,7 @@ void UpdateStrapdownEquationsNED() q12 = states[1]*states[2]; q13 = states[1]*states[3]; q23 = states[2]*states[3]; - + Tbn.x.x = q00 + q11 - q22 - q33; Tbn.y.y = q00 - q11 + q22 - q33; Tbn.z.z = q00 - q11 - q22 + q33; @@ -255,35 +258,35 @@ void UpdateStrapdownEquationsNED() Tbn.y.z = 2*(q23 - q01); Tbn.z.x = 2*(q13 - q02); Tbn.z.y = 2*(q23 + q01); - + Tnb = Tbn.transpose(); - - // transform body delta velocities to delta velocities in the nav frame - // * and + operators have been overloaded + +// transform body delta velocities to delta velocities in the nav frame +// * and + operators have been overloaded //delVelNav = Tbn*dVelIMU + gravityNED*dtIMU; delVelNav.x = Tbn.x.x*dVelIMU.x + Tbn.x.y*dVelIMU.y + Tbn.x.z*dVelIMU.z + gravityNED.x*dtIMU; delVelNav.y = Tbn.y.x*dVelIMU.x + Tbn.y.y*dVelIMU.y + Tbn.y.z*dVelIMU.z + gravityNED.y*dtIMU; delVelNav.z = Tbn.z.x*dVelIMU.x + Tbn.z.y*dVelIMU.y + Tbn.z.z*dVelIMU.z + gravityNED.z*dtIMU; - - // calculate the magnitude of the nav acceleration (required for GPS - // variance estimation) + +// calculate the magnitude of the nav acceleration (required for GPS +// variance estimation) accNavMag = delVelNav.length()/dtIMU; - - // If calculating position save previous velocity + +// If calculating position save previous velocity lastVelocity[0] = states[4]; lastVelocity[1] = states[5]; lastVelocity[2] = states[6]; - - // Sum delta velocities to get velocity + +// Sum delta velocities to get velocity states[4] = states[4] + delVelNav.x; states[5] = states[5] + delVelNav.y; states[6] = states[6] + delVelNav.z; - - // If calculating postions, do a trapezoidal integration for position + +// If calculating postions, do a trapezoidal integration for position states[7] = states[7] + 0.5f*(states[4] + lastVelocity[0])*dtIMU; states[8] = states[8] + 0.5f*(states[5] + lastVelocity[1])*dtIMU; states[9] = states[9] + 0.5f*(states[6] + lastVelocity[2])*dtIMU; - + } void CovariancePrediction() @@ -313,15 +316,15 @@ void CovariancePrediction() float dax_b; float day_b; float daz_b; - + // arrays - float processNoise[n_states]; + float processNoise[21]; float SF[14]; float SG[8]; float SQ[11]; float SPP[13]; - float nextP[n_states][n_states]; - + float nextP[21][21]; + // calculate covariance prediction process noise const float yawVarScale = 1.0f; windVelSigma = dt*0.1f; @@ -335,7 +338,7 @@ void CovariancePrediction() for (uint8_t i=15; i<=17; i++) processNoise[i] = magEarthSigma; for (uint8_t i=18; i<=20; i++) processNoise[i] = magBodySigma; for (uint8_t i= 0; i<=20; i++) processNoise[i] = sq(processNoise[i]); - + // set variables used to calculate covariance growth dvx = summedDelVel.x; dvy = summedDelVel.y; @@ -357,7 +360,7 @@ void CovariancePrediction() dvxCov = sq(dt*0.5f); dvyCov = sq(dt*0.5f); dvzCov = sq(dt*0.5f); - + // Predicted covariance calculation SF[0] = 2*dvx*q1 + 2*dvy*q2 + 2*dvz*q3; SF[1] = 2*dvx*q3 + 2*dvy*q0 - 2*dvz*q1; @@ -373,7 +376,7 @@ void CovariancePrediction() SF[11] = q3/2; SF[12] = 2*dvz*q0; SF[13] = 2*dvy*q1; - + SG[0] = q0/2; SG[1] = sq(q3); SG[2] = sq(q2); @@ -382,7 +385,7 @@ void CovariancePrediction() SG[5] = 2*q2*q3; SG[6] = 2*q1*q3; SG[7] = 2*q1*q2; - + SQ[0] = dvzCov*(SG[5] - 2*q0*q1)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvyCov*(SG[5] + 2*q0*q1)*(SG[1] - SG[2] + SG[3] - SG[4]) + dvxCov*(SG[6] - 2*q0*q2)*(SG[7] + 2*q0*q3); SQ[1] = dvzCov*(SG[6] + 2*q0*q2)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvxCov*(SG[6] - 2*q0*q2)*(SG[1] + SG[2] - SG[3] - SG[4]) + dvyCov*(SG[5] + 2*q0*q1)*(SG[7] - 2*q0*q3); SQ[2] = dvzCov*(SG[5] - 2*q0*q1)*(SG[6] + 2*q0*q2) - dvyCov*(SG[7] - 2*q0*q3)*(SG[1] - SG[2] + SG[3] - SG[4]) - dvxCov*(SG[7] + 2*q0*q3)*(SG[1] + SG[2] - SG[3] - SG[4]); @@ -394,7 +397,7 @@ void CovariancePrediction() SQ[8] = (dayCov*q2*q3)/4 - (daxCov*q1*SG[0])/2 - (dazCov*q2*q3)/4; SQ[9] = sq(SG[0]); SQ[10] = sq(q1); - + SPP[0] = SF[12] + SF[13] - 2*dvx*q2; SPP[1] = 2*dvx*q0 - 2*dvy*q3 + 2*dvz*q2; SPP[2] = 2*dvx*q3 + 2*dvy*q0 - 2*dvz*q1; @@ -403,7 +406,7 @@ void CovariancePrediction() SPP[5] = SF[9]; SPP[6] = SF[7]; SPP[7] = SF[8]; - + nextP[0][0] = P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3] + (daxCov*SQ[10])/4 + SF[6]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) + SPP[7]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SPP[6]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[5]*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]) + SPP[4]*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]) + SPP[3]*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]) + (dayCov*sq(q2))/4 + (dazCov*sq(q3))/4; nextP[0][1] = P[0][1] + SQ[8] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3] + SF[5]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[4]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SPP[7]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[3]*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]) - SPP[4]*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]) - (q0*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]))/2; nextP[0][2] = P[0][2] + SQ[7] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3] + SF[3]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[5]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[6]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) - SPP[3]*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]) + SPP[5]*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]) - (q0*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]))/2; @@ -845,12 +848,12 @@ void CovariancePrediction() nextP[20][18] = P[20][18]; nextP[20][19] = P[20][19]; nextP[20][20] = P[20][20]; - - for (uint8_t i=0; i< n_states; i++) + + for (uint8_t i=0; i<= 20; i++) { nextP[i][i] = nextP[i][i] + processNoise[i]; } - + // If on ground or no magnetometer fitted, inhibit magnetometer bias updates by // setting the coresponding covariance terms to zero. if (onGround || !useCompass) @@ -858,7 +861,7 @@ void CovariancePrediction() zeroRows(nextP,15,20); zeroCols(nextP,15,20); } - + // If on ground or not using airspeed sensing, inhibit wind velocity // covariance growth. if (onGround || !useAirspeed) @@ -866,7 +869,7 @@ void CovariancePrediction() zeroRows(nextP,13,14); zeroCols(nextP,13,14); } - + // If the total position variance exceds 1E6 (1000m), then stop covariance // growth by setting the predicted to the previous values // This prevent an ill conditioned matrix from occurring for long periods @@ -882,11 +885,11 @@ void CovariancePrediction() } } } - + // Force symmetry on the covariance matrix to prevent ill-conditioning // of the matrix which would cause the filter to blow-up - for (uint8_t i=0; i< n_states; i++) P[i][i] = nextP[i][i]; - for (uint8_t i=1; i< n_states; i++) + for (uint8_t i=0; i<=20; i++) P[i][i] = nextP[i][i]; + for (uint8_t i=1; i<=20; i++) { for (uint8_t j=0; j<=i-1; j++) { @@ -894,14 +897,14 @@ void CovariancePrediction() P[j][i] = P[i][j]; } } - + // } void FuseVelposNED() { - - // declare variables used by fault isolation logic + +// declare variables used by fault isolation logic uint32_t gpsRetryTime = 30000; // time in msec before GPS fusion will be retried following innovation consistency failure uint32_t gpsRetryTimeNoTAS = 5000; // retry time if no TAS measurement available uint32_t hgtRetryTime = 5000; // height measurement retry time @@ -915,43 +918,43 @@ void FuseVelposNED() bool velTimeout; bool posTimeout; bool hgtTimeout; - - // declare variables used to check measurement errors + +// declare variables used to check measurement errors float velInnov[3] = {0.0,0.0,0.0}; float posInnov[2] = {0.0,0.0}; float hgtInnov = 0.0; - - // declare variables used to control access to arrays + +// declare variables used to control access to arrays bool fuseData[6] = {false,false,false,false,false,false}; uint8_t stateIndex; - unsigned obsIndex; - unsigned indexLimit; - - // declare variables used by state and covariance update calculations + uint8_t obsIndex; + uint8_t indexLimit; + +// declare variables used by state and covariance update calculations float velErr; float posErr; float R_OBS[6]; float observation[6]; float SK; float quatMag; - - // Perform sequential fusion of GPS measurements. This assumes that the - // errors in the different velocity and position components are - // uncorrelated which is not true, however in the absence of covariance - // data from the GPS receiver it is the only assumption we can make - // so we might as well take advantage of the computational efficiencies - // associated with sequential fusion + +// Perform sequential fusion of GPS measurements. This assumes that the +// errors in the different velocity and position components are +// uncorrelated which is not true, however in the absence of covariance +// data from the GPS receiver it is the only assumption we can make +// so we might as well take advantage of the computational efficiencies +// associated with sequential fusion if (fuseVelData || fusePosData || fuseHgtData) { // set the GPS data timeout depending on whether airspeed data is present if (useAirspeed) horizRetryTime = gpsRetryTime; else horizRetryTime = gpsRetryTimeNoTAS; - + // Form the observation vector for (uint8_t i=0; i<=2; i++) observation[i] = velNED[i]; for (uint8_t i=3; i<=4; i++) observation[i] = posNE[i-3]; observation[5] = -(hgtMea); - + // Estimate the GPS Velocity, GPS horiz position and height measurement variances. velErr = 0.2*accNavMag; // additional error in GPS velocities caused by manoeuvring posErr = 0.2*accNavMag; // additional error in GPS position caused by manoeuvring @@ -961,7 +964,7 @@ void FuseVelposNED() R_OBS[3] = R_OBS[2]; R_OBS[4] = 4.0f + sq(posErr); R_OBS[5] = 4.0f; - + // Set innovation variances to zero default for (uint8_t i = 0; i<=5; i++) { @@ -1069,7 +1072,7 @@ void FuseVelposNED() stateIndex = 4 + obsIndex; // Calculate the measurement innovation, using states from a // different time coordinate if fusing height data - if (obsIndex <= 2) + if (obsIndex >= 0 && obsIndex <= 2) { innovVelPos[obsIndex] = statesAtVelTime[stateIndex] - observation[obsIndex]; } @@ -1126,7 +1129,7 @@ void FuseVelposNED() void FuseMagnetometer() { - + static float q0 = 1.0; static float q1 = 0.0; static float q2 = 0.0; @@ -1137,7 +1140,7 @@ void FuseMagnetometer() static float magXbias = 0.0; static float magYbias = 0.0; static float magZbias = 0.0; - static unsigned obsIndex; + static uint8_t obsIndex; uint8_t indexLimit; float DCM[3][3] = { @@ -1148,17 +1151,17 @@ void FuseMagnetometer() static float MagPred[3] = {0.0,0.0,0.0}; static float R_MAG; static float SH_MAG[9] = {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0}; - float H_MAG[n_states]; + float H_MAG[21]; float SK_MX[6]; float SK_MY[5]; float SK_MZ[6]; - - // Perform sequential fusion of Magnetometer measurements. - // This assumes that the errors in the different components are - // uncorrelated which is not true, however in the absence of covariance - // data fit is the only assumption we can make - // so we might as well take advantage of the computational efficiencies - // associated with sequential fusion + +// Perform sequential fusion of Magnetometer measurements. +// This assumes that the errors in the different components are +// uncorrelated which is not true, however in the absence of covariance +// data fit is the only assumption we can make +// so we might as well take advantage of the computational efficiencies +// associated with sequential fusion if (useCompass && (fuseMagData || obsIndex == 1 || obsIndex == 2)) { // Limit range of states modified when on ground @@ -1170,10 +1173,10 @@ void FuseMagnetometer() { indexLimit = 12; } - + // Sequential fusion of XYZ components to spread processing load across // three prediction time steps. - + // Calculate observation jacobians and Kalman gains if (fuseMagData) { @@ -1188,7 +1191,7 @@ void FuseMagnetometer() magXbias = statesAtMagMeasTime[18]; magYbias = statesAtMagMeasTime[19]; magZbias = statesAtMagMeasTime[20]; - + // rotate predicted earth components into body axes and calculate // predicted measurments DCM[0][0] = q0*q0 + q1*q1 - q2*q2 - q3*q3; @@ -1203,10 +1206,10 @@ void FuseMagnetometer() MagPred[0] = DCM[0][0]*magN + DCM[0][1]*magE + DCM[0][2]*magD + magXbias; MagPred[1] = DCM[1][0]*magN + DCM[1][1]*magE + DCM[1][2]*magD + magYbias; MagPred[2] = DCM[2][0]*magN + DCM[2][1]*magE + DCM[2][2]*magD + magZbias; - + // scale magnetometer observation error with total angular rate R_MAG = 0.0025f + sq(0.05f*dAngIMU.length()/dtIMU); - + // Calculate observation jacobians SH_MAG[0] = 2*magD*q3 + 2*magE*q2 + 2*magN*q1; SH_MAG[1] = 2*magD*q0 - 2*magE*q1 + 2*magN*q2; @@ -1226,7 +1229,7 @@ void FuseMagnetometer() H_MAG[16] = 2*q0*q3 + 2*q1*q2; H_MAG[17] = 2*q1*q3 - 2*q0*q2; H_MAG[18] = 1; - + // Calculate Kalman gain SK_MX[0] = 1/(P[18][18] + R_MAG + P[1][18]*SH_MAG[0] + P[3][18]*SH_MAG[2] - P[15][18]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) - (2*magD*q0 - 2*magE*q1 + 2*magN*q2)*(P[18][2] + P[1][2]*SH_MAG[0] + P[3][2]*SH_MAG[2] - P[15][2]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][2]*(2*q0*q3 + 2*q1*q2) - P[17][2]*(2*q0*q2 - 2*q1*q3) - P[2][2]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[18][0] + P[1][0]*SH_MAG[0] + P[3][0]*SH_MAG[2] - P[15][0]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][0]*(2*q0*q3 + 2*q1*q2) - P[17][0]*(2*q0*q2 - 2*q1*q3) - P[2][0]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[18][1] + P[1][1]*SH_MAG[0] + P[3][1]*SH_MAG[2] - P[15][1]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][1]*(2*q0*q3 + 2*q1*q2) - P[17][1]*(2*q0*q2 - 2*q1*q3) - P[2][1]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[2]*(P[18][3] + P[1][3]*SH_MAG[0] + P[3][3]*SH_MAG[2] - P[15][3]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][3]*(2*q0*q3 + 2*q1*q2) - P[17][3]*(2*q0*q2 - 2*q1*q3) - P[2][3]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6])*(P[18][15] + P[1][15]*SH_MAG[0] + P[3][15]*SH_MAG[2] - P[15][15]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][15]*(2*q0*q3 + 2*q1*q2) - P[17][15]*(2*q0*q2 - 2*q1*q3) - P[2][15]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][15]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[16][18]*(2*q0*q3 + 2*q1*q2) - P[17][18]*(2*q0*q2 - 2*q1*q3) - P[2][18]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + (2*q0*q3 + 2*q1*q2)*(P[18][16] + P[1][16]*SH_MAG[0] + P[3][16]*SH_MAG[2] - P[15][16]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][16]*(2*q0*q3 + 2*q1*q2) - P[17][16]*(2*q0*q2 - 2*q1*q3) - P[2][16]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q2 - 2*q1*q3)*(P[18][17] + P[1][17]*SH_MAG[0] + P[3][17]*SH_MAG[2] - P[15][17]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][17]*(2*q0*q3 + 2*q1*q2) - P[17][17]*(2*q0*q2 - 2*q1*q3) - P[2][17]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[0][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); SK_MX[1] = SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]; @@ -1257,7 +1260,7 @@ void FuseMagnetometer() Kfusion[20] = SK_MX[0]*(P[20][18] + P[20][1]*SH_MAG[0] + P[20][3]*SH_MAG[2] + P[20][0]*SK_MX[3] - P[20][2]*SK_MX[2] - P[20][15]*SK_MX[1] + P[20][16]*SK_MX[5] - P[20][17]*SK_MX[4]); varInnovMag[0] = 1.0f/SK_MX[0]; innovMag[0] = MagPred[0] - magData.x; - + // reset the observation index to 0 (we start by fusing the X // measurement) obsIndex = 0; @@ -1274,7 +1277,7 @@ void FuseMagnetometer() H_MAG[16] = SH_MAG[4] - SH_MAG[3] - SH_MAG[5] + SH_MAG[6]; H_MAG[17] = 2*q0*q1 + 2*q2*q3; H_MAG[19] = 1; - + // Calculate Kalman gain SK_MY[0] = 1/(P[19][19] + R_MAG + P[0][19]*SH_MAG[2] + P[1][19]*SH_MAG[1] + P[2][19]*SH_MAG[0] - P[16][19]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - (2*q0*q3 - 2*q1*q2)*(P[19][15] + P[0][15]*SH_MAG[2] + P[1][15]*SH_MAG[1] + P[2][15]*SH_MAG[0] - P[16][15]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][15]*(2*q0*q3 - 2*q1*q2) + P[17][15]*(2*q0*q1 + 2*q2*q3) - P[3][15]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (2*q0*q1 + 2*q2*q3)*(P[19][17] + P[0][17]*SH_MAG[2] + P[1][17]*SH_MAG[1] + P[2][17]*SH_MAG[0] - P[16][17]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][17]*(2*q0*q3 - 2*q1*q2) + P[17][17]*(2*q0*q1 + 2*q2*q3) - P[3][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[19][3] + P[0][3]*SH_MAG[2] + P[1][3]*SH_MAG[1] + P[2][3]*SH_MAG[0] - P[16][3]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][3]*(2*q0*q3 - 2*q1*q2) + P[17][3]*(2*q0*q1 + 2*q2*q3) - P[3][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[15][19]*(2*q0*q3 - 2*q1*q2) + P[17][19]*(2*q0*q1 + 2*q2*q3) + SH_MAG[2]*(P[19][0] + P[0][0]*SH_MAG[2] + P[1][0]*SH_MAG[1] + P[2][0]*SH_MAG[0] - P[16][0]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][0]*(2*q0*q3 - 2*q1*q2) + P[17][0]*(2*q0*q1 + 2*q2*q3) - P[3][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[19][1] + P[0][1]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[2][1]*SH_MAG[0] - P[16][1]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][1]*(2*q0*q3 - 2*q1*q2) + P[17][1]*(2*q0*q1 + 2*q2*q3) - P[3][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[19][2] + P[0][2]*SH_MAG[2] + P[1][2]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[16][2]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][2]*(2*q0*q3 - 2*q1*q2) + P[17][2]*(2*q0*q1 + 2*q2*q3) - P[3][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6])*(P[19][16] + P[0][16]*SH_MAG[2] + P[1][16]*SH_MAG[1] + P[2][16]*SH_MAG[0] - P[16][16]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][16]*(2*q0*q3 - 2*q1*q2) + P[17][16]*(2*q0*q1 + 2*q2*q3) - P[3][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[3][19]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); SK_MY[1] = SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]; @@ -1317,7 +1320,7 @@ void FuseMagnetometer() H_MAG[16] = 2*q2*q3 - 2*q0*q1; H_MAG[17] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]; H_MAG[20] = 1; - + // Calculate Kalman gain SK_MZ[0] = 1/(P[20][20] + R_MAG + P[0][20]*SH_MAG[1] + P[3][20]*SH_MAG[0] + P[17][20]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) - (2*magD*q1 + 2*magE*q0 - 2*magN*q3)*(P[20][1] + P[0][1]*SH_MAG[1] + P[3][1]*SH_MAG[0] + P[17][1]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][1]*(2*q0*q2 + 2*q1*q3) - P[16][1]*(2*q0*q1 - 2*q2*q3) - P[1][1]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[20][2] + P[0][2]*SH_MAG[1] + P[3][2]*SH_MAG[0] + P[17][2]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][2]*(2*q0*q2 + 2*q1*q3) - P[16][2]*(2*q0*q1 - 2*q2*q3) - P[1][2]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[20][0] + P[0][0]*SH_MAG[1] + P[3][0]*SH_MAG[0] + P[17][0]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][0]*(2*q0*q2 + 2*q1*q3) - P[16][0]*(2*q0*q1 - 2*q2*q3) - P[1][0]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[20][3] + P[0][3]*SH_MAG[1] + P[3][3]*SH_MAG[0] + P[17][3]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][3]*(2*q0*q2 + 2*q1*q3) - P[16][3]*(2*q0*q1 - 2*q2*q3) - P[1][3]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6])*(P[20][17] + P[0][17]*SH_MAG[1] + P[3][17]*SH_MAG[0] + P[17][17]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][17]*(2*q0*q2 + 2*q1*q3) - P[16][17]*(2*q0*q1 - 2*q2*q3) - P[1][17]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[15][20]*(2*q0*q2 + 2*q1*q3) - P[16][20]*(2*q0*q1 - 2*q2*q3) - P[1][20]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + (2*q0*q2 + 2*q1*q3)*(P[20][15] + P[0][15]*SH_MAG[1] + P[3][15]*SH_MAG[0] + P[17][15]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][15]*(2*q0*q2 + 2*q1*q3) - P[16][15]*(2*q0*q1 - 2*q2*q3) - P[1][15]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][15]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q1 - 2*q2*q3)*(P[20][16] + P[0][16]*SH_MAG[1] + P[3][16]*SH_MAG[0] + P[17][16]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][16]*(2*q0*q2 + 2*q1*q3) - P[16][16]*(2*q0*q1 - 2*q2*q3) - P[1][16]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[2][20]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); SK_MZ[1] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]; @@ -1348,9 +1351,9 @@ void FuseMagnetometer() Kfusion[20] = SK_MZ[0]*(P[20][20] + P[20][0]*SH_MAG[1] + P[20][3]*SH_MAG[0] - P[20][1]*SK_MZ[2] + P[20][2]*SK_MZ[3] + P[20][17]*SK_MZ[1] + P[20][15]*SK_MZ[5] - P[20][16]*SK_MZ[4]); varInnovMag[2] = 1.0f/SK_MZ[0]; innovMag[2] = MagPred[2] - magData.z; - + } - + // Check the innovation for consistency and don't fuse if > 5Sigma if ((innovMag[obsIndex]*innovMag[obsIndex]/varInnovMag[obsIndex]) < 25.0) { @@ -1434,18 +1437,18 @@ void FuseAirspeed() const float R_TAS = 2.0f; float SH_TAS[3]; float SK_TAS; - float H_TAS[n_states]; - float Kfusion[n_states]; + float H_TAS[21]; + float Kfusion[21]; float VtasPred; float quatMag; - + // Copy required states to local variable names vn = statesAtVtasMeasTime[4]; ve = statesAtVtasMeasTime[5]; vd = statesAtVtasMeasTime[6]; vwn = statesAtVtasMeasTime[13]; vwe = statesAtVtasMeasTime[14]; - + // Need to check that it is flying before fusing airspeed data // Calculate the predicted airspeed VtasPred = sqrt((ve - vwe)*(ve - vwe) + (vn - vwn)*(vn - vwn) + vd*vd); @@ -1462,7 +1465,7 @@ void FuseAirspeed() H_TAS[6] = vd*SH_TAS[0]; H_TAS[13] = -SH_TAS[2]; H_TAS[14] = -SH_TAS[1]; - + // Calculate Kalman gains SK_TAS = 1.0f/(R_TAS + SH_TAS[2]*(P[4][4]*SH_TAS[2] + P[5][4]*SH_TAS[1] - P[13][4]*SH_TAS[2] - P[14][4]*SH_TAS[1] + P[6][4]*vd*SH_TAS[0]) + SH_TAS[1]*(P[4][5]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[13][5]*SH_TAS[2] - P[14][5]*SH_TAS[1] + P[6][5]*vd*SH_TAS[0]) - SH_TAS[2]*(P[4][13]*SH_TAS[2] + P[5][13]*SH_TAS[1] - P[13][13]*SH_TAS[2] - P[14][13]*SH_TAS[1] + P[6][13]*vd*SH_TAS[0]) - SH_TAS[1]*(P[4][14]*SH_TAS[2] + P[5][14]*SH_TAS[1] - P[13][14]*SH_TAS[2] - P[14][14]*SH_TAS[1] + P[6][14]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[4][6]*SH_TAS[2] + P[5][6]*SH_TAS[1] - P[13][6]*SH_TAS[2] - P[14][6]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0])); Kfusion[0] = SK_TAS*(P[0][4]*SH_TAS[2] - P[0][13]*SH_TAS[2] + P[0][5]*SH_TAS[1] - P[0][14]*SH_TAS[1] + P[0][6]*vd*SH_TAS[0]); @@ -1487,7 +1490,7 @@ void FuseAirspeed() Kfusion[19] = SK_TAS*(P[19][4]*SH_TAS[2] - P[19][13]*SH_TAS[2] + P[19][5]*SH_TAS[1] - P[19][14]*SH_TAS[1] + P[19][6]*vd*SH_TAS[0]); Kfusion[20] = SK_TAS*(P[20][4]*SH_TAS[2] - P[20][13]*SH_TAS[2] + P[20][5]*SH_TAS[1] - P[20][14]*SH_TAS[1] + P[20][6]*vd*SH_TAS[0]); varInnovVtas = 1.0f/SK_TAS; - + // Calculate the measurement innovation innovVtas = VtasPred - VtasMeas; // Check the innovation for consistency and don't fuse if > 5Sigma @@ -1550,6 +1553,7 @@ void FuseAirspeed() } } } + void zeroRows(float covMat[n_states][n_states], uint8_t first, uint8_t last) { uint8_t row; @@ -1598,7 +1602,7 @@ void RecallStates(float statesForFusion[n_states], uint32_t msec) long int bestTimeDelta = 200; uint8_t storeIndex; uint8_t bestStoreIndex = 0; - for (storeIndex=0; storeIndex < n_states; storeIndex++) + for (storeIndex=0; storeIndex < data_buffer_size; storeIndex++) { timeDelta = msec - statetimeStamp[storeIndex]; if (timeDelta < 0) timeDelta = -timeDelta; diff --git a/src/modules/fw_att_pos_estimator/estimator.h b/src/modules/fw_att_pos_estimator/estimator.h index d95745c80c..a7dd08a935 100644 --- a/src/modules/fw_att_pos_estimator/estimator.h +++ b/src/modules/fw_att_pos_estimator/estimator.h @@ -117,6 +117,9 @@ extern float gpsLon; extern float gpsHgt; extern uint8_t GPSstatus; +// Baro input +extern float baroHgt; + extern bool statesInitialised; const float covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions From e32d92e7322fef0acdfb8913c930d90abfbfa627 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 31 Jan 2014 10:00:03 +0100 Subject: [PATCH 034/381] =?UTF-8?q?Switch=20to=20Paul=E2=80=99s=20estimato?= =?UTF-8?q?r=20for=20all=20fixed=20wing=20setups?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ROMFS/px4fmu_common/init.d/rc.fw_apps | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps index d354fb06fa..9ab310ab9e 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_apps +++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps @@ -6,7 +6,7 @@ # # Start the attitude and position estimator # -att_pos_estimator_ekf start +fw_att_pos_estimator start # # Start attitude controller From 9695ae8cee414429c782403ffaa8ca0c9d90dc2a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 31 Jan 2014 10:00:26 +0100 Subject: [PATCH 035/381] Update API to include baro altitude. --- .../fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 7a71e774c8..88c75903e2 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -400,6 +400,7 @@ FixedwingEstimator::task_main() fuseHgtData = false; fuseMagData = false; fuseVtasData = false; + statesInitialised = false; /* wakeup source(s) */ struct pollfd fds[2]; @@ -577,7 +578,7 @@ FixedwingEstimator::task_main() if (baro_updated) { orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); - /* XXX leverage baro */ + baroHgt = _baro.altitude; } bool gps_updated; @@ -601,6 +602,7 @@ FixedwingEstimator::task_main() if (hrt_elapsed_time(&start_time) > 500000 && !_initialized) { InitialiseFilter(); _initialized = true; + warnx("init done."); continue; } From d4ccd9bc45d82dff38b19848ab88eaae35853b94 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 31 Jan 2014 14:08:27 +0100 Subject: [PATCH 036/381] Fix a number of interface scaling / offset stupidities, should be closer to operational now on HW. --- .../fw_att_pos_estimator_main.cpp | 69 +++++++++++-------- 1 file changed, 39 insertions(+), 30 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 88c75903e2..10f7d42acd 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -596,7 +596,7 @@ FixedwingEstimator::task_main() gpsGndSpd = sqrtf(_gps.vel_n_m_s * _gps.vel_n_m_s + _gps.vel_e_m_s * _gps.vel_e_m_s); gpsVelD = _gps.vel_d_m_s; gpsLat = math::radians(_gps.lat / (double)1e7); - gpsLon = math::radians(_gps.lon / (double)1e7); + gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI; gpsHgt = _gps.alt / 1e3f; if (hrt_elapsed_time(&start_time) > 500000 && !_initialized) { @@ -610,7 +610,9 @@ FixedwingEstimator::task_main() if (_initialized) { /* convert GPS measurements to horizontal NE, altitude and 3D velocity NED */ - calcvelNED(velNED, gpsCourse, gpsGndSpd, gpsVelD); + velNED[0] = _gps.vel_n_m_s; + velNED[1] = _gps.vel_e_m_s; + velNED[2] = _gps.vel_d_m_s; calcposNED(posNED, gpsLat, gpsLon, gpsHgt, latRef, lonRef, hgtRef); posNE[0] = posNED[0]; @@ -657,30 +659,28 @@ FixedwingEstimator::task_main() orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag); // XXX we compensate the offsets upfront - should be close to zero. - // XXX the purpose of the 0.001 factor is unclear // 0.001f - magData.x = 0.001f * _mag.x; - magBias.x = 0.0f; // _mag_offsets.x_offset + magData.x = _mag.x; + magBias.x = 0.0001f; // _mag_offsets.x_offset - magData.y = 0.001f * _mag.y; - magBias.y = 0.0f; // _mag_offsets.y_offset + magData.y = _mag.y; + magBias.y = 0.0001f; // _mag_offsets.y_offset - magData.z = 0.001f * _mag.z; - magBias.z = 0.0f; // _mag_offsets.y_offset + magData.z = _mag.z; + magBias.z = 0.0001f; // _mag_offsets.y_offset #else // XXX we compensate the offsets upfront - should be close to zero. - // XXX the purpose of the 0.001 factor is unclear // 0.001f - magData.x = 0.001f * _sensor_combined.magnetometer_ga[0]; - magBias.x = 0.0f; // _mag_offsets.x_offset + magData.x = _sensor_combined.magnetometer_ga[0]; + magBias.x = 0.0001f; // _mag_offsets.x_offset - magData.y = 0.001f * _sensor_combined.magnetometer_ga[1]; - magBias.y = 0.0f; // _mag_offsets.y_offset + magData.y = _sensor_combined.magnetometer_ga[1]; + magBias.y = 0.0001f; // _mag_offsets.y_offset - magData.z = 0.001f * _sensor_combined.magnetometer_ga[2]; - magBias.z = 0.0f; // _mag_offsets.y_offset + magData.z = _sensor_combined.magnetometer_ga[2]; + magBias.z = 0.0001f; // _mag_offsets.y_offset #endif @@ -749,14 +749,14 @@ FixedwingEstimator::task_main() _att.roll = euler.getPhi(); _att.pitch = euler.getTheta(); _att.yaw = euler.getPsi(); - // XXX find the right state - _att.rollspeed = _gyro.x - states[11]; - _att.pitchspeed = _gyro.y - states[12]; - _att.yawspeed = _gyro.z - states[13]; + + _att.rollspeed = angRate.x - states[10]; + _att.pitchspeed = angRate.y - states[11]; + _att.yawspeed = angRate.z - states[12]; // gyro offsets - _att.rate_offsets[0] = states[11]; - _att.rate_offsets[1] = states[12]; - _att.rate_offsets[2] = states[13]; + _att.rate_offsets[0] = states[10]; + _att.rate_offsets[1] = states[11]; + _att.rate_offsets[2] = states[12]; /* lazily publish the attitude only once available */ if (_att_pub > 0) { @@ -880,13 +880,22 @@ int fw_att_pos_estimator_main(int argc, char *argv[]) printf("attitude: roll: %8.4f, pitch %8.4f, yaw: %8.4f degrees\n", (double)math::degrees(euler.getPhi()), (double)math::degrees(euler.getTheta()), (double)math::degrees(euler.getPsi())); - printf("states (quat) [1-4]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)states[0], (double)states[1], (double)states[2], (double)states[3]); - printf("states (vel) [5-7]: %8.4f, %8.4f, %8.4f\n", (double)states[4], (double)states[5], (double)states[6]); - printf("states (pos) [8-10]: %8.4f, %8.4f, %8.4f\n", (double)states[7], (double)states[8], (double)states[9]); - printf("states [11-13]: %8.4f, %8.4f, %8.4f\n", (double)states[10], (double)states[11], (double)states[12]); - printf("states [14-16]: %8.4f, %8.4f, %8.4f\n", (double)states[13], (double)states[14], (double)states[15]); - printf("states [17-19]: %8.4f, %8.4f, %8.4f\n", (double)states[16], (double)states[17], (double)states[18]); - printf("states [20-21]: %8.4f, %8.4f\n", (double)states[19], (double)states[20]); + // State vector: + // 0-3: quaternions (q0, q1, q2, q3) + // 4-6: Velocity - m/sec (North, East, Down) + // 7-9: Position - m (North, East, Down) + // 10-12: Delta Angle bias - rad (X,Y,Z) + // 13-14: Wind Vector - m/sec (North,East) + // 15-17: Earth Magnetic Field Vector - milligauss (North, East, Down) + // 18-20: Body Magnetic Field Vector - milligauss (X,Y,Z) + + printf("states (quat) [1-4]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)states[0], (double)states[1], (double)states[2], (double)states[3]); + printf("states (vel m/s) [5-7]: %8.4f, %8.4f, %8.4f\n", (double)states[4], (double)states[5], (double)states[6]); + printf("states (pos m) [8-10]: %8.4f, %8.4f, %8.4f\n", (double)states[7], (double)states[8], (double)states[9]); + printf("states (delta ang) [11-13]: %8.4f, %8.4f, %8.4f\n", (double)states[10], (double)states[11], (double)states[12]); + printf("states (wind) [14-15]: %8.4f, %8.4f\n", (double)states[13], (double)states[14]); + printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)states[15], (double)states[16], (double)states[17]); + printf("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f\n", (double)states[18], (double)states[19], (double)states[20]); exit(0); } else { From 40a3510e75733d240265b05afff9002787d6067a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 31 Jan 2014 14:11:45 +0100 Subject: [PATCH 037/381] Fix estimator timestamp handling for the two interface cases --- .../fw_att_pos_estimator_main.cpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 88c75903e2..88684588c5 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -456,6 +456,8 @@ FixedwingEstimator::task_main() perf_count(_perf_gyro); + hrt_abstime last_sensor_timestamp; + /* load local copies */ #ifndef SENSOR_COMBINED_SUB orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro); @@ -468,7 +470,7 @@ FixedwingEstimator::task_main() orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); } - + last_sensor_timestamp = _gyro.timestamp; IMUmsec = _gyro.timestamp / 1e3f; float deltaT = (_gyro.timestamp - last_run) / 1e6f; @@ -510,7 +512,7 @@ FixedwingEstimator::task_main() // Copy gyro and accel - + last_sensor_timestamp = _sensor_combined.timestamp; IMUmsec = _sensor_combined.timestamp / 1e3f; float deltaT = (_sensor_combined.timestamp - last_run) / 1e6f; @@ -737,7 +739,7 @@ FixedwingEstimator::task_main() for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) _att.R[i][j] = R(i, j); - _att.timestamp = _gyro.timestamp; + _att.timestamp = last_sensor_timestamp; _att.q[0] = states[0]; _att.q[1] = states[1]; _att.q[2] = states[2]; @@ -745,7 +747,7 @@ FixedwingEstimator::task_main() _att.q_valid = true; _att.R_valid = true; - _att.timestamp = _gyro.timestamp; + _att.timestamp = last_sensor_timestamp; _att.roll = euler.getPhi(); _att.pitch = euler.getTheta(); _att.yaw = euler.getPsi(); @@ -768,7 +770,7 @@ FixedwingEstimator::task_main() _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att); } - _local_pos.timestamp = _gyro.timestamp; + _local_pos.timestamp = last_sensor_timestamp; _local_pos.x = states[7]; _local_pos.y = states[8]; _local_pos.z = states[9]; From 8e35f6727c20960c05c6e7fad38fccb80f0bdc5e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 31 Jan 2014 16:52:54 +0100 Subject: [PATCH 038/381] Testing missing init bit in array initialization --- src/modules/fw_att_pos_estimator/estimator.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index c7c9b6476d..9b57dfd550 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -1730,6 +1730,7 @@ void calcEarthRateNED(Vector3f &omega, float latitude) void CovarianceInit() { // Calculate the initial covariance matrix P + P[0][0] = 0.25f*sq(1.0f*deg2rad); P[1][1] = 0.25f*sq(1.0f*deg2rad); P[2][2] = 0.25f*sq(1.0f*deg2rad); P[3][3] = 0.25f*sq(10.0f*deg2rad); From 9cb59e33a6da2bcd2bb49bc3d49ce527597582f9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 31 Jan 2014 17:43:49 +0100 Subject: [PATCH 039/381] Initializing variables --- .../fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 10f7d42acd..3ed8163455 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -392,8 +392,6 @@ FixedwingEstimator::task_main() parameters_update(); - Vector3f lastAngRate; - Vector3f lastAccel; /* set initial filter state */ fuseVelData = false; fusePosData = false; @@ -402,6 +400,11 @@ FixedwingEstimator::task_main() fuseVtasData = false; statesInitialised = false; + /* initialize measurement data */ + VtasMeas = 0.0f; + Vector3f lastAngRate = {0.0f, 0.0f, 0.0f}; + Vector3f lastAccel = {0.0f, 0.0f, 0.0f}; + /* wakeup source(s) */ struct pollfd fds[2]; From 70ffa27acd6b5cd276e45d8c029ea743c32b2bc7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 1 Feb 2014 12:09:54 +0100 Subject: [PATCH 040/381] Rewrote the filter mainloop to match the order in the offboard simulator, added a number of scaling fixes, initializing all structs correctly --- .../fw_att_pos_estimator/estimator.cpp | 49 +++- src/modules/fw_att_pos_estimator/estimator.h | 7 +- .../fw_att_pos_estimator_main.cpp | 237 ++++++++++-------- 3 files changed, 167 insertions(+), 126 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index 9b57dfd550..3373319d0c 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -1,5 +1,8 @@ #include "estimator.h" +// For debugging only +#include + // Global variables float KH[n_states][n_states]; // intermediate result used for covariance updates float KHP[n_states][n_states]; // intermediate result used for covariance updates @@ -32,14 +35,15 @@ float posNED[3]; // North, East Down position (m) 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 +float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time +float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time + float innovMag[3]; // innovation output float varInnovMag[3]; // innovation variance output Vector3f magData; // magnetometer flux radings in X,Y,Z body axes -float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time float innovVtas; // innovation output float varInnovVtas; // innovation variance output float VtasMeas; // true airspeed measurement (m/s) -float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time float latRef; // WGS-84 latitude of reference point (rad) float lonRef; // WGS-84 longitude of reference point (rad) float hgtRef; // WGS-84 height of reference point (m) @@ -1125,6 +1129,8 @@ void FuseVelposNED() } } } + + //printf("velh: %s, posh: %s, hgth: %s\n", ((velHealth) ? "OK" : "FAIL"), ((posHealth) ? "OK" : "FAIL"), ((hgtHealth) ? "OK" : "FAIL")); } void FuseMagnetometer() @@ -1586,13 +1592,15 @@ float sq(float valIn) } // Store states in a history array along with time stamp -void StoreStates() +void StoreStates(uint64_t timestamp_ms) { static uint8_t storeIndex = 0; - if (storeIndex == data_buffer_size) storeIndex = 0; - for (uint8_t i=0; i<=n_states; i++) storedStates[i][storeIndex] = states[i]; - statetimeStamp[storeIndex] = millis(); - storeIndex = storeIndex + 1; + for (unsigned i=0; i 1.0f) + if (deltaT > 1.0f || deltaT < 0.000001f) deltaT = 0.01f; // Always store data, independent of init status @@ -549,41 +556,13 @@ FixedwingEstimator::task_main() #endif - if (_initialized) { + bool airspeed_updated; + orb_check(_airspeed_sub, &airspeed_updated); + if (airspeed_updated) { + orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); + perf_count(_perf_airspeed); - /* predict states and covariances */ - - /* run the strapdown INS every sensor update */ - UpdateStrapdownEquationsNED(); - - /* store the predictions */ - StoreStates(); - - /* evaluate if on ground */ - OnGroundCheck(); - - /* prepare the delta angles and time used by the covariance prediction */ - summedDelAng = summedDelAng + correctedDelAng; - summedDelVel = summedDelVel + correctedDelVel; - dt += dtIMU; - - /* predict the covairance if the total delta angle has exceeded the threshold - * or the time limit will be exceeded on the next measurement update - */ - if ((dt >= (covTimeStepMax - dtIMU)) || (summedDelAng.length() > covDelAngMax)) { - CovariancePrediction(); - summedDelAng = summedDelAng.zero(); - summedDelVel = summedDelVel.zero(); - dt = 0.0f; - } - } - - bool baro_updated; - orb_check(_baro_sub, &baro_updated); - if (baro_updated) { - orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); - - baroHgt = _baro.altitude; + VtasMeas = _airspeed.true_airspeed_m_s; } bool gps_updated; @@ -592,65 +571,36 @@ FixedwingEstimator::task_main() orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps); perf_count(_perf_gps); - if (_gps.fix_type > 2) { + if (_gps.fix_type < 3) { + gps_updated = false; + } else { /* fuse GPS updates */ //_gps.timestamp / 1e3; GPSstatus = _gps.fix_type; - gpsCourse = _gps.cog_rad; - gpsGndSpd = sqrtf(_gps.vel_n_m_s * _gps.vel_n_m_s + _gps.vel_e_m_s * _gps.vel_e_m_s); - gpsVelD = _gps.vel_d_m_s; + velNED[0] = _gps.vel_n_m_s; + velNED[1] = _gps.vel_e_m_s; + velNED[2] = _gps.vel_d_m_s; + + // warnx("GPS updated: status: %d, vel: %8.4f %8.4f %8.4f", (int)GPSstatus, velNED[0], velNED[1], velNED[2]); + gpsLat = math::radians(_gps.lat / (double)1e7); gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI; gpsHgt = _gps.alt / 1e3f; - if (hrt_elapsed_time(&start_time) > 500000 && !_initialized) { - InitialiseFilter(); - _initialized = true; - - warnx("init done."); - continue; - } - - if (_initialized) { - - /* convert GPS measurements to horizontal NE, altitude and 3D velocity NED */ - velNED[0] = _gps.vel_n_m_s; - velNED[1] = _gps.vel_e_m_s; - velNED[2] = _gps.vel_d_m_s; - calcposNED(posNED, gpsLat, gpsLon, gpsHgt, latRef, lonRef, hgtRef); - - posNE[0] = posNED[0]; - posNE[1] = posNED[1]; - hgtMea = -posNED[2]; - - // set flags for further processing - fuseVelData = true; - fusePosData = true; - fuseHgtData = true; - - /* recall states after adjusting for delays */ - RecallStates(statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); - RecallStates(statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); - RecallStates(statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms)); - - /* run the actual fusion */ - FuseVelposNED(); - } - - } else { - - /* do not fuse anything, we got no position / vel update */ - fuseVelData = false; - fusePosData = false; - fuseHgtData = false; } - } else { - /* do not fuse anything, we got no position / vel update */ - fuseVelData = false; - fusePosData = false; - fuseHgtData = false; + } + + bool baro_updated; + orb_check(_baro_sub, &baro_updated); + if (baro_updated) { + orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); + + baroHgt = _baro.altitude; + + // Could use a blend of GPS and baro alt data if desired + hgtMea = 1.0f*baroHgt + 0.0f*gpsHgt; } #ifndef SENSOR_COMBINED_SUB @@ -689,41 +639,107 @@ FixedwingEstimator::task_main() #endif - if (_initialized) { + } - fuseMagData = true; - RecallStates(statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); - FuseMagnetometer(); + + /** + * PART TWO: EXECUTE THE FILTER + **/ + + if (hrt_elapsed_time(&start_time) > 500000 && !_initialized && (GPSstatus == 3)) { + InitialiseFilter(velNED); + _initialized = true; + + warnx("init done."); + } + + if (_initialized) { + + /* predict states and covariances */ + + /* run the strapdown INS every sensor update */ + UpdateStrapdownEquationsNED(); + + /* store the predictions */ + StoreStates(IMUmsec); + + /* evaluate if on ground */ + OnGroundCheck(); + + /* prepare the delta angles and time used by the covariance prediction */ + summedDelAng = summedDelAng + correctedDelAng; + summedDelVel = summedDelVel + correctedDelVel; + dt += dtIMU; + + /* predict the covairance if the total delta angle has exceeded the threshold + * or the time limit will be exceeded on the next measurement update + */ + if ((dt >= (covTimeStepMax - dtIMU)) || (summedDelAng.length() > covDelAngMax)) { + CovariancePrediction(); + summedDelAng = summedDelAng.zero(); + summedDelVel = summedDelVel.zero(); + dt = 0.0f; } + } + + + if (gps_updated && _initialized) { + + /* convert GPS measurements to horizontal NE, altitude and 3D velocity NED */ + calcposNED(posNED, gpsLat, gpsLon, gpsHgt, latRef, lonRef, hgtRef); + + posNE[0] = posNED[0]; + posNE[1] = posNED[1]; + + // set flags for further processing + fuseVelData = true; + fusePosData = true; + + /* recall states after adjusting for delays */ + RecallStates(statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); + RecallStates(statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); + + /* run the actual fusion */ + FuseVelposNED(); + } else { + fuseVelData = false; + fusePosData = false; + } + + if (baro_updated && _initialized) { + + fuseHgtData = true; + // recall states stored at time of measurement after adjusting for delays + RecallStates(statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms)); + // run the fusion step + FuseVelposNED(); + } else { + fuseHgtData = false; + } + + if (mag_updated && _initialized) { + fuseMagData = true; + RecallStates(statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); } else { fuseMagData = false; } - bool airspeed_updated; - orb_check(_airspeed_sub, &airspeed_updated); - if (airspeed_updated && _initialized) { - orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); - perf_count(_perf_airspeed); + if (_initialized) { + FuseMagnetometer(); + } - if (_airspeed.true_airspeed_m_s > 8.0f /* XXX magic number */) { - - VtasMeas = _airspeed.true_airspeed_m_s; - - if (_initialized) { - - fuseVtasData = true; - RecallStates(statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data - FuseAirspeed(); - } - } else { - fuseVtasData = false; - } + if (airspeed_updated && _initialized + && _airspeed.true_airspeed_m_s > 8.0f /* XXX magic number */) { + fuseVtasData = true; + RecallStates(statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data + FuseAirspeed(); } else { fuseVtasData = false; } + // Publish results if (_initialized) { // State vector: @@ -894,6 +910,7 @@ int fw_att_pos_estimator_main(int argc, char *argv[]) // 15-17: Earth Magnetic Field Vector - milligauss (North, East, Down) // 18-20: Body Magnetic Field Vector - milligauss (X,Y,Z) + printf("dt: %8.6f\n", dtIMU); printf("states (quat) [1-4]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)states[0], (double)states[1], (double)states[2], (double)states[3]); printf("states (vel m/s) [5-7]: %8.4f, %8.4f, %8.4f\n", (double)states[4], (double)states[5], (double)states[6]); printf("states (pos m) [8-10]: %8.4f, %8.4f, %8.4f\n", (double)states[7], (double)states[8], (double)states[9]); From 0962d041f17beda7d4495c4d450dd39967a11a90 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 1 Feb 2014 17:23:03 +0100 Subject: [PATCH 041/381] Estimator init --- src/modules/fw_att_pos_estimator/estimator.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index 9b57dfd550..f2db4aa1ed 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -322,7 +322,7 @@ void CovariancePrediction() float SF[14]; float SG[8]; float SQ[11]; - float SPP[13]; + float SPP[13] = {0}; float nextP[21][21]; // calculate covariance prediction process noise From c73fbca0ebef6a0333b364bfe90891b580eccace Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Feb 2014 01:32:13 +0100 Subject: [PATCH 042/381] Move instance to new task context --- src/modules/mavlink/mavlink_main.cpp | 33 +++++++++++++--------------- src/modules/mavlink/mavlink_main.h | 6 ++--- 2 files changed, 18 insertions(+), 21 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 6c04d2aba0..f70c3ce759 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -156,7 +156,7 @@ namespace mavlink Mavlink *g_mavlink; } -Mavlink::Mavlink(const char *port, unsigned baud_rate) : +Mavlink::Mavlink() : _task_should_exit(false), _mavlink_task(-1), @@ -215,9 +215,9 @@ int Mavlink::instance_count() return inst_index + 1; } -Mavlink* Mavlink::new_instance(const char *port, unsigned baud_rate) +Mavlink* Mavlink::new_instance() { - Mavlink* inst = new Mavlink(port, baud_rate); + Mavlink* inst = new Mavlink(); Mavlink* parent = ::_head; while (parent->_next != nullptr) parent = parent->_next; @@ -1740,21 +1740,23 @@ Mavlink::task_main(int argc, char *argv[]) int Mavlink::start_helper(int argc, char *argv[]) { - // This is beyond evil.. and needs a lock to be safe - return Mavlink::get_instance(Mavlink::instance_count() - 1)->task_main(argc, argv); + // Create the instance in task context + Mavlink *instance = Mavlink::new_instance(); + // This will actually only return once MAVLink exits + return instance->task_main(argc, argv); } int -Mavlink::start(Mavlink* mavlink) +Mavlink::start() { /* start the task */ char buf[32]; sprintf(buf, "mavlink if%d", Mavlink::instance_count()); - mavlink->_mavlink_task = task_spawn_cmd(buf, + /*mavlink->_mavlink_task = */task_spawn_cmd(buf, SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, + SCHED_PRIORITY_DEFAULT, 2048, (main_t)&Mavlink::start_helper, NULL); @@ -1763,10 +1765,10 @@ Mavlink::start(Mavlink* mavlink) // usleep(200); // } - if (mavlink->_mavlink_task < 0) { - warn("task start failed"); - return -errno; - } + // if (mavlink->_mavlink_task < 0) { + // warn("task start failed"); + // return -errno; + // } return OK; } @@ -1790,13 +1792,8 @@ int mavlink_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { - Mavlink *instance = Mavlink::new_instance("/dev/ttyS0", 57600); - - if (mavlink::g_mavlink == nullptr) - mavlink::g_mavlink = instance; - // Instantiate thread - Mavlink::start(instance); + Mavlink::start(); // if (mavlink::g_mavlink != nullptr) { // errx(1, "already running"); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index e50b0f0c0f..9e2475e2ba 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -143,7 +143,7 @@ public: /** * Constructor */ - Mavlink(const char *port, unsigned baud_rate); + Mavlink(); /** * Destructor, also kills the mavlinks task. @@ -155,7 +155,7 @@ public: * * @return OK on success. */ - static int start(Mavlink* mavlink); + static int start(); /** * Display the mavlink status. @@ -164,7 +164,7 @@ public: static int instance_count(); - static Mavlink* new_instance(const char *port, unsigned baud_rate); + static Mavlink* new_instance(); static Mavlink* get_instance(unsigned instance); From a5e02db6d55f246ae71bd5de356dfe17b89bf26d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Feb 2014 01:32:36 +0100 Subject: [PATCH 043/381] Move orb listener to new thread context --- src/modules/mavlink/mavlink_orb_listener.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_orb_listener.cpp b/src/modules/mavlink/mavlink_orb_listener.cpp index a6bcd4c0a2..ffff1838c9 100644 --- a/src/modules/mavlink/mavlink_orb_listener.cpp +++ b/src/modules/mavlink/mavlink_orb_listener.cpp @@ -726,13 +726,13 @@ MavlinkOrbListener::uorb_receive_thread(void *arg) void * MavlinkOrbListener::uorb_start_helper(void *context) { - return ((MavlinkOrbListener *)context)->uorb_receive_thread(NULL); + MavlinkOrbListener* urcv = new MavlinkOrbListener(((Mavlink *)context)); + return urcv->uorb_receive_thread(NULL); } pthread_t MavlinkOrbListener::uorb_receive_start(Mavlink* mavlink) { - MavlinkOrbListener* urcv = new MavlinkOrbListener(mavlink); /* --- SENSORS RAW VALUE --- */ mavlink->get_subs()->sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); @@ -840,7 +840,7 @@ MavlinkOrbListener::uorb_receive_start(Mavlink* mavlink) pthread_attr_setstacksize(&uorb_attr, 2048); pthread_t thread; - pthread_create(&thread, &uorb_attr, MavlinkOrbListener::uorb_start_helper, urcv); + pthread_create(&thread, &uorb_attr, MavlinkOrbListener::uorb_start_helper, mavlink); pthread_attr_destroy(&uorb_attr); return thread; From b1e5304a3f50975a1d282bf5b7418ff276a26c45 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Feb 2014 01:32:53 +0100 Subject: [PATCH 044/381] Move serial port listener to new thread context --- 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 0f1d5293ef..3752bde104 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -843,13 +843,13 @@ void MavlinkReceiver::print_status() void * MavlinkReceiver::start_helper(void *context) { - return ((MavlinkReceiver *)context)->receive_thread(NULL); + MavlinkReceiver *rcv = new MavlinkReceiver(((Mavlink *)context)); + return rcv->receive_thread(NULL); } pthread_t MavlinkReceiver::receive_start(Mavlink *mavlink) { - MavlinkReceiver *rcv = new MavlinkReceiver(mavlink); pthread_attr_t receiveloop_attr; pthread_attr_init(&receiveloop_attr); @@ -866,7 +866,7 @@ MavlinkReceiver::receive_start(Mavlink *mavlink) pthread_attr_setstacksize(&receiveloop_attr, 3000); pthread_t thread; - pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, rcv); + pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, mavlink); pthread_attr_destroy(&receiveloop_attr); return thread; From 62d3369dc94458ed30b9326814296cd9a33a3cc9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Feb 2014 11:06:53 +0100 Subject: [PATCH 045/381] Fixed init --- src/modules/mavlink/mavlink_main.cpp | 180 +++++++++++---------------- src/modules/mavlink/mavlink_main.h | 7 +- 2 files changed, 79 insertions(+), 108 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 6c04d2aba0..c29d9957de 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -159,6 +159,7 @@ namespace mavlink Mavlink::Mavlink(const char *port, unsigned baud_rate) : _task_should_exit(false), + thread_running(false), _mavlink_task(-1), _mavlink_fd(-1), _mavlink_incoming_fd(-1), @@ -207,23 +208,23 @@ int Mavlink::instance_count() /* note: a local buffer count will help if this ever is called often */ Mavlink* inst = ::_head; unsigned inst_index = 0; - while (inst->_next != nullptr) { + while (inst != nullptr) { inst = inst->_next; inst_index++; } - return inst_index + 1; + return inst_index; } Mavlink* Mavlink::new_instance(const char *port, unsigned baud_rate) { Mavlink* inst = new Mavlink(port, baud_rate); - Mavlink* parent = ::_head; - while (parent->_next != nullptr) - parent = parent->_next; + Mavlink* next = ::_head; + while (next != nullptr) + next = next->_next; - /* now parent points to a null pointer, fill it */ - parent->_next = inst; + /* now parent has a null pointer, fill it */ + next = inst; return inst; } @@ -377,7 +378,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * case 921600: speed = B921600; break; default: - warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600\n\n", baud); + warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\t9600, 19200, 38400, 57600\t\n115200\n230400\n460800\n921600\n", baud); return -EINVAL; } @@ -437,15 +438,15 @@ Mavlink::set_hil_on_off(bool hil_enabled) /* ramp up some HIL-related subscriptions */ unsigned hil_rate_interval; - if (baudrate < 19200) { + if (_baudrate < 19200) { /* 10 Hz */ hil_rate_interval = 100; - } else if (baudrate < 38400) { + } else if (_baudrate < 38400) { /* 10 Hz */ hil_rate_interval = 100; - } else if (baudrate < 115200) { + } else if (_baudrate < 115200) { /* 20 Hz */ hil_rate_interval = 50; @@ -1438,82 +1439,48 @@ Mavlink::task_main(int argc, char *argv[]) /* initialize logging device */ // YYY - _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - - mavlink_log_info(_mavlink_fd, "[mavlink] started"); - - /* wakeup source(s) */ - struct pollfd fds[1]; - - /* Setup of loop */ - fds[0].fd = _params_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])), 100); - - /* timed out - periodic check for _task_should_exit, etc. */ - if (pret == 0) { - continue; - } - - /* this is undesirable but not much we can do - might want to flag unhappy status */ - if (pret < 0) { - warn("poll error %d, %d", pret, errno); - continue; - } - - perf_begin(_loop_perf); - - /* parameters updated */ - if (fds[0].revents & POLLIN) { - parameters_update(); - } - - - + _mavlink_fd = 0;//open(MAVLINK_LOG_DEVICE, 0); + //mavlink_log_info(_mavlink_fd, "[mavlink] started"); /* initialize mavlink text message buffering */ // mavlink_logbuffer_init(&lb, 10); int ch; char *device_name = "/dev/ttyS1"; - baudrate = 57600; + _baudrate = 57600; - // /* work around some stupidity in task_create's argv handling */ - // argc -= 2; - // argv += 2; + /* work around some stupidity in task_create's argv handling */ + argc -= 2; + argv += 2; - // while ((ch = getopt(argc, argv, "b:d:eo")) != EOF) { - // switch (ch) { - // case 'b': - // baudrate = strtoul(optarg, NULL, 10); + while ((ch = getopt(argc, argv, "b:d:eo")) != EOF) { + switch (ch) { + case 'b': + _baudrate = strtoul(optarg, NULL, 10); - // if (baudrate < 9600 || baudrate > 921600) - // errx(1, "invalid baud rate '%s'", optarg); + if (_baudrate < 9600 || _baudrate > 921600) + errx(1, "invalid baud rate '%s'", optarg); - // break; + break; - // case 'd': - // device_name = optarg; - // break; + case 'd': + device_name = optarg; + break; - // case 'e': - // mavlink_link_termination_allowed = true; - // break; + case 'e': + mavlink_link_termination_allowed = true; + break; - // case 'o': - // _mode = MODE_ONBOARD; - // break; + case 'o': + _mode = MODE_ONBOARD; + break; - // default: - // usage(); - // break; - // } - // } + default: + usage(); + break; + } + } struct termios uart_config_original; @@ -1529,13 +1496,13 @@ Mavlink::task_main(int argc, char *argv[]) fflush(stdout); /* default values for arguments */ - uart = mavlink_open_uart(baudrate, device_name, &uart_config_original, &usb_uart); + uart = mavlink_open_uart(_baudrate, device_name, &uart_config_original, &usb_uart); if (uart < 0) err(1, "could not open %s", device_name); /* create the device node that's used for sending text log messages, etc. */ - register_driver(MAVLINK_LOG_DEVICE, &fops, 0666, NULL); + //register_driver(MAVLINK_LOG_DEVICE, &fops, 0666, NULL); /* Initialize system properties */ mavlink_update_system(); @@ -1552,7 +1519,7 @@ Mavlink::task_main(int argc, char *argv[]) mavlink_wpm_init(wpm); /* all subscriptions are now active, set up initial guess about rate limits */ - if (baudrate >= 230400) { + if (_baudrate >= 230400) { /* 200 Hz / 5 ms */ set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_HIGHRES_IMU, 20); set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_RAW_IMU, 20); @@ -1566,7 +1533,7 @@ Mavlink::task_main(int argc, char *argv[]) /* 10 Hz */ set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 100); - } else if (baudrate >= 115200) { + } else if (_baudrate >= 115200) { /* 20 Hz / 50 ms */ set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_HIGHRES_IMU, 50); set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_RAW_IMU, 50); @@ -1579,7 +1546,7 @@ Mavlink::task_main(int argc, char *argv[]) /* 2 Hz */ set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); - } else if (baudrate >= 57600) { + } else if (_baudrate >= 57600) { /* 10 Hz / 100 ms */ set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_RAW_IMU, 300); set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_HIGHRES_IMU, 300); @@ -1616,7 +1583,35 @@ Mavlink::task_main(int argc, char *argv[]) /* arm counter to go off immediately */ unsigned lowspeed_counter = 10; - while (!thread_should_exit) { + /* wakeup source(s) */ + struct pollfd fds[1]; + + /* Setup of loop */ + fds[0].fd = _params_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])), 100); + + /* timed out - periodic check for _task_should_exit, etc. */ + if (pret == 0) { + continue; + } + + /* this is undesirable but not much we can do - might want to flag unhappy status */ + if (pret < 0) { + warn("poll error %d, %d", pret, errno); + continue; + } + + perf_begin(_loop_perf); + + /* parameters updated */ + if (fds[0].revents & POLLIN) { + parameters_update(); + } /* 1 Hz */ if (lowspeed_counter == 10) { @@ -1670,31 +1665,20 @@ Mavlink::task_main(int argc, char *argv[]) mavlink_waypoint_eventloop(hrt_absolute_time()); - /* sleep quarter the time */ - usleep(25000); - /* check if waypoint has been reached against the last positions */ mavlink_waypoint_eventloop(hrt_absolute_time()); - /* sleep quarter the time */ - usleep(25000); /* send parameters at 20 Hz (if queued for sending) */ mavlink_pm_queued_send(); mavlink_waypoint_eventloop(hrt_absolute_time()); - /* sleep quarter the time */ - usleep(25000); - mavlink_waypoint_eventloop(hrt_absolute_time()); - if (baudrate > 57600) { + if (_baudrate > 57600) { mavlink_pm_queued_send(); } - /* sleep 10 ms */ - usleep(10000); - // /* send one string at 10 Hz */ // if (!mavlink_logbuffer_is_empty(&lb)) { // struct mavlink_logmessage msg; @@ -1705,8 +1689,7 @@ Mavlink::task_main(int argc, char *argv[]) // } // } - /* sleep 15 ms */ - usleep(15000); + perf_end(_loop_perf); } /* wait for threads to complete */ @@ -1721,17 +1704,6 @@ Mavlink::task_main(int argc, char *argv[]) thread_running = false; - return 0; - - - - - - - - perf_end(_loop_perf); - } - warnx("exiting."); _mavlink_task = -1; @@ -1755,7 +1727,7 @@ Mavlink::start(Mavlink* mavlink) mavlink->_mavlink_task = task_spawn_cmd(buf, SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 2048, + 4096, (main_t)&Mavlink::start_helper, NULL); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index e50b0f0c0f..34e2b59eea 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -270,7 +270,8 @@ protected: private: - bool _task_should_exit; /**< if true, sensor task should exit */ + bool _task_should_exit; /**< if true, mavlink task should exit */ + bool thread_running; int _mavlink_task; /**< task handle for sensor task */ int _mavlink_incoming_fd; /**< file descriptor on which to receive incoming strings */ @@ -285,8 +286,6 @@ private: orb_advert_t mission_pub; struct mission_s mission; uint8_t missionlib_msg_buf[300]; //XXX MAGIC NUMBER - bool thread_running; - bool thread_should_exit; MAVLINK_MODE _mode; uint8_t mavlink_wpm_comp_id; @@ -304,7 +303,7 @@ private: bool verbose; int uart; - int baudrate; + int _baudrate; bool gcs_link; /** * If the queue index is not at 0, the queue sending From 1e6011cc87042b40cf02705deb33f29f3afdb5a8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Feb 2014 16:59:39 +0100 Subject: [PATCH 046/381] Stop the through hoop jumping now that we can handle multiple interfaces --- ROMFS/px4fmu_common/init.d/rc.usb | 31 ------------------------------- 1 file changed, 31 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index 0cd8a0e042..3d8be089ee 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -5,38 +5,7 @@ echo "Starting MAVLink on this USB console" -# Stop tone alarm -tone_alarm stop - -# -# Check for UORB -# -if uorb start -then - echo "uORB started" -fi - -# Tell MAVLink that this link is "fast" -if mavlink stop -then - echo "stopped other MAVLink instance" -fi mavlink start -b 230400 -d /dev/ttyACM0 -# Stop commander -if commander stop -then - echo "Commander stopped" -fi -sleep 1 - -# Start the commander -if commander start -then - echo "Commander started" -fi - -echo "MAVLink started, exiting shell.." - # Exit shell to make it available to MAVLink exit From 8d2d171bb83c4f24f709c20ce8c89b6d1f529616 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 2 Feb 2014 17:00:16 +0100 Subject: [PATCH 047/381] Fixes and cleanups --- src/modules/mavlink/mavlink_main.cpp | 54 +++++++++++++--------------- 1 file changed, 25 insertions(+), 29 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index fa05758664..b5bf9ece0b 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -144,7 +144,11 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length #endif } - write(uart, ch, (size_t)(sizeof(uint8_t) * length)); + size_t desired = (size_t)(sizeof(uint8_t) * length); + int ret = write(uart, ch, desired); + + if (ret != desired) + warn("write err"); } @@ -1509,11 +1513,11 @@ Mavlink::task_main(int argc, char *argv[]) /* start the MAVLink receiver */ // MavlinkReceiver rcv(this); - receive_thread = MavlinkReceiver::receive_start(this); + //receive_thread = MavlinkReceiver::receive_start(this); /* start the ORB receiver */ //MavlinkOrbListener listener(this); - uorb_receive_thread = MavlinkOrbListener::uorb_receive_start(this); + //uorb_receive_thread = MavlinkOrbListener::uorb_receive_start(this); /* initialize waypoint manager */ mavlink_wpm_init(wpm); @@ -1595,11 +1599,6 @@ Mavlink::task_main(int argc, char *argv[]) /* wait for up to 100ms for data */ int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); - /* timed out - periodic check for _task_should_exit, etc. */ - if (pret == 0) { - continue; - } - /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { warn("poll error %d, %d", pret, errno); @@ -1722,26 +1721,6 @@ int Mavlink::start() { - /* start the task */ - char buf[32]; - sprintf(buf, "mavlink if%d", Mavlink::instance_count()); - - /*mavlink->_mavlink_task = */task_spawn_cmd(buf, - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 2048, - (main_t)&Mavlink::start_helper, - NULL); - - // while (!this->is_running()) { - // usleep(200); - // } - - // if (mavlink->_mavlink_task < 0) { - // warn("task start failed"); - // return -errno; - // } - return OK; } @@ -1765,7 +1744,24 @@ int mavlink_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { // Instantiate thread - Mavlink::start(); + char buf[32]; + sprintf(buf, "mavlink if%d", Mavlink::instance_count()); + + /*mavlink->_mavlink_task = */task_spawn_cmd(buf, + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2048, + (main_t)&Mavlink::start_helper, + (const char **)argv); + + // while (!this->is_running()) { + // usleep(200); + // } + + // if (mavlink->_mavlink_task < 0) { + // warn("task start failed"); + // return -errno; + // } // if (mavlink::g_mavlink != nullptr) { // errx(1, "already running"); From 30882e103b8551cbdd40db7c4bc4a8a809893d6a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 5 Feb 2014 13:22:58 +0100 Subject: [PATCH 048/381] Moved to using references for arrays --- .../fw_att_pos_estimator/estimator.cpp | 23 +++++++++--------- src/modules/fw_att_pos_estimator/estimator.h | 24 +++++++++---------- 2 files changed, 22 insertions(+), 25 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index 95408b729b..24e3f4f162 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -53,7 +53,6 @@ float EAS2TAS = 1.0f; // ratio f true to equivalent airspeed // GPS input data variables float gpsCourse; -float gpsGndSpd; float gpsVelD; float gpsLat; float gpsLon; @@ -1560,7 +1559,7 @@ void FuseAirspeed() } } -void zeroRows(float covMat[n_states][n_states], uint8_t first, uint8_t last) +void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) { uint8_t row; uint8_t col; @@ -1573,7 +1572,7 @@ 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) +void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) { uint8_t row; uint8_t col; @@ -1604,7 +1603,7 @@ void StoreStates(uint64_t timestamp_ms) } // Output the state vector stored at the time that best matches that specified by msec -void RecallStates(float statesForFusion[n_states], uint32_t msec) +void RecallStates(float (&statesForFusion)[n_states], uint32_t msec) { long int timeDelta; long int bestTimeDelta = 200; @@ -1630,7 +1629,7 @@ void RecallStates(float statesForFusion[n_states], uint32_t msec) } } -void quat2Tnb(Mat3f &Tnb, float quat[4]) +void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]) { // Calculate the nav to body cosine matrix float q00 = sq(quat[0]); @@ -1655,7 +1654,7 @@ void quat2Tnb(Mat3f &Tnb, float quat[4]) Tnb.y.z = 2*(q23 + q01); } -void quat2Tbn(Mat3f &Tbn, float quat[4]) +void quat2Tbn(Mat3f &Tbn, const float (&quat)[4]) { // Calculate the body to nav cosine matrix float q00 = sq(quat[0]); @@ -1680,7 +1679,7 @@ void quat2Tbn(Mat3f &Tbn, float quat[4]) Tbn.z.y = 2*(q23 + q01); } -void eul2quat(float quat[4], float eul[3]) +void eul2quat(float (&quat)[4], const float (&eul)[3]) { float u1 = cos(0.5f*eul[0]); float u2 = cos(0.5f*eul[1]); @@ -1694,28 +1693,28 @@ void eul2quat(float quat[4], float eul[3]) quat[3] = u1*u2*u6-u4*u5*u3; } -void quat2eul(float y[3], float u[4]) +void quat2eul(float (&y)[3], const float (&u)[4]) { y[0] = atan2((2.0*(u[2]*u[3]+u[0]*u[1])) , (u[0]*u[0]-u[1]*u[1]-u[2]*u[2]+u[3]*u[3])); y[1] = -asin(2.0*(u[1]*u[3]-u[0]*u[2])); y[2] = atan2((2.0*(u[1]*u[2]+u[0]*u[3])) , (u[0]*u[0]+u[1]*u[1]-u[2]*u[2]-u[3]*u[3])); } -void calcvelNED(float velNED[3], float gpsCourse, float gpsGndSpd, float gpsVelD) +void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD) { velNED[0] = gpsGndSpd*cos(gpsCourse); velNED[1] = gpsGndSpd*sin(gpsCourse); velNED[2] = gpsVelD; } -void calcposNED(float posNED[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef) +void calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef) { posNED[0] = earthRadius * (lat - latRef); posNED[1] = earthRadius * cos(latRef) * (lon - lonRef); posNED[2] = -(hgt - hgtRef); } -void calcLLH(float posNED[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef) +void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef) { lat = latRef + posNED[0] * earthRadiusInv; lon = lonRef + posNED[1] * earthRadiusInv / cos(latRef); @@ -1799,7 +1798,7 @@ void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, fl initQuat[3] = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading; } -void InitialiseFilter(float initvelNED[3]) +void InitialiseFilter(float (&initvelNED)[3]) { // Do the data structure init for (unsigned i = 0; i < n_states; i++) { diff --git a/src/modules/fw_att_pos_estimator/estimator.h b/src/modules/fw_att_pos_estimator/estimator.h index 2a0e43bb5e..01a1ace996 100644 --- a/src/modules/fw_att_pos_estimator/estimator.h +++ b/src/modules/fw_att_pos_estimator/estimator.h @@ -134,41 +134,39 @@ void FuseMagnetometer(); void FuseAirspeed(); -void zeroRows(float covMat[n_states][n_states], uint8_t first, uint8_t last); +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); +void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last); float sq(float valIn); -void quatNorm(float quatOut[4], float quatIn[4]); +void quatNorm(float (&quatOut)[4], const float quatIn[4]); // store staes along with system time stamp in msces void StoreStates(uint64_t timestamp_ms); // recall stste vector stored at closest time to the one specified by msec -void RecallStates(float statesForFusion[n_states], uint32_t msec); +void RecallStates(float (&statesForFusion)[n_states], uint32_t msec); -void quat2Tnb(Mat3f &Tnb, float quat[4]); - -void quat2Tbn(Mat3f &Tbn, float quat[4]); +void quat2Tbn(Mat3f &Tbn, const float (&quat)[4]); void calcEarthRateNED(Vector3f &omega, float latitude); -void eul2quat(float quat[4], float eul[3]); +void eul2quat(float (&quat)[4], const float (&eul)[3]); -void quat2eul(float eul[3],float quat[4]); +void quat2eul(float (&eul)[3], const float (&quat)[4]); -void calcvelNED(float velNED[3], float gpsCourse, float gpsGndSpd, float gpsVelD); +void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD); -void calcposNED(float posNED[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); +void calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); -void calcLLH(float posNED[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); +void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); void OnGroundCheck(); void CovarianceInit(); -void InitialiseFilter(float initvelNED[3]); +void InitialiseFilter(float (&initvelNED)[3]); uint32_t millis(); From dc2f40b80d6291d4f9b2060bb1d5c86d704a3ca1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 5 Feb 2014 14:23:48 +0100 Subject: [PATCH 049/381] Updated uploader from Bootloader repo --- Tools/px_uploader.py | 48 ++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 48 insertions(+) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index cce5e5e54a..23dc484505 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -50,6 +50,9 @@ # Currently only used for informational purposes. # +# for python2.7 compatibility +from __future__ import print_function + import sys import argparse import binascii @@ -154,6 +157,8 @@ class uploader(object): PROG_MULTI = b'\x27' READ_MULTI = b'\x28' # rev2 only GET_CRC = b'\x29' # rev3+ + GET_OTP = b'\x2a' # rev4+ , get a word from OTP area + GET_SN = b'\x2b' # rev4+ , get a word from SN area REBOOT = b'\x30' INFO_BL_REV = b'\x01' # bootloader protocol revision @@ -175,6 +180,8 @@ class uploader(object): def __init__(self, portname, baudrate): # open the port, keep the default timeout short so we can poll quickly self.port = serial.Serial(portname, baudrate, timeout=0.5) + self.otp = b'' + self.sn = b'' def close(self): if self.port is not None: @@ -237,6 +244,22 @@ class uploader(object): self.__getSync() return value + # send the GET_OTP command and wait for an info parameter + def __getOTP(self, param): + t = struct.pack("I", param) # int param as 32bit ( 4 byte ) char array. + self.__send(uploader.GET_OTP + t + uploader.EOC) + value = self.__recv(4) + self.__getSync() + return value + + # send the GET_OTP command and wait for an info parameter + def __getSN(self, param): + t = struct.pack("I", param) # int param as 32bit ( 4 byte ) char array. + self.__send(uploader.GET_SN + t + uploader.EOC) + value = self.__recv(4) + self.__getSync() + return value + # send the CHIP_ERASE command and wait for the bootloader to become ready def __erase(self): self.__send(uploader.CHIP_ERASE @@ -353,6 +376,31 @@ class uploader(object): if self.fw_maxsize < fw.property('image_size'): raise RuntimeError("Firmware image is too large for this board") + # OTP added in v4: + if self.bl_rev > 3: + for byte in range(0,32*6,4): + x = self.__getOTP(byte) + self.otp = self.otp + x + print(binascii.hexlify(x).decode('utf-8') + ' ', end='') + # see src/modules/systemlib/otp.h in px4 code: + self.otp_id = self.otp[0:4] + self.otp_idtype = self.otp[4:5] + self.otp_vid = self.otp[8:4:-1] + self.otp_pid = self.otp[12:8:-1] + self.otp_coa = self.otp[32:160] + # show user: + print("type: " + self.otp_id.decode('utf-8')) + print("idtype: " + binascii.b2a_qp(self.otp_idtype).decode('utf-8')) + print("vid: " + binascii.hexlify(self.otp_vid).decode('utf-8')) + print("pid: "+ binascii.hexlify(self.otp_pid).decode('utf-8')) + print("coa: "+ binascii.b2a_base64(self.otp_coa).decode('utf-8')) + print("sn: ", end='') + for byte in range(0,12,4): + x = self.__getSN(byte) + x = x[::-1] # reverse the bytes + self.sn = self.sn + x + print(binascii.hexlify(x).decode('utf-8'), end='') # show user + print('') print("erase...") self.__erase() From 274e3c0ba732c7698f64866c37653ccfb72b9eb3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 9 Feb 2014 18:30:46 +0100 Subject: [PATCH 050/381] Better status reporting --- .../fw_att_pos_estimator_main.cpp | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 8efd875575..33f35bfd0d 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -918,6 +918,17 @@ int fw_att_pos_estimator_main(int argc, char *argv[]) printf("states (wind) [14-15]: %8.4f, %8.4f\n", (double)states[13], (double)states[14]); printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)states[15], (double)states[16], (double)states[17]); printf("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f\n", (double)states[18], (double)states[19], (double)states[20]); + printf("states: %s %s %s %s", + (statesInitialised) ? "INITIALIZED" : "NON_INIT", + (onGround) ? "ON_GROUND" : "AIRBORNE", + (fuseVelData) ? "FUSE_VEL" : "INH_VEL", + (fusePosData) ? "FUSE_POS" : "INH_POS", + (fuseHgtData) ? "FUSE_HGT" : "INH_HGT"); + (fuseMagData) ? "FUSE_MAG" : "INH_MAG", + (fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS"); + (useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD", + (useCompass) ? "USE_COMPASS" : "IGN_COMPASS"); + exit(0); } else { From 3cad5e352e122f874b5d197388288ec9f8a20c9b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 9 Feb 2014 18:34:11 +0100 Subject: [PATCH 051/381] Fixed build error --- .../fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 33f35bfd0d..71df13d274 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -918,14 +918,14 @@ int fw_att_pos_estimator_main(int argc, char *argv[]) printf("states (wind) [14-15]: %8.4f, %8.4f\n", (double)states[13], (double)states[14]); printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)states[15], (double)states[16], (double)states[17]); printf("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f\n", (double)states[18], (double)states[19], (double)states[20]); - printf("states: %s %s %s %s", + printf("states: %s %s %s %s %s %s %s %s %s", (statesInitialised) ? "INITIALIZED" : "NON_INIT", (onGround) ? "ON_GROUND" : "AIRBORNE", (fuseVelData) ? "FUSE_VEL" : "INH_VEL", (fusePosData) ? "FUSE_POS" : "INH_POS", - (fuseHgtData) ? "FUSE_HGT" : "INH_HGT"); + (fuseHgtData) ? "FUSE_HGT" : "INH_HGT", (fuseMagData) ? "FUSE_MAG" : "INH_MAG", - (fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS"); + (fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS", (useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD", (useCompass) ? "USE_COMPASS" : "IGN_COMPASS"); From efecd85658eecd0b0651a21c37d3936589074e91 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 9 Feb 2014 23:54:04 +0100 Subject: [PATCH 052/381] Further build cleanup --- src/modules/fw_att_pos_estimator/estimator.cpp | 4 +--- src/modules/fw_att_pos_estimator/estimator.h | 3 +-- .../fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 4 +++- 3 files changed, 5 insertions(+), 6 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index 24e3f4f162..9be12e700f 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -22,7 +22,6 @@ Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s Vector3f dVelIMU; Vector3f dAngIMU; float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec) -float dt; // time lapsed since last covariance prediction uint8_t fusionModeGPS = 0; // 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 @@ -292,7 +291,7 @@ void UpdateStrapdownEquationsNED() } -void CovariancePrediction() +void CovariancePrediction(float dt) { // scalars float windVelSigma; @@ -1868,5 +1867,4 @@ void InitialiseFilter(float (&initvelNED)[3]) summedDelVel.x = 0.0f; summedDelVel.y = 0.0f; summedDelVel.z = 0.0f; - dt = 0.0f; } diff --git a/src/modules/fw_att_pos_estimator/estimator.h b/src/modules/fw_att_pos_estimator/estimator.h index 01a1ace996..088304cb3a 100644 --- a/src/modules/fw_att_pos_estimator/estimator.h +++ b/src/modules/fw_att_pos_estimator/estimator.h @@ -68,7 +68,6 @@ extern Vector3f correctedDelVel; // delta velocities along the XYZ body axes cor extern Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad) extern Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s) -extern float dt; // time lapsed since last covariance prediction extern float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec) extern bool onGround; // boolean true when the flight vehicle is on the ground (not flying) @@ -126,7 +125,7 @@ const float covDelAngMax = 0.05f; // maximum delta angle between covariance pred void UpdateStrapdownEquationsNED(); -void CovariancePrediction(); +void CovariancePrediction(float dt); void FuseVelposNED(); diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 71df13d274..23cd985302 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -405,6 +405,8 @@ FixedwingEstimator::task_main() Vector3f lastAngRate = {0.0f, 0.0f, 0.0f}; Vector3f lastAccel = {0.0f, 0.0f, 0.0f}; + float dt = 0.0f; // time lapsed since last covariance prediction + /* wakeup source(s) */ struct pollfd fds[2]; @@ -675,7 +677,7 @@ FixedwingEstimator::task_main() * or the time limit will be exceeded on the next measurement update */ if ((dt >= (covTimeStepMax - dtIMU)) || (summedDelAng.length() > covDelAngMax)) { - CovariancePrediction(); + CovariancePrediction(dt); summedDelAng = summedDelAng.zero(); summedDelVel = summedDelVel.zero(); dt = 0.0f; From 6b931a2738f4e6100fbd3cb7bdc168489c4f8b85 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 10 Feb 2014 08:58:36 +0100 Subject: [PATCH 053/381] Testing: Autostart fake GPS in HIL, to be REMOVED --- ROMFS/px4fmu_common/init.d/rcS | 1 + 1 file changed, 1 insertion(+) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 6f4e1f3b56..2a3941e594 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -386,6 +386,7 @@ then then sleep 1 mavlink start -b 230400 -d /dev/ttyACM0 + gps start -f usleep 5000 else if [ $TTYS1_BUSY == yes ] From 2fd5c9d277d8c78a19e38e2a44c9d91c6612aed7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 10 Feb 2014 09:04:51 +0100 Subject: [PATCH 054/381] Removed some debugging, added other, still WIP --- src/modules/fw_att_pos_estimator/estimator.h | 2 +- .../fw_att_pos_estimator_main.cpp | 107 ++++++++++-------- 2 files changed, 60 insertions(+), 49 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/estimator.h b/src/modules/fw_att_pos_estimator/estimator.h index 01a1ace996..ce3050cbe6 100644 --- a/src/modules/fw_att_pos_estimator/estimator.h +++ b/src/modules/fw_att_pos_estimator/estimator.h @@ -121,7 +121,7 @@ extern float baroHgt; extern bool statesInitialised; -const float covTimeStepMax = 0.02f; // maximum time allowed between covariance predictions +const float covTimeStepMax = 0.2f; // maximum time allowed between covariance predictions const float covDelAngMax = 0.05f; // maximum delta angle between covariance predictions void UpdateStrapdownEquationsNED(); diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 71df13d274..28e547cbe2 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -99,6 +99,8 @@ uint32_t millis() return IMUmsec; } +static void print_status(); + class FixedwingEstimator { public: @@ -383,11 +385,11 @@ FixedwingEstimator::task_main() /* rate limit gyro updates to 50 Hz */ /* XXX remove this!, BUT increase the data buffer size! */ - orb_set_interval(_gyro_sub, 6); + orb_set_interval(_gyro_sub, 4); #else _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); /* XXX remove this!, BUT increase the data buffer size! */ - orb_set_interval(_sensor_combined_sub, 6); + orb_set_interval(_sensor_combined_sub, 4); #endif parameters_update(); @@ -502,10 +504,7 @@ FixedwingEstimator::task_main() dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU; lastAngRate = angRate; - // dVelIMU = 0.5f * (accel + lastAccel) * dtIMU; - dVelIMU.x = 0.0f; - dVelIMU.y = 0.0f; - dVelIMU.z = 0.0f; + dVelIMU = 0.5f * (accel + lastAccel) * dtIMU; lastAccel = accel; @@ -585,7 +584,7 @@ FixedwingEstimator::task_main() // warnx("GPS updated: status: %d, vel: %8.4f %8.4f %8.4f", (int)GPSstatus, velNED[0], velNED[1], velNED[2]); gpsLat = math::radians(_gps.lat / (double)1e7); - gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI; + gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI; gpsHgt = _gps.alt / 1e3f; } @@ -616,26 +615,26 @@ FixedwingEstimator::task_main() // XXX we compensate the offsets upfront - should be close to zero. // 0.001f magData.x = _mag.x; - magBias.x = 0.0001f; // _mag_offsets.x_offset + magBias.x = 0.000001f; // _mag_offsets.x_offset magData.y = _mag.y; - magBias.y = 0.0001f; // _mag_offsets.y_offset + magBias.y = 0.000001f; // _mag_offsets.y_offset magData.z = _mag.z; - magBias.z = 0.0001f; // _mag_offsets.y_offset + magBias.z = 0.000001f; // _mag_offsets.y_offset #else // XXX we compensate the offsets upfront - should be close to zero. // 0.001f magData.x = _sensor_combined.magnetometer_ga[0]; - magBias.x = 0.0001f; // _mag_offsets.x_offset + magBias.x = 0.000001f; // _mag_offsets.x_offset magData.y = _sensor_combined.magnetometer_ga[1]; - magBias.y = 0.0001f; // _mag_offsets.y_offset + magBias.y = 0.000001f; // _mag_offsets.y_offset magData.z = _sensor_combined.magnetometer_ga[2]; - magBias.z = 0.0001f; // _mag_offsets.y_offset + magBias.z = 0.000001f; // _mag_offsets.y_offset #endif @@ -664,11 +663,13 @@ FixedwingEstimator::task_main() StoreStates(IMUmsec); /* evaluate if on ground */ - OnGroundCheck(); + // OnGroundCheck(); + onGround = false; /* prepare the delta angles and time used by the covariance prediction */ summedDelAng = summedDelAng + correctedDelAng; summedDelVel = summedDelVel + correctedDelVel; + dt += dtIMU; /* predict the covairance if the total delta angle has exceeded the threshold @@ -680,6 +681,7 @@ FixedwingEstimator::task_main() summedDelVel = summedDelVel.zero(); dt = 0.0f; } + } @@ -730,7 +732,7 @@ FixedwingEstimator::task_main() } if (airspeed_updated && _initialized - && _airspeed.true_airspeed_m_s > 8.0f /* XXX magic number */) { + && _airspeed.true_airspeed_m_s > 6.0f /* XXX magic number */) { fuseVtasData = true; RecallStates(statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data @@ -742,6 +744,8 @@ FixedwingEstimator::task_main() // Publish results if (_initialized) { + + // State vector: // 0-3: quaternions (q0, q1, q2, q3) // 4-6: Velocity - m/sec (North, East, Down) @@ -858,6 +862,45 @@ FixedwingEstimator::start() return OK; } +void print_status() +{ + math::Quaternion q(states[0], states[1], states[2], states[3]); + math::EulerAngles euler(q); + + printf("attitude: roll: %8.4f, pitch %8.4f, yaw: %8.4f degrees\n", + (double)math::degrees(euler.getPhi()), (double)math::degrees(euler.getTheta()), (double)math::degrees(euler.getPsi())); + + // State vector: + // 0-3: quaternions (q0, q1, q2, q3) + // 4-6: Velocity - m/sec (North, East, Down) + // 7-9: Position - m (North, East, Down) + // 10-12: Delta Angle bias - rad (X,Y,Z) + // 13-14: Wind Vector - m/sec (North,East) + // 15-17: Earth Magnetic Field Vector - milligauss (North, East, Down) + // 18-20: Body Magnetic Field Vector - milligauss (X,Y,Z) + + printf("dtIMU: %8.6f dt: %8.6f IMUmsec: %d\n", dtIMU, dt, (int)IMUmsec); + printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)dVelIMU.x, (double)dVelIMU.y, (double)dVelIMU.z, (double)accel.x, (double)accel.y, (double)accel.z); + printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)dAngIMU.x, (double)dAngIMU.y, (double)dAngIMU.z, (double)correctedDelAng.x, (double)correctedDelAng.y, (double)correctedDelAng.z); + printf("states (quat) [1-4]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)states[0], (double)states[1], (double)states[2], (double)states[3]); + printf("states (vel m/s) [5-7]: %8.4f, %8.4f, %8.4f\n", (double)states[4], (double)states[5], (double)states[6]); + printf("states (pos m) [8-10]: %8.4f, %8.4f, %8.4f\n", (double)states[7], (double)states[8], (double)states[9]); + printf("states (delta ang) [11-13]: %8.4f, %8.4f, %8.4f\n", (double)states[10], (double)states[11], (double)states[12]); + printf("states (wind) [14-15]: %8.4f, %8.4f\n", (double)states[13], (double)states[14]); + printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)states[15], (double)states[16], (double)states[17]); + printf("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f\n", (double)states[18], (double)states[19], (double)states[20]); + printf("states: %s %s %s %s %s %s %s %s %s\n", + (statesInitialised) ? "INITIALIZED" : "NON_INIT", + (onGround) ? "ON_GROUND" : "AIRBORNE", + (fuseVelData) ? "FUSE_VEL" : "INH_VEL", + (fusePosData) ? "FUSE_POS" : "INH_POS", + (fuseHgtData) ? "FUSE_HGT" : "INH_HGT", + (fuseMagData) ? "FUSE_MAG" : "INH_MAG", + (fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS", + (useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD", + (useCompass) ? "USE_COMPASS" : "IGN_COMPASS"); +} + int fw_att_pos_estimator_main(int argc, char *argv[]) { if (argc < 1) @@ -895,39 +938,7 @@ int fw_att_pos_estimator_main(int argc, char *argv[]) if (estimator::g_estimator) { warnx("running"); - math::Quaternion q(states[0], states[1], states[2], states[3]); - math::EulerAngles euler(q); - - printf("attitude: roll: %8.4f, pitch %8.4f, yaw: %8.4f degrees\n", - (double)math::degrees(euler.getPhi()), (double)math::degrees(euler.getTheta()), (double)math::degrees(euler.getPsi())); - - // State vector: - // 0-3: quaternions (q0, q1, q2, q3) - // 4-6: Velocity - m/sec (North, East, Down) - // 7-9: Position - m (North, East, Down) - // 10-12: Delta Angle bias - rad (X,Y,Z) - // 13-14: Wind Vector - m/sec (North,East) - // 15-17: Earth Magnetic Field Vector - milligauss (North, East, Down) - // 18-20: Body Magnetic Field Vector - milligauss (X,Y,Z) - - printf("dt: %8.6f\n", dtIMU); - printf("states (quat) [1-4]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)states[0], (double)states[1], (double)states[2], (double)states[3]); - printf("states (vel m/s) [5-7]: %8.4f, %8.4f, %8.4f\n", (double)states[4], (double)states[5], (double)states[6]); - printf("states (pos m) [8-10]: %8.4f, %8.4f, %8.4f\n", (double)states[7], (double)states[8], (double)states[9]); - printf("states (delta ang) [11-13]: %8.4f, %8.4f, %8.4f\n", (double)states[10], (double)states[11], (double)states[12]); - printf("states (wind) [14-15]: %8.4f, %8.4f\n", (double)states[13], (double)states[14]); - printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)states[15], (double)states[16], (double)states[17]); - printf("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f\n", (double)states[18], (double)states[19], (double)states[20]); - printf("states: %s %s %s %s %s %s %s %s %s", - (statesInitialised) ? "INITIALIZED" : "NON_INIT", - (onGround) ? "ON_GROUND" : "AIRBORNE", - (fuseVelData) ? "FUSE_VEL" : "INH_VEL", - (fusePosData) ? "FUSE_POS" : "INH_POS", - (fuseHgtData) ? "FUSE_HGT" : "INH_HGT", - (fuseMagData) ? "FUSE_MAG" : "INH_MAG", - (fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS", - (useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD", - (useCompass) ? "USE_COMPASS" : "IGN_COMPASS"); + print_status(); exit(0); From d572424996124b1f2dafdcbd48baf1abc85ee627 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 10 Feb 2014 09:06:47 +0100 Subject: [PATCH 055/381] Build fix hackery --- .../fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 9c98a49bbf..b32b3686fd 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -357,6 +357,8 @@ FixedwingEstimator::task_main_trampoline(int argc, char *argv[]) estimator::g_estimator->task_main(); } +static float dt = 0.0f; // time lapsed since last covariance prediction + void FixedwingEstimator::task_main() { @@ -407,8 +409,6 @@ FixedwingEstimator::task_main() Vector3f lastAngRate = {0.0f, 0.0f, 0.0f}; Vector3f lastAccel = {0.0f, 0.0f, 0.0f}; - float dt = 0.0f; // time lapsed since last covariance prediction - /* wakeup source(s) */ struct pollfd fds[2]; From a5045ccee663c500011b8a5a94554f4cbb263352 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 11 Feb 2014 14:38:18 +0100 Subject: [PATCH 056/381] Mavlink: get rid of some warnings, initialize channel --- src/modules/mavlink/mavlink_main.cpp | 54 +++++++++----------- src/modules/mavlink/mavlink_main.h | 8 +-- src/modules/mavlink/mavlink_orb_listener.cpp | 9 ++-- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 4 files changed, 32 insertions(+), 41 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index b5bf9ece0b..21ba51b21c 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -36,15 +36,18 @@ * MAVLink 1.0 protocol implementation. * * @author Lorenz Meier + * @author Julian Oes */ #include #include #include #include +#include #include #include #include +#include #include #include #include @@ -66,27 +69,15 @@ #include #include #include +#include #include #include #include #include #include - #include "mavlink_bridge_header.h" -#include #include "math.h" /* isinf / isnan checks */ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include #include "mavlink_main.h" #include "mavlink_orb_listener.h" #include "mavlink_receiver.h" @@ -144,8 +135,11 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length #endif } - size_t desired = (size_t)(sizeof(uint8_t) * length); - int ret = write(uart, ch, desired); + warnx("uart: %d", uart); + warnx("channel: %d", channel); + + ssize_t desired = (sizeof(uint8_t) * length); + ssize_t ret = write(uart, ch, desired); if (ret != desired) warn("write err"); @@ -160,12 +154,11 @@ namespace mavlink Mavlink *g_mavlink; } -Mavlink::Mavlink() : - +Mavlink::Mavlink() : + _mavlink_fd(-1), _task_should_exit(false), thread_running(false), _mavlink_task(-1), - _mavlink_fd(-1), _mavlink_incoming_fd(-1), /* performance counters */ @@ -276,8 +269,6 @@ Mavlink::parameters_update() int Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) { - static unsigned int total_counter = 0; - switch (cmd) { case (int)MAVLINK_IOC_SEND_TEXT_INFO: case (int)MAVLINK_IOC_SEND_TEXT_CRITICAL: @@ -430,7 +421,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * } int -Mavlink::set_hil_on_off(bool hil_enabled) +Mavlink::set_hil_enabled(bool hil_enabled) { int ret = OK; @@ -1398,7 +1389,7 @@ Mavlink::mavlink_missionlib_send_message(mavlink_message_t *msg) { uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg); - mavlink_send_uart_bytes(chan, missionlib_msg_buf, len); + mavlink_send_uart_bytes(_chan, missionlib_msg_buf, len); } @@ -1441,9 +1432,8 @@ Mavlink::task_main(int argc, char *argv[]) fflush(stdout); /* initialize logging device */ - // YYY - - _mavlink_fd = 0;//open(MAVLINK_LOG_DEVICE, 0); + // TODO + _mavlink_fd = -1;//open(MAVLINK_LOG_DEVICE, 0); //mavlink_log_info(_mavlink_fd, "[mavlink] started"); @@ -1451,8 +1441,9 @@ Mavlink::task_main(int argc, char *argv[]) // mavlink_logbuffer_init(&lb, 10); int ch; - char *device_name = "/dev/ttyS1"; + const char *device_name = "/dev/ttyS1"; _baudrate = 57600; + _chan = MAVLINK_COMM_0; /* work around some stupidity in task_create's argv handling */ argc -= 2; @@ -1587,7 +1578,7 @@ Mavlink::task_main(int argc, char *argv[]) /* arm counter to go off immediately */ unsigned lowspeed_counter = 10; - /* wakeup source(s) */ + /* wakeup source(s) */ struct pollfd fds[1]; /* Setup of loop */ @@ -1623,16 +1614,17 @@ Mavlink::task_main(int argc, char *argv[]) get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); /* send heartbeat */ - mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_base_mode, mavlink_custom_mode, mavlink_state); + warnx("send heartbeat, chan: %d", _chan); + mavlink_msg_heartbeat_send(_chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_base_mode, mavlink_custom_mode, mavlink_state); /* switch HIL mode if required */ if (v_status.hil_state == HIL_STATE_ON) - set_hil_on_off(true); + set_hil_enabled(true); else if (v_status.hil_state == HIL_STATE_OFF) - set_hil_on_off(false); + set_hil_enabled(false); /* send status (values already copied in the section above) */ - mavlink_msg_sys_status_send(chan, + mavlink_msg_sys_status_send(_chan, v_status.onboard_control_sensors_present, v_status.onboard_control_sensors_enabled, v_status.onboard_control_sensors_health, diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index bf8c63d389..8c9430829b 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -182,7 +182,7 @@ public: void set_mode(enum MAVLINK_MODE); enum MAVLINK_MODE get_mode() { return _mode; } - bool hil_enabled() { return _mavlink_hil_enabled; }; + bool get_hil_enabled() { return _mavlink_hil_enabled; }; /** * Handle waypoint related messages. @@ -206,7 +206,7 @@ public: * requested change could not be made or was * redundant. */ - int set_hil_on_off(bool hil_enabled); + int set_hil_enabled(bool hil_enabled); struct mavlink_subscriptions { int sensor_sub; @@ -242,7 +242,7 @@ public: struct mavlink_subscriptions subs; struct mavlink_subscriptions* get_subs() { return &subs; } - mavlink_channel_t get_chan() { return chan; } + mavlink_channel_t get_chan() { return _chan; } /** Global position */ struct vehicle_global_position_s global_pos; @@ -289,7 +289,7 @@ private: MAVLINK_MODE _mode; uint8_t mavlink_wpm_comp_id; - mavlink_channel_t chan; + mavlink_channel_t _chan; // XXX probably should be in a header... // extern pthread_t receive_start(int uart); diff --git a/src/modules/mavlink/mavlink_orb_listener.cpp b/src/modules/mavlink/mavlink_orb_listener.cpp index ffff1838c9..6d64569de5 100644 --- a/src/modules/mavlink/mavlink_orb_listener.cpp +++ b/src/modules/mavlink/mavlink_orb_listener.cpp @@ -276,9 +276,9 @@ MavlinkOrbListener::l_vehicle_status(const struct listener *l) /* enable or disable HIL */ if (l->listener->v_status.hil_state == HIL_STATE_ON) - l->mavlink->set_hil_on_off(true); + l->mavlink->set_hil_enabled(true); else if (l->listener->v_status.hil_state == HIL_STATE_OFF) - l->mavlink->set_hil_on_off(false); + l->mavlink->set_hil_enabled(false); /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state = 0; @@ -464,7 +464,7 @@ MavlinkOrbListener::l_actuator_outputs(const struct listener *l) act_outputs.output[7]); /* only send in HIL mode and only send first group for HIL */ - if (l->mavlink->hil_enabled() && l->listener->armed.armed && ids[l->arg] == ORB_ID(actuator_outputs_0)) { + if (l->mavlink->get_hil_enabled() && l->listener->armed.armed && ids[l->arg] == ORB_ID(actuator_outputs_0)) { /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state = 0; @@ -683,7 +683,6 @@ MavlinkOrbListener::uorb_receive_thread(void *arg) struct listener* next = _listeners; unsigned i = 0; - while (next != nullptr) { fds[i].fd = *next->subp; @@ -708,7 +707,7 @@ MavlinkOrbListener::uorb_receive_thread(void *arg) } else { - unsigned i = 0; + i = 0; struct listener* cb = _listeners; while (cb != nullptr) { diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 3752bde104..94d97fc12c 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -346,7 +346,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) * COMMAND_LONG message or a SET_MODE message */ - if (_mavlink->hil_enabled()) { + if (_mavlink->get_hil_enabled()) { uint64_t timestamp = hrt_absolute_time(); From 19105bebc58a045446746813a50bf74faaa3ad39 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 11 Feb 2014 16:16:57 +0100 Subject: [PATCH 057/381] Mavlink: hearbeat sending works now --- src/modules/mavlink/mavlink_main.cpp | 136 ++++++++++++++------------- src/modules/mavlink/mavlink_main.h | 7 +- 2 files changed, 73 insertions(+), 70 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 21ba51b21c..cb620349d9 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -135,9 +135,6 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length #endif } - warnx("uart: %d", uart); - warnx("channel: %d", channel); - ssize_t desired = (sizeof(uint8_t) * length); ssize_t ret = write(uart, ch, desired); @@ -155,7 +152,7 @@ namespace mavlink } Mavlink::Mavlink() : - _mavlink_fd(-1), +// _mavlink_fd(-1), _task_should_exit(false), thread_running(false), _mavlink_task(-1), @@ -217,12 +214,18 @@ Mavlink* Mavlink::new_instance() { Mavlink* inst = new Mavlink(); Mavlink* next = ::_head; - while (next != nullptr) - next = next->_next; - - /* now parent has a null pointer, fill it */ - next = inst; + /* create the first instance at _head */ + if (::_head == nullptr) { + ::_head = inst; + /* afterwards follow the next and append the instance */ + } else { + while (next != nullptr) { + next = next->_next; + } + /* now parent has a null pointer, fill it */ + next = inst; + } return inst; } @@ -246,7 +249,7 @@ int Mavlink::get_uart_fd(unsigned index) { Mavlink* inst = get_instance(index); if (inst) - return inst->_mavlink_fd; + return inst->get_uart_fd(); return -1; } @@ -379,7 +382,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * /* open uart */ warnx("UART is %s, baudrate is %d\n", uart_name, baud); - uart = open(uart_name, O_RDWR | O_NOCTTY); + _uart = open(uart_name, O_RDWR | O_NOCTTY); /* Try to set baud rate */ struct termios uart_config; @@ -387,14 +390,14 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * *is_usb = false; /* Back up the original uart configuration to restore it after exit */ - if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { + if ((termios_state = tcgetattr(_uart, uart_config_original)) < 0) { warnx("ERROR get termios config %s: %d\n", uart_name, termios_state); - close(uart); + close(_uart); return -1; } /* Fill the struct for the new configuration */ - tcgetattr(uart, &uart_config); + tcgetattr(_uart, &uart_config); /* Clear ONLCR flag (which appends a CR for every LF) */ uart_config.c_oflag &= ~ONLCR; @@ -405,19 +408,19 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * /* Set baud rate */ 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); - close(uart); + close(_uart); return -1; } } - if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) { + if ((termios_state = tcsetattr(_uart, TCSANOW, &uart_config)) < 0) { warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name); - close(uart); + close(_uart); return -1; } - return uart; + return _uart; } int @@ -451,7 +454,7 @@ Mavlink::set_hil_enabled(bool hil_enabled) } orb_set_interval(subs.spa_sub, hil_rate_interval); - set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, hil_rate_interval); + set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, hil_rate_interval); } if (!hil_enabled && _mavlink_hil_enabled) { @@ -541,50 +544,50 @@ Mavlink::get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_bas } -int Mavlink::set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval) +int Mavlink::set_mavlink_interval_limit(int mavlink_msg_id, int min_interval) { int ret = OK; switch (mavlink_msg_id) { case MAVLINK_MSG_ID_SCALED_IMU: /* sensor sub triggers scaled IMU */ - orb_set_interval(subs->sensor_sub, min_interval); + orb_set_interval(subs.sensor_sub, min_interval); break; case MAVLINK_MSG_ID_HIGHRES_IMU: /* sensor sub triggers highres IMU */ - orb_set_interval(subs->sensor_sub, min_interval); + orb_set_interval(subs.sensor_sub, min_interval); break; case MAVLINK_MSG_ID_RAW_IMU: /* sensor sub triggers RAW IMU */ - orb_set_interval(subs->sensor_sub, min_interval); + orb_set_interval(subs.sensor_sub, min_interval); break; case MAVLINK_MSG_ID_ATTITUDE: /* attitude sub triggers attitude */ - orb_set_interval(subs->att_sub, min_interval); + orb_set_interval(subs.att_sub, min_interval); break; case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW: /* actuator_outputs triggers this message */ - orb_set_interval(subs->act_0_sub, min_interval); - orb_set_interval(subs->act_1_sub, min_interval); - orb_set_interval(subs->act_2_sub, min_interval); - orb_set_interval(subs->act_3_sub, min_interval); - orb_set_interval(subs->actuators_sub, min_interval); - orb_set_interval(subs->actuators_effective_sub, min_interval); - orb_set_interval(subs->spa_sub, min_interval); - orb_set_interval(subs->rates_setpoint_sub, min_interval); + orb_set_interval(subs.act_0_sub, min_interval); + orb_set_interval(subs.act_1_sub, min_interval); + orb_set_interval(subs.act_2_sub, min_interval); + orb_set_interval(subs.act_3_sub, min_interval); + orb_set_interval(subs.actuators_sub, min_interval); + orb_set_interval(subs.actuators_effective_sub, min_interval); + orb_set_interval(subs.spa_sub, min_interval); + orb_set_interval(subs.rates_setpoint_sub, min_interval); break; case MAVLINK_MSG_ID_MANUAL_CONTROL: /* manual_control_setpoint triggers this message */ - orb_set_interval(subs->man_control_sp_sub, min_interval); + orb_set_interval(subs.man_control_sp_sub, min_interval); break; case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT: - orb_set_interval(subs->debug_key_value, min_interval); + orb_set_interval(subs.debug_key_value, min_interval); break; default: @@ -1433,7 +1436,7 @@ Mavlink::task_main(int argc, char *argv[]) /* initialize logging device */ // TODO - _mavlink_fd = -1;//open(MAVLINK_LOG_DEVICE, 0); +// _mavlink_fd = -1;//open(MAVLINK_LOG_DEVICE, 0); //mavlink_log_info(_mavlink_fd, "[mavlink] started"); @@ -1491,9 +1494,9 @@ Mavlink::task_main(int argc, char *argv[]) fflush(stdout); /* default values for arguments */ - uart = mavlink_open_uart(_baudrate, device_name, &uart_config_original, &usb_uart); + _uart = mavlink_open_uart(_baudrate, device_name, &uart_config_original, &usb_uart); - if (uart < 0) + if (_uart < 0) err(1, "could not open %s", device_name); /* create the device node that's used for sending text log messages, etc. */ @@ -1516,57 +1519,57 @@ Mavlink::task_main(int argc, char *argv[]) /* all subscriptions are now active, set up initial guess about rate limits */ if (_baudrate >= 230400) { /* 200 Hz / 5 ms */ - set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_HIGHRES_IMU, 20); - set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_RAW_IMU, 20); + set_mavlink_interval_limit(MAVLINK_MSG_ID_HIGHRES_IMU, 20); + set_mavlink_interval_limit(MAVLINK_MSG_ID_RAW_IMU, 20); /* 50 Hz / 20 ms */ - set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_ATTITUDE, 30); + set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 30); /* 20 Hz / 50 ms */ - set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 10); - set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50); + set_mavlink_interval_limit(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 10); + set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50); /* 10 Hz */ - set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_GPS_RAW_INT, 100); + set_mavlink_interval_limit(MAVLINK_MSG_ID_GPS_RAW_INT, 100); /* 10 Hz */ - set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 100); + set_mavlink_interval_limit(MAVLINK_MSG_ID_MANUAL_CONTROL, 100); } else if (_baudrate >= 115200) { /* 20 Hz / 50 ms */ - set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_HIGHRES_IMU, 50); - set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_RAW_IMU, 50); - set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_ATTITUDE, 50); - set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50); + set_mavlink_interval_limit(MAVLINK_MSG_ID_HIGHRES_IMU, 50); + set_mavlink_interval_limit(MAVLINK_MSG_ID_RAW_IMU, 50); + set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 50); + set_mavlink_interval_limit(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50); /* 5 Hz / 200 ms */ - set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200); + set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200); /* 5 Hz / 200 ms */ - set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_GPS_RAW_INT, 200); + set_mavlink_interval_limit(MAVLINK_MSG_ID_GPS_RAW_INT, 200); /* 2 Hz */ - set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); + set_mavlink_interval_limit(MAVLINK_MSG_ID_MANUAL_CONTROL, 500); } else if (_baudrate >= 57600) { /* 10 Hz / 100 ms */ - set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_RAW_IMU, 300); - set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_HIGHRES_IMU, 300); + set_mavlink_interval_limit(MAVLINK_MSG_ID_RAW_IMU, 300); + set_mavlink_interval_limit(MAVLINK_MSG_ID_HIGHRES_IMU, 300); /* 10 Hz / 100 ms ATTITUDE */ - set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_ATTITUDE, 200); + set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 200); /* 5 Hz / 200 ms */ - set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200); + set_mavlink_interval_limit(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200); /* 5 Hz / 200 ms */ - set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 500); + set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 500); /* 2 Hz */ - set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); + set_mavlink_interval_limit(MAVLINK_MSG_ID_MANUAL_CONTROL, 500); /* 2 Hz */ - set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_GPS_RAW_INT, 500); + set_mavlink_interval_limit(MAVLINK_MSG_ID_GPS_RAW_INT, 500); } else { /* very low baud rate, limit to 1 Hz / 1000 ms */ - set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_RAW_IMU, 1000); - set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_ATTITUDE, 1000); - set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_HIGHRES_IMU, 1000); + set_mavlink_interval_limit(MAVLINK_MSG_ID_RAW_IMU, 1000); + set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 1000); + set_mavlink_interval_limit(MAVLINK_MSG_ID_HIGHRES_IMU, 1000); /* 1 Hz / 1000 ms */ - set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 1000); + set_mavlink_interval_limit(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 1000); /* 0.5 Hz */ - set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 2000); + set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 2000); /* 0.1 Hz */ - set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 10000); + set_mavlink_interval_limit(MAVLINK_MSG_ID_MANUAL_CONTROL, 10000); } int mission_result_sub = orb_subscribe(ORB_ID(mission_result)); @@ -1581,6 +1584,8 @@ Mavlink::task_main(int argc, char *argv[]) /* wakeup source(s) */ struct pollfd fds[1]; + _params_sub = orb_subscribe(ORB_ID(parameter_update)); + /* Setup of loop */ fds[0].fd = _params_sub; fds[0].events = POLLIN; @@ -1614,7 +1619,6 @@ Mavlink::task_main(int argc, char *argv[]) get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); /* send heartbeat */ - warnx("send heartbeat, chan: %d", _chan); mavlink_msg_heartbeat_send(_chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_base_mode, mavlink_custom_mode, mavlink_state); /* switch HIL mode if required */ @@ -1688,7 +1692,7 @@ Mavlink::task_main(int argc, char *argv[]) pthread_join(uorb_receive_thread, NULL); /* Reset the UART flags to original state */ - tcsetattr(uart, TCSANOW, &uart_config_original); + tcsetattr(_uart, TCSANOW, &uart_config_original); /* destroy log buffer */ //mavlink_logbuffer_destroy(&lb); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 8c9430829b..fb71128919 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -170,7 +170,7 @@ public: static int get_uart_fd(unsigned index); - int get_uart_fd() { return uart; } + int get_uart_fd() { return _uart; } enum MAVLINK_MODE { MODE_TX_HEARTBEAT_ONLY=0, @@ -265,7 +265,6 @@ protected: * registering clone devices etc. */ struct file_operations fops; - int _mavlink_fd; Mavlink* _next; private: @@ -302,7 +301,7 @@ private: mavlink_wpm_storage *wpm; bool verbose; - int uart; + int _uart; int _baudrate; bool gcs_link; /** @@ -390,7 +389,7 @@ private: int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); - int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval); + int set_mavlink_interval_limit(int mavlink_msg_id, int min_interval); /** * Callback for param interface. From ea2a69d8bf15cdb0c4a3234ed2851f9380d5adfc Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 11 Feb 2014 22:36:36 +0100 Subject: [PATCH 058/381] Mavlink: get orb_listener to work --- src/modules/mavlink/mavlink_main.cpp | 29 ++++++-- src/modules/mavlink/mavlink_orb_listener.cpp | 72 +++++++++++--------- src/modules/mavlink/mavlink_orb_listener.h | 6 +- src/modules/uORB/topics/vehicle_attitude.h | 2 +- 4 files changed, 65 insertions(+), 44 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index cb620349d9..f1ec6e8dce 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -153,6 +153,7 @@ namespace mavlink Mavlink::Mavlink() : // _mavlink_fd(-1), + _next(nullptr), _task_should_exit(false), thread_running(false), _mavlink_task(-1), @@ -1448,6 +1449,8 @@ Mavlink::task_main(int argc, char *argv[]) _baudrate = 57600; _chan = MAVLINK_COMM_0; + _mode = MODE_OFFBOARD; + /* work around some stupidity in task_create's argv handling */ argc -= 2; argv += 2; @@ -1488,7 +1491,23 @@ Mavlink::task_main(int argc, char *argv[]) warnx("MAVLink v1.0 serial interface starting..."); /* inform about mode */ - warnx((_mode == MODE_ONBOARD) ? "ONBOARD MODE" : "DOWNLINK MODE"); + switch (_mode) { + case MODE_TX_HEARTBEAT_ONLY: + warnx("MODE_TX_HEARTBEAT_ONLY"); + break; + case MODE_OFFBOARD: + warnx("MODE_OFFBOARD"); + break; + case MODE_ONBOARD: + warnx("MODE_ONBOARD"); + break; + case MODE_HIL: + warnx("MODE_HIL"); + break; + default: + warnx("Error: Unknown mode"); + break; + } /* Flush stdout in case MAVLink is about to take it over */ fflush(stdout); @@ -1506,12 +1525,12 @@ Mavlink::task_main(int argc, char *argv[]) mavlink_update_system(); /* start the MAVLink receiver */ - // MavlinkReceiver rcv(this); - //receive_thread = MavlinkReceiver::receive_start(this); +// MavlinkReceiver rcv(this); + receive_thread = MavlinkReceiver::receive_start(this); /* start the ORB receiver */ - //MavlinkOrbListener listener(this); - //uorb_receive_thread = MavlinkOrbListener::uorb_receive_start(this); +// MavlinkOrbListener listener(this); + uorb_receive_thread = MavlinkOrbListener::uorb_receive_start(this); /* initialize waypoint manager */ mavlink_wpm_init(wpm); diff --git a/src/modules/mavlink/mavlink_orb_listener.cpp b/src/modules/mavlink/mavlink_orb_listener.cpp index 6d64569de5..418e1c1219 100644 --- a/src/modules/mavlink/mavlink_orb_listener.cpp +++ b/src/modules/mavlink/mavlink_orb_listener.cpp @@ -36,6 +36,7 @@ * Monitors ORB topics and sends update messages as appropriate. * * @author Lorenz Meier + * @author Julian Oes */ // XXX trim includes @@ -80,7 +81,7 @@ cm_uint16_from_m_float(float m) return (uint16_t)(m * 100.0f); } -MavlinkOrbListener::MavlinkOrbListener(Mavlink* parent) : +MavlinkOrbListener::MavlinkOrbListener(Mavlink* parent) : thread_should_exit(false), _loop_perf(perf_alloc(PC_ELAPSED, "mavlink orb")), @@ -88,31 +89,31 @@ MavlinkOrbListener::MavlinkOrbListener(Mavlink* parent) : _listeners(nullptr), _n_listeners(0) { - add_listener(MavlinkOrbListener::l_sensor_combined, &_mavlink->get_subs()->sensor_sub, 0); - add_listener(MavlinkOrbListener::l_vehicle_attitude, &_mavlink->get_subs()->att_sub, 0); - add_listener(MavlinkOrbListener::l_vehicle_gps_position, &_mavlink->get_subs()->gps_sub, 0); - add_listener(MavlinkOrbListener::l_vehicle_status, &status_sub, 0); - add_listener(MavlinkOrbListener::l_rc_channels, &rc_sub, 0); - add_listener(MavlinkOrbListener::l_input_rc, &_mavlink->get_subs()->input_rc_sub, 0); - add_listener(MavlinkOrbListener::l_global_position, &_mavlink->get_subs()->global_pos_sub, 0); - add_listener(MavlinkOrbListener::l_local_position, &_mavlink->get_subs()->local_pos_sub, 0); - add_listener(MavlinkOrbListener::l_global_position_setpoint, &_mavlink->get_subs()->triplet_sub, 0); - add_listener(MavlinkOrbListener::l_local_position_setpoint, &_mavlink->get_subs()->spl_sub, 0); - add_listener(MavlinkOrbListener::l_attitude_setpoint, &_mavlink->get_subs()->spa_sub, 0); - add_listener(MavlinkOrbListener::l_actuator_outputs, &_mavlink->get_subs()->act_0_sub, 0); - add_listener(MavlinkOrbListener::l_actuator_outputs, &_mavlink->get_subs()->act_1_sub, 1); - add_listener(MavlinkOrbListener::l_actuator_outputs, &_mavlink->get_subs()->act_2_sub, 2); - add_listener(MavlinkOrbListener::l_actuator_outputs, &_mavlink->get_subs()->act_3_sub, 3); - add_listener(MavlinkOrbListener::l_actuator_armed, &_mavlink->get_subs()->armed_sub, 0); - add_listener(MavlinkOrbListener::l_manual_control_setpoint, &_mavlink->get_subs()->man_control_sp_sub, 0); - add_listener(MavlinkOrbListener::l_vehicle_attitude_controls, &_mavlink->get_subs()->actuators_sub, 0); - add_listener(MavlinkOrbListener::l_debug_key_value, &_mavlink->get_subs()->debug_key_value, 0); - add_listener(MavlinkOrbListener::l_optical_flow, &_mavlink->get_subs()->optical_flow, 0); - add_listener(MavlinkOrbListener::l_vehicle_rates_setpoint, &_mavlink->get_subs()->rates_setpoint_sub, 0); - add_listener(MavlinkOrbListener::l_home, &_mavlink->get_subs()->home_sub, 0); - add_listener(MavlinkOrbListener::l_airspeed, &_mavlink->get_subs()->airspeed_sub, 0); - add_listener(MavlinkOrbListener::l_nav_cap, &_mavlink->get_subs()->navigation_capabilities_sub, 0); - add_listener(MavlinkOrbListener::l_control_mode, &_mavlink->get_subs()->control_mode_sub, 0); + add_listener(MavlinkOrbListener::l_sensor_combined, &(_mavlink->get_subs()->sensor_sub), 0); + add_listener(MavlinkOrbListener::l_vehicle_attitude, &(_mavlink->get_subs()->att_sub), 0); + add_listener(MavlinkOrbListener::l_vehicle_gps_position, &(_mavlink->get_subs()->gps_sub), 0); + add_listener(MavlinkOrbListener::l_vehicle_status, &(_mavlink->get_subs()->status_sub), 0); + add_listener(MavlinkOrbListener::l_home, &(_mavlink->get_subs()->home_sub), 0); + add_listener(MavlinkOrbListener::l_control_mode, &(_mavlink->get_subs()->control_mode_sub), 0); + add_listener(MavlinkOrbListener::l_rc_channels, &(_mavlink->get_subs()->rc_sub), 0); + add_listener(MavlinkOrbListener::l_input_rc, &(_mavlink->get_subs()->input_rc_sub), 0); + add_listener(MavlinkOrbListener::l_global_position, &(_mavlink->get_subs()->global_pos_sub), 0); + add_listener(MavlinkOrbListener::l_local_position, &(_mavlink->get_subs()->local_pos_sub), 0); + add_listener(MavlinkOrbListener::l_global_position_setpoint, &(_mavlink->get_subs()->triplet_sub), 0); + add_listener(MavlinkOrbListener::l_local_position_setpoint, &(_mavlink->get_subs()->spl_sub), 0); + add_listener(MavlinkOrbListener::l_attitude_setpoint, &(_mavlink->get_subs()->spa_sub), 0); + add_listener(MavlinkOrbListener::l_actuator_outputs, &(_mavlink->get_subs()->act_0_sub), 0); + add_listener(MavlinkOrbListener::l_actuator_outputs, &(_mavlink->get_subs()->act_1_sub), 1); + add_listener(MavlinkOrbListener::l_actuator_outputs, &(_mavlink->get_subs()->act_2_sub), 2); + add_listener(MavlinkOrbListener::l_actuator_outputs, &(_mavlink->get_subs()->act_3_sub), 3); + add_listener(MavlinkOrbListener::l_actuator_armed, &(_mavlink->get_subs()->armed_sub), 0); + add_listener(MavlinkOrbListener::l_manual_control_setpoint, &(_mavlink->get_subs()->man_control_sp_sub), 0); + add_listener(MavlinkOrbListener::l_vehicle_attitude_controls, &(_mavlink->get_subs()->actuators_sub), 0); + add_listener(MavlinkOrbListener::l_debug_key_value, &(_mavlink->get_subs()->debug_key_value), 0); + add_listener(MavlinkOrbListener::l_optical_flow, &(_mavlink->get_subs()->optical_flow), 0); + add_listener(MavlinkOrbListener::l_vehicle_rates_setpoint, &(_mavlink->get_subs()->rates_setpoint_sub), 0); + add_listener(MavlinkOrbListener::l_airspeed, &(_mavlink->get_subs()->airspeed_sub), 0); + add_listener(MavlinkOrbListener::l_nav_cap, &(_mavlink->get_subs()->navigation_capabilities_sub), 0); } @@ -124,15 +125,18 @@ void MavlinkOrbListener::add_listener(void (*callback)(const struct listener *l) nl->subp = subp; nl->arg = arg; nl->next = nullptr; + nl->mavlink = _mavlink; + nl->listener = this; - // Register it - struct listener *next = _listeners; - while (next->next != nullptr) { - next = next->next; + if (_listeners == nullptr) { + _listeners = nl; + } else { + struct listener *next = _listeners; + while (next->next != nullptr) { + next = next->next; + } + next->next = nl; } - - // Attach - next->next = nl; _n_listeners++; } @@ -271,7 +275,7 @@ void MavlinkOrbListener::l_vehicle_status(const struct listener *l) { /* immediately communicate state _mavlink->get_chan()ges back to user */ - orb_copy(ORB_ID(vehicle_status), l->listener->status_sub, &l->listener->v_status); + orb_copy(ORB_ID(vehicle_status), l->mavlink->get_subs()->status_sub, &(l->mavlink->v_status)); orb_copy(ORB_ID(actuator_armed), l->mavlink->get_subs()->armed_sub, &l->listener->armed); /* enable or disable HIL */ diff --git a/src/modules/mavlink/mavlink_orb_listener.h b/src/modules/mavlink/mavlink_orb_listener.h index c9f35a1fbd..349a11f2e9 100644 --- a/src/modules/mavlink/mavlink_orb_listener.h +++ b/src/modules/mavlink/mavlink_orb_listener.h @@ -36,6 +36,7 @@ * MAVLink 1.0 protocol interface definition. * * @author Lorenz Meier + * @author Julian Oes */ #include @@ -106,7 +107,7 @@ public: void (*callback)(const struct listener *l); int *subp; uintptr_t arg; - struct listener* next; + struct listener *next; Mavlink *mavlink; MavlinkOrbListener* listener; }; @@ -172,9 +173,6 @@ private: struct airspeed_s airspeed; struct home_position_s home; - int status_sub; - int rc_sub; - unsigned int sensors_raw_counter; unsigned int attitude_counter; unsigned int gps_counter; diff --git a/src/modules/uORB/topics/vehicle_attitude.h b/src/modules/uORB/topics/vehicle_attitude.h index e5a35ff9bc..40328af146 100755 --- a/src/modules/uORB/topics/vehicle_attitude.h +++ b/src/modules/uORB/topics/vehicle_attitude.h @@ -76,7 +76,7 @@ struct vehicle_attitude_s { float rate_offsets[3]; /**< Offsets of the body angular rates from zero */ float R[3][3]; /**< Rotation matrix body to world, (Tait-Bryan, NED) */ float q[4]; /**< Quaternion (NED) */ - float g_comp[3]; /**< Compensated gravity vector */ + float g_comp[3]; /**< Compensated gravity vector */ bool R_valid; /**< Rotation matrix valid */ bool q_valid; /**< Quaternion valid */ From 4b567ef6310c07d46307437ac9ba76c76a5b2c69 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 11 Feb 2014 23:59:02 +0100 Subject: [PATCH 059/381] gpio_led: bugs fixed, PX4FMUv2 support added --- makefiles/config_px4fmu-v2_default.mk | 1 + src/modules/gpio_led/gpio_led.c | 80 ++++++++++++++++++--------- 2 files changed, 55 insertions(+), 26 deletions(-) diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 2ad2f63a95..3a37438938 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -71,6 +71,7 @@ MODULES += modules/commander MODULES += modules/navigator MODULES += modules/mavlink MODULES += modules/mavlink_onboard +MODULES += modules/gpio_led # # Estimation modules (EKF/ SO3 / other filters) diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index d383146f99..f5f3dea761 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -51,7 +51,6 @@ #include #include #include -#include #include #include #include @@ -63,8 +62,6 @@ struct gpio_led_s { int pin; struct vehicle_status_s status; int vehicle_status_sub; - struct actuator_armed_s armed; - int actuator_armed_sub; bool led_state; int counter; }; @@ -81,6 +78,7 @@ void gpio_led_cycle(FAR void *arg); int gpio_led_main(int argc, char *argv[]) { if (argc < 2) { +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 errx(1, "usage: gpio_led {start|stop} [-p <1|2|a1|a2|r1|r2>]\n" "\t-p\tUse pin:\n" "\t\t1\tPX4FMU GPIO_EXT1 (default)\n" @@ -88,7 +86,15 @@ int gpio_led_main(int argc, char *argv[]) "\t\ta1\tPX4IO ACC1\n" "\t\ta2\tPX4IO ACC2\n" "\t\tr1\tPX4IO RELAY1\n" - "\t\tr2\tPX4IO RELAY2"); + "\t\tr2\tPX4IO RELAY2" + ); +#endif +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + errx(1, "usage: gpio_led {start|stop} [-p ]\n" + "\t-p\tUse pin:\n" + "\t\tn\tAUX OUT pin number (default: 1)\n" + ); +#endif } else { @@ -98,10 +104,14 @@ int gpio_led_main(int argc, char *argv[]) } bool use_io = false; - int pin = GPIO_EXT_1; + + /* by default use GPIO_EXT_1 on FMUv1 and GPIO_SERVO_1 on FMUv2 */ + int pin = 1; if (argc > 2) { if (!strcmp(argv[2], "-p")) { +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + if (!strcmp(argv[3], "1")) { use_io = false; pin = GPIO_EXT_1; @@ -129,6 +139,20 @@ int gpio_led_main(int argc, char *argv[]) } else { errx(1, "unsupported pin: %s", argv[3]); } + +#endif +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + unsigned int n = strtoul(argv[3], NULL, 10); + + if (n >= 1 && n <= 6) { + use_io = false; + pin = n; + + } else { + errx(1, "unsupported pin: %s", argv[3]); + } + +#endif } } @@ -144,6 +168,8 @@ int gpio_led_main(int argc, char *argv[]) gpio_led_started = true; char pin_name[24]; +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + if (use_io) { if (pin & (PX4IO_P_SETUP_RELAYS_ACC1 | PX4IO_P_SETUP_RELAYS_ACC2)) { sprintf(pin_name, "PX4IO ACC%i", (pin >> 3)); @@ -154,9 +180,13 @@ int gpio_led_main(int argc, char *argv[]) } else { sprintf(pin_name, "PX4FMU GPIO_EXT%i", pin); - } +#endif +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + sprintf(pin_name, "AUX OUT %i", pin); +#endif + warnx("start, using pin: %s", pin_name); } @@ -186,6 +216,7 @@ void gpio_led_start(FAR void *arg) if (priv->use_io) { gpio_dev = PX4IO_DEVICE_PATH; + } else { gpio_dev = PX4FMU_DEVICE_PATH; } @@ -204,8 +235,10 @@ void gpio_led_start(FAR void *arg) /* px4fmu only, px4io doesn't support GPIO_SET_OUTPUT and will ignore */ ioctl(priv->gpio_fd, GPIO_SET_OUTPUT, priv->pin); - /* subscribe to vehicle status topic */ + /* initialize vehicle status structure */ memset(&priv->status, 0, sizeof(priv->status)); + + /* subscribe to vehicle status topic */ priv->vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); /* add worker to queue */ @@ -224,38 +257,32 @@ void gpio_led_cycle(FAR void *arg) FAR struct gpio_led_s *priv = (FAR struct gpio_led_s *)arg; /* check for status updates*/ - bool status_updated; - orb_check(priv->vehicle_status_sub, &status_updated); + bool updated; + orb_check(priv->vehicle_status_sub, &updated); - if (status_updated) + if (updated) orb_copy(ORB_ID(vehicle_status), priv->vehicle_status_sub, &priv->status); - orb_check(priv->vehicle_status_sub, &status_updated); - - if (status_updated) - orb_copy(ORB_ID(actuator_armed), priv->actuator_armed_sub, &priv->armed); - /* select pattern for current status */ int pattern = 0; - if (priv->armed.armed) { - if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { + if (priv->status.arming_state == ARMING_STATE_ARMED_ERROR) { + pattern = 0x2A; // *_*_*_ fast blink (armed, error) + + } else if (priv->status.arming_state == ARMING_STATE_ARMED) { + if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE && priv->status.failsafe_state == FAILSAFE_STATE_NORMAL) { pattern = 0x3f; // ****** solid (armed) } else { - pattern = 0x2A; // *_*_*_ fast blink (armed, battery warning) + pattern = 0x3e; // *****_ slow blink (armed, battery low or failsafe) } - } else { - if (priv->armed.ready_to_arm) { - pattern = 0x00; // ______ off (disarmed, preflight check) + } else if (priv->status.arming_state == ARMING_STATE_STANDBY) { + pattern = 0x38; // ***___ slow blink (disarmed, ready) - } else if (priv->armed.ready_to_arm && priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { - pattern = 0x38; // ***___ slow blink (disarmed, ready) + } else if (priv->status.arming_state == ARMING_STATE_STANDBY_ERROR) { + pattern = 0x28; // *_*___ slow double blink (disarmed, error) - } else { - pattern = 0x28; // *_*___ slow double blink (disarmed, not good to arm) - } } /* blink pattern */ @@ -266,6 +293,7 @@ void gpio_led_cycle(FAR void *arg) if (led_state_new) { ioctl(priv->gpio_fd, GPIO_SET, priv->pin); + } else { ioctl(priv->gpio_fd, GPIO_CLEAR, priv->pin); } From 99b426c27c004b6430c4058151b0dd3854989414 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 12 Feb 2014 12:38:35 +0100 Subject: [PATCH 060/381] Mavlink: bring mavlink log messages to life --- src/include/mavlink/mavlink_log.h | 3 ++ src/modules/mavlink/mavlink_main.cpp | 56 +++++++++++++++------------- src/modules/mavlink/mavlink_main.h | 5 +-- 3 files changed, 36 insertions(+), 28 deletions(-) diff --git a/src/include/mavlink/mavlink_log.h b/src/include/mavlink/mavlink_log.h index 5054937e07..0ea655cac3 100644 --- a/src/include/mavlink/mavlink_log.h +++ b/src/include/mavlink/mavlink_log.h @@ -100,6 +100,7 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...); */ #define mavlink_log_info(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_INFO, _text, ##__VA_ARGS__); + struct mavlink_logmessage { char text[MAVLINK_LOG_MAXLEN + 1]; unsigned char severity; @@ -112,6 +113,7 @@ struct mavlink_logbuffer { struct mavlink_logmessage *elems; }; +__BEGIN_DECLS void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size); void mavlink_logbuffer_destroy(struct mavlink_logbuffer *lb); @@ -125,6 +127,7 @@ void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_ int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem); void mavlink_logbuffer_vasprintf(struct mavlink_logbuffer *lb, int severity, const char *fmt, ...); +__END_DECLS #endif diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index f1ec6e8dce..4f4d949476 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -90,6 +90,12 @@ static const int ERROR = -1; static Mavlink* _head = nullptr; +/* TODO: if this is a class member it crashes */ +static struct file_operations fops; + +static struct mavlink_logbuffer lb; +static unsigned int total_counter; + /** * mavlink app start / stop handling function * @@ -152,7 +158,7 @@ namespace mavlink } Mavlink::Mavlink() : -// _mavlink_fd(-1), + _mavlink_fd(-1), _next(nullptr), _task_should_exit(false), thread_running(false), @@ -277,14 +283,15 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) case (int)MAVLINK_IOC_SEND_TEXT_INFO: case (int)MAVLINK_IOC_SEND_TEXT_CRITICAL: case (int)MAVLINK_IOC_SEND_TEXT_EMERGENCY: { - const char *txt = (const char *)arg; - printf("logmsg: %s\n", txt); - //struct mavlink_logmessage msg; - //strncpy(msg.text, txt, sizeof(msg.text)); - //mavlink_logbuffer_write(&lb, &msg); - //total_counter++; - return OK; - } + + const char *txt = (const char *)arg; +// printf("logmsg: %s\n", txt); + struct mavlink_logmessage msg; + strncpy(msg.text, txt, sizeof(msg.text)); + mavlink_logbuffer_write(&lb, &msg); + total_counter++; + return OK; + } default: return ENOTTY; @@ -1435,14 +1442,8 @@ Mavlink::task_main(int argc, char *argv[]) warnx("Initializing.."); fflush(stdout); - /* initialize logging device */ - // TODO -// _mavlink_fd = -1;//open(MAVLINK_LOG_DEVICE, 0); - - //mavlink_log_info(_mavlink_fd, "[mavlink] started"); - /* initialize mavlink text message buffering */ - // mavlink_logbuffer_init(&lb, 10); + mavlink_logbuffer_init(&lb, 10); int ch; const char *device_name = "/dev/ttyS1"; @@ -1519,7 +1520,12 @@ Mavlink::task_main(int argc, char *argv[]) err(1, "could not open %s", device_name); /* create the device node that's used for sending text log messages, etc. */ - //register_driver(MAVLINK_LOG_DEVICE, &fops, 0666, NULL); + register_driver(MAVLINK_LOG_DEVICE, &fops, 0666, NULL); + + /* initialize logging device */ + _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + + mavlink_log_info(_mavlink_fd, "[mavlink] started"); /* Initialize system properties */ mavlink_update_system(); @@ -1693,15 +1699,15 @@ Mavlink::task_main(int argc, char *argv[]) mavlink_pm_queued_send(); } - // /* send one string at 10 Hz */ - // if (!mavlink_logbuffer_is_empty(&lb)) { - // struct mavlink_logmessage msg; - // int lb_ret = mavlink_logbuffer_read(&lb, &msg); + /* send one string at 10 Hz */ + if (!mavlink_logbuffer_is_empty(&lb)) { + struct mavlink_logmessage msg; + int lb_ret = mavlink_logbuffer_read(&lb, &msg); - // if (lb_ret == OK) { - // mavlink_missionlib_send_gcs_string(msg.text); - // } - // } + if (lb_ret == OK) { + mavlink_missionlib_send_gcs_string(msg.text); + } + } perf_end(_loop_perf); } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index fb71128919..9b289c9658 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -264,11 +264,11 @@ protected: * Pointer to the default cdev file operations table; useful for * registering clone devices etc. */ - struct file_operations fops; + Mavlink* _next; private: - + int _mavlink_fd; bool _task_should_exit; /**< if true, mavlink task should exit */ bool thread_running; int _mavlink_task; /**< task handle for sensor task */ @@ -311,7 +311,6 @@ private: */ unsigned int mavlink_param_queue_index; - struct mavlink_logbuffer lb; bool mavlink_link_termination_allowed; /** From 76ae004e5c6c1dcc05f1eb784f6dc14cff2a3671 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 12 Feb 2014 16:21:50 +0100 Subject: [PATCH 061/381] Defconfig: allow for more file descriptors --- nuttx-configs/px4fmu-v1/nsh/defconfig | 2 +- nuttx-configs/px4fmu-v2/nsh/defconfig | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index 1dc96b3c3f..2e82b8a2d9 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -405,7 +405,7 @@ CONFIG_SIG_SIGWORK=4 CONFIG_MAX_TASKS=32 CONFIG_MAX_TASK_ARGS=10 CONFIG_NPTHREAD_KEYS=4 -CONFIG_NFILE_DESCRIPTORS=32 +CONFIG_NFILE_DESCRIPTORS=64 CONFIG_NFILE_STREAMS=8 CONFIG_NAME_MAX=32 CONFIG_PREALLOC_MQ_MSGS=4 diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 2a734c27e5..0508726777 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -439,7 +439,7 @@ CONFIG_SIG_SIGWORK=4 CONFIG_MAX_TASKS=32 CONFIG_MAX_TASK_ARGS=10 CONFIG_NPTHREAD_KEYS=4 -CONFIG_NFILE_DESCRIPTORS=32 +CONFIG_NFILE_DESCRIPTORS=64 CONFIG_NFILE_STREAMS=8 CONFIG_NAME_MAX=32 CONFIG_PREALLOC_MQ_MSGS=4 From cc5756f61ffa9b84abcbebfcf89d213d99d100d9 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 12 Feb 2014 18:35:45 +0100 Subject: [PATCH 062/381] Mavlink: enable log messages to multiple UARTs --- src/modules/mavlink/mavlink_main.cpp | 20 ++++++++++++-------- src/modules/mavlink/mavlink_main.h | 3 +++ 2 files changed, 15 insertions(+), 8 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 2585593eaa..7e146604b7 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -93,9 +93,6 @@ static Mavlink* _head = nullptr; /* TODO: if this is a class member it crashes */ static struct file_operations fops; -static struct mavlink_logbuffer lb; -static unsigned int total_counter; - /** * mavlink app start / stop handling function * @@ -227,11 +224,11 @@ Mavlink* Mavlink::new_instance() ::_head = inst; /* afterwards follow the next and append the instance */ } else { - while (next != nullptr) { + while (next->_next != nullptr) { next = next->_next; } /* now parent has a null pointer, fill it */ - next = inst; + next->_next = inst; } return inst; } @@ -288,8 +285,15 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) // printf("logmsg: %s\n", txt); struct mavlink_logmessage msg; strncpy(msg.text, txt, sizeof(msg.text)); - mavlink_logbuffer_write(&lb, &msg); - total_counter++; + + Mavlink* inst = ::_head; + while (inst != nullptr) { + + mavlink_logbuffer_write(&inst->lb, &msg); + inst->total_counter++; + inst = inst->_next; + + } return OK; } @@ -1451,7 +1455,7 @@ Mavlink::task_main(int argc, char *argv[]) fflush(stdout); /* initialize mavlink text message buffering */ - mavlink_logbuffer_init(&lb, 10); + mavlink_logbuffer_init(&lb, 5); int ch; const char *device_name = "/dev/ttyS1"; diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index ce2d2b34a2..b812af95a8 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -293,6 +293,9 @@ private: // XXX probably should be in a header... // extern pthread_t receive_start(int uart); + struct mavlink_logbuffer lb; + unsigned int total_counter; + pthread_t receive_thread; pthread_t uorb_receive_thread; From 61a849bf6ba11b98b7332f89d0b32226601f4d63 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 12 Feb 2014 19:13:57 +0100 Subject: [PATCH 063/381] Mavlink: don't allow multiple instances on the same device --- src/modules/mavlink/mavlink_main.cpp | 19 ++++++++++++++++++- src/modules/mavlink/mavlink_main.h | 4 ++++ 2 files changed, 22 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 7e146604b7..cbbe3c31f7 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -155,6 +155,7 @@ namespace mavlink } Mavlink::Mavlink() : + device_name("/dev/ttyS1"), _mavlink_fd(-1), _next(nullptr), _task_should_exit(false), @@ -249,6 +250,19 @@ Mavlink* Mavlink::get_instance(unsigned instance) return inst; } +bool Mavlink::instance_exists(const char *device_name, Mavlink *self) +{ + Mavlink* inst = ::_head; + while (inst != nullptr) { + + /* don't compare with itself */ + if (inst != self && !strcmp(device_name, inst->device_name)) + return true; + inst = inst->_next; + } + return false; +} + int Mavlink::get_uart_fd(unsigned index) { Mavlink* inst = get_instance(index); @@ -1458,7 +1472,6 @@ Mavlink::task_main(int argc, char *argv[]) mavlink_logbuffer_init(&lb, 5); int ch; - const char *device_name = "/dev/ttyS1"; _baudrate = 57600; _chan = MAVLINK_COMM_0; @@ -1496,6 +1509,10 @@ Mavlink::task_main(int argc, char *argv[]) } } + if (Mavlink::instance_exists(device_name, this)) { + errx(1, "mavlink instance for %s already running", device_name); + } + struct termios uart_config_original; bool usb_uart; diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index b812af95a8..bf7675267c 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -168,10 +168,14 @@ public: static Mavlink* get_instance(unsigned instance); + static bool instance_exists(const char *device_name, Mavlink *self); + static int get_uart_fd(unsigned index); int get_uart_fd() { return _uart; } + const char *device_name; + enum MAVLINK_MODE { MODE_TX_HEARTBEAT_ONLY=0, MODE_OFFBOARD, From 346ae5b9f4fc2da13e6d890521f48768b6b6e8c2 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 13 Feb 2014 19:13:10 +0100 Subject: [PATCH 064/381] Mavlink: allow to stop (WIP) --- src/modules/mavlink/mavlink_main.cpp | 62 +++++++++++++++----- src/modules/mavlink/mavlink_main.h | 5 +- src/modules/mavlink/mavlink_orb_listener.cpp | 3 +- src/modules/mavlink/mavlink_orb_listener.h | 2 - src/modules/mavlink/mavlink_receiver.cpp | 5 +- src/modules/mavlink/mavlink_receiver.h | 2 - 6 files changed, 54 insertions(+), 25 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index cbbe3c31f7..0d9d061900 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -250,6 +250,43 @@ Mavlink* Mavlink::get_instance(unsigned instance) return inst; } +int Mavlink::destroy_all_instances() +{ + /* start deleting from the end */ + Mavlink *inst_to_del = nullptr; + Mavlink *next_inst = ::_head; + + unsigned iterations = 0; + + warnx("waiting for instances to stop"); + while (next_inst != nullptr) { + + inst_to_del = next_inst; + next_inst = inst_to_del->_next; + + /* set flag to stop thread and wait for all threads to finish */ + inst_to_del->_task_should_exit = true; + while (inst_to_del->thread_running) { + printf("."); + usleep(10000); + iterations++; + + if (iterations > 10000) { + warnx("ERROR: Couldn't stop all mavlink instances."); + return ERROR; + } + } + delete inst_to_del; + } + + /* reset head */ + ::_head = nullptr; + + printf("\n"); + warnx("all instances stopped"); + return OK; +} + bool Mavlink::instance_exists(const char *device_name, Mavlink *self) { Mavlink* inst = ::_head; @@ -1495,9 +1532,9 @@ Mavlink::task_main(int argc, char *argv[]) device_name = optarg; break; - case 'e': - mavlink_link_termination_allowed = true; - break; +// case 'e': +// mavlink_link_termination_allowed = true; +// break; case 'o': _mode = MODE_ONBOARD; @@ -1749,7 +1786,7 @@ Mavlink::task_main(int argc, char *argv[]) tcsetattr(_uart, TCSANOW, &uart_config_original); /* destroy log buffer */ - //mavlink_logbuffer_destroy(&lb); + mavlink_logbuffer_destroy(&lb); thread_running = false; @@ -1782,7 +1819,7 @@ Mavlink::status() static void usage() { - errx(1, "usage: mavlink {start|stop|status}"); + errx(1, "usage: mavlink {start|stop|status} [-d device] [-b baudrate] [-o]"); } int mavlink_main(int argc, char *argv[]) @@ -1830,21 +1867,16 @@ int mavlink_main(int argc, char *argv[]) // } return 0; - } - // if (mavlink::g_mavlink == nullptr) - // errx(1, "not running"); - - // if (!strcmp(argv[1], "stop")) { - // delete mavlink::g_mavlink; - // mavlink::g_mavlink = nullptr; + } else if (!strcmp(argv[1], "stop")) { + return Mavlink::destroy_all_instances(); // } else if (!strcmp(argv[1], "status")) { // mavlink::g_mavlink->status(); - // } else { - // usage(); - // } + } else { + usage(); + } return 0; } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index bf7675267c..d5bbb746b9 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -168,6 +168,8 @@ public: static Mavlink* get_instance(unsigned instance); + static int destroy_all_instances(); + static bool instance_exists(const char *device_name, Mavlink *self); static int get_uart_fd(unsigned index); @@ -263,6 +265,8 @@ public: /** Position setpoint triplet */ struct position_setpoint_triplet_s pos_sp_triplet; + bool _task_should_exit; /**< if true, mavlink task should exit */ + protected: /** * Pointer to the default cdev file operations table; useful for @@ -273,7 +277,6 @@ protected: private: int _mavlink_fd; - bool _task_should_exit; /**< if true, mavlink task should exit */ bool thread_running; int _mavlink_task; /**< task handle for sensor task */ diff --git a/src/modules/mavlink/mavlink_orb_listener.cpp b/src/modules/mavlink/mavlink_orb_listener.cpp index cd9408f2ff..a2b71c931c 100644 --- a/src/modules/mavlink/mavlink_orb_listener.cpp +++ b/src/modules/mavlink/mavlink_orb_listener.cpp @@ -83,7 +83,6 @@ cm_uint16_from_m_float(float m) MavlinkOrbListener::MavlinkOrbListener(Mavlink* parent) : - thread_should_exit(false), _loop_perf(perf_alloc(PC_ELAPSED, "mavlink orb")), _mavlink(parent), _listeners(nullptr), @@ -678,7 +677,7 @@ MavlinkOrbListener::uorb_receive_thread(void *arg) /* Invoke callback to set initial state */ //listeners[i].callback(&listener[i]); - while (!thread_should_exit) { + while (!_mavlink->_task_should_exit) { int poll_ret = poll(fds, _n_listeners, timeout); diff --git a/src/modules/mavlink/mavlink_orb_listener.h b/src/modules/mavlink/mavlink_orb_listener.h index 560b474230..317a489d4b 100644 --- a/src/modules/mavlink/mavlink_orb_listener.h +++ b/src/modules/mavlink/mavlink_orb_listener.h @@ -116,8 +116,6 @@ public: private: - bool thread_should_exit; /**< if true, sensor task should exit */ - perf_counter_t _loop_perf; /**< loop performance counter */ Mavlink* _mavlink; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 89dcb1d7bb..16a1b9aff6 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -106,8 +106,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : telemetry_status_pub(-1), lat0(0), lon0(0), - alt0(0), - thread_should_exit(false) + alt0(0) { } @@ -807,7 +806,7 @@ MavlinkReceiver::receive_thread(void *arg) ssize_t nread = 0; - while (!thread_should_exit) { + while (!_mavlink->_task_should_exit) { if (poll(fds, 1, timeout) > 0) { if (nread < sizeof(buf)) { /* to avoid reading very small chunks wait for data before reading */ diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index d8d3b54529..6614e13f45 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -101,8 +101,6 @@ public: private: - bool thread_should_exit; /**< if true, sensor task should exit */ - perf_counter_t _loop_perf; /**< loop performance counter */ Mavlink* _mavlink; From ef46cd5e909115c8c208c409fcded0dd02037d5e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 13 Feb 2014 20:54:10 +0100 Subject: [PATCH 065/381] Mavlink: allow to stop (compiling, working) --- 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 16a1b9aff6..44fff24166 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -129,7 +129,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) usleep(50000); /* terminate other threads and this thread */ - thread_should_exit = true; + _mavlink->_task_should_exit = true; } else { From 523637e0f1fb0247111818d0a88ce8c4574728ba Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 14 Feb 2014 13:36:59 +0100 Subject: [PATCH 066/381] Mavlink: Start multiple uart listeners, HIL working --- src/modules/mavlink/mavlink_main.cpp | 41 +++++++++++---- src/modules/mavlink/mavlink_main.h | 55 +++++++------------- src/modules/mavlink/mavlink_orb_listener.cpp | 12 ++--- src/modules/mavlink/mavlink_orb_listener.h | 23 +------- src/modules/mavlink/mavlink_receiver.cpp | 16 +++--- src/modules/mavlink/mavlink_receiver.h | 3 +- 6 files changed, 68 insertions(+), 82 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 0d9d061900..79aa6135b3 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -309,6 +309,16 @@ int Mavlink::get_uart_fd(unsigned index) return -1; } +int Mavlink::get_uart_fd() +{ + return _uart; +} + +int Mavlink::get_channel() +{ + return (int)_chan; +} + void Mavlink::parameters_update() { @@ -945,7 +955,7 @@ void Mavlink::mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8 wpa.target_component = compid; wpa.type = type; - mavlink_msg_mission_ack_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpa); + mavlink_msg_mission_ack_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpa); mavlink_missionlib_send_message(&msg); if (verbose) warnx("Sent waypoint ack (%u) to ID %u", wpa.type, wpa.target_system); @@ -968,7 +978,7 @@ void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq) wpc.seq = seq; - mavlink_msg_mission_current_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpc); + mavlink_msg_mission_current_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpc); mavlink_missionlib_send_message(&msg); if (verbose) warnx("Broadcasted new current waypoint %u", wpc.seq); @@ -988,7 +998,7 @@ void Mavlink::mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uin wpc.target_component = compid; wpc.count = mission.count; - mavlink_msg_mission_count_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpc); + mavlink_msg_mission_count_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpc); mavlink_missionlib_send_message(&msg); if (verbose) warnx("Sent waypoint count (%u) to ID %u", wpc.count, wpc.target_system); @@ -1018,7 +1028,7 @@ void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t wp.target_system = sysid; wp.target_component = compid; wp.seq = seq; - mavlink_msg_mission_item_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wp); + mavlink_msg_mission_item_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wp); mavlink_missionlib_send_message(&msg); if (verbose) warnx("Sent waypoint %u to ID %u", wp.seq, wp.target_system); @@ -1036,7 +1046,7 @@ void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, u wpr.target_system = sysid; wpr.target_component = compid; wpr.seq = seq; - mavlink_msg_mission_request_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpr); + mavlink_msg_mission_request_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpr); mavlink_missionlib_send_message(&msg); if (verbose) warnx("Sent waypoint request %u to ID %u", wpr.seq, wpr.target_system); @@ -1061,7 +1071,7 @@ void Mavlink::mavlink_wpm_send_waypoint_reached(uint16_t seq) wp_reached.seq = seq; - mavlink_msg_mission_item_reached_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wp_reached); + mavlink_msg_mission_item_reached_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wp_reached); mavlink_missionlib_send_message(&msg); if (verbose) warnx("Sent waypoint %u reached message", wp_reached.seq); @@ -1318,7 +1328,7 @@ void Mavlink::mavlink_wpm_message_handler(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 == mavlink_wpm_comp_id) { + if (wp.target_system == mavlink_system.sysid && wp.target_component == _mavlink_wpm_comp_id) { wpm->timestamp_lastaction = now; @@ -1576,6 +1586,21 @@ Mavlink::task_main(int argc, char *argv[]) break; } + switch(_mode) { + case MODE_OFFBOARD: + case MODE_HIL: + _mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER; + break; + case MODE_ONBOARD: + _mavlink_wpm_comp_id = MAV_COMP_ID_CAMERA; + break; + case MODE_TX_HEARTBEAT_ONLY: + default: + _mavlink_wpm_comp_id = MAV_COMP_ID_ALL; + warnx("Error: Unknown mode"); + break; + } + /* Flush stdout in case MAVLink is about to take it over */ fflush(stdout); @@ -1597,11 +1622,9 @@ Mavlink::task_main(int argc, char *argv[]) mavlink_update_system(); /* start the MAVLink receiver */ -// MavlinkReceiver rcv(this); receive_thread = MavlinkReceiver::receive_start(this); /* start the ORB receiver */ -// MavlinkOrbListener listener(this); uorb_receive_thread = MavlinkOrbListener::uorb_receive_start(this); /* initialize waypoint manager */ diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index d5bbb746b9..c667a41daa 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -96,46 +96,27 @@ enum MAVLINK_WPM_CODES { }; -/* WAYPOINT MANAGER - MISSION LIB */ - -#define MAVLINK_WPM_MAX_WP_COUNT 15 -#define MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE ///< Enable double buffer and in-flight updates -#ifndef MAVLINK_WPM_TEXT_FEEDBACK -#define MAVLINK_WPM_TEXT_FEEDBACK 0 ///< Report back status information as text -#endif -#define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication timeout in useconds -#define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000 ///< When to send a new setpoint +#define MAVLINK_WPM_MAX_WP_COUNT 255 +#define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication timeout in useconds +#define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000 ///< When to send a new setpoint #define MAVLINK_WPM_PROTOCOL_DELAY_DEFAULT 40000 struct mavlink_wpm_storage { - mavlink_mission_item_t waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Currently active waypoints -#ifdef MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE - mavlink_mission_item_t rcv_waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Receive buffer for next waypoints -#endif - uint16_t size; - uint16_t max_size; - uint16_t rcv_size; - enum MAVLINK_WPM_STATES current_state; - int16_t current_wp_id; ///< Waypoint in current transmission - int16_t current_active_wp_id; ///< Waypoint the system is currently heading towards - uint16_t current_count; - uint8_t current_partner_sysid; - uint8_t current_partner_compid; - uint64_t timestamp_lastaction; - uint64_t timestamp_last_send_setpoint; - uint64_t timestamp_firstinside_orbit; - uint64_t timestamp_lastoutside_orbit; - uint32_t timeout; - uint32_t delay_setpoint; - float accept_range_yaw; - float accept_range_distance; - bool yaw_reached; - bool pos_reached; - bool idle; - int current_dataman_id; + uint16_t size; + uint16_t max_size; + enum MAVLINK_WPM_STATES current_state; + int16_t current_wp_id; ///< Waypoint in current transmission + uint16_t current_count; + uint8_t current_partner_sysid; + uint8_t current_partner_compid; + uint64_t timestamp_lastaction; + uint64_t timestamp_last_send_setpoint; + uint32_t timeout; + int current_dataman_id; }; + class Mavlink { @@ -174,7 +155,9 @@ public: static int get_uart_fd(unsigned index); - int get_uart_fd() { return _uart; } + int get_uart_fd(); + + int get_channel(); const char *device_name; @@ -294,7 +277,7 @@ private: uint8_t missionlib_msg_buf[300]; //XXX MAGIC NUMBER MAVLINK_MODE _mode; - uint8_t mavlink_wpm_comp_id; + uint8_t _mavlink_wpm_comp_id; mavlink_channel_t _chan; // XXX probably should be in a header... diff --git a/src/modules/mavlink/mavlink_orb_listener.cpp b/src/modules/mavlink/mavlink_orb_listener.cpp index a2b71c931c..fdc196371b 100644 --- a/src/modules/mavlink/mavlink_orb_listener.cpp +++ b/src/modules/mavlink/mavlink_orb_listener.cpp @@ -55,14 +55,14 @@ #include #include #include +#include + #include #include "mavlink_orb_listener.h" #include "mavlink_main.h" - void *uorb_receive_thread(void *arg); - uint16_t cm_uint16_from_m_float(float m); @@ -647,9 +647,9 @@ void * MavlinkOrbListener::uorb_receive_thread(void *arg) { /* Set thread name */ - char buf[32]; - sprintf(buf, "mavlink rcv%d", Mavlink::instance_count()); - prctl(PR_SET_NAME, buf, getpid()); + char thread_name[18]; + sprintf(thread_name, "mavlink_uorb_rcv_%d", _mavlink->get_channel()); + prctl(PR_SET_NAME, thread_name, getpid()); /* * set up poll to block for new data, @@ -822,7 +822,7 @@ MavlinkOrbListener::uorb_receive_start(Mavlink* mavlink) pthread_attr_setstacksize(&uorb_attr, 2048); pthread_t thread; - pthread_create(&thread, &uorb_attr, MavlinkOrbListener::uorb_start_helper, mavlink); + pthread_create(&thread, &uorb_attr, MavlinkOrbListener::uorb_start_helper, (void*)mavlink); pthread_attr_destroy(&uorb_attr); return thread; diff --git a/src/modules/mavlink/mavlink_orb_listener.h b/src/modules/mavlink/mavlink_orb_listener.h index 317a489d4b..26a2e832f7 100644 --- a/src/modules/mavlink/mavlink_orb_listener.h +++ b/src/modules/mavlink/mavlink_orb_listener.h @@ -87,20 +87,7 @@ public: */ ~MavlinkOrbListener(); - // * - // * Start the mavlink task. - // * - // * @return OK on success. - - // int start(); - - /** - * Display the mavlink status. - */ - void status(); - static pthread_t uorb_receive_start(Mavlink *mavlink); - void * uorb_receive_thread(void *arg); struct listener { void (*callback)(const struct listener *l); @@ -124,15 +111,7 @@ private: unsigned _n_listeners; static const unsigned _max_listeners = 32; - /** - * Shim for calling task_main from task_create. - */ - void task_main_trampoline(int argc, char *argv[]); - - /** - * Main sensor collection task. - */ - void task_main() __attribute__((noreturn)); + void *uorb_receive_thread(void *arg); static void l_sensor_combined(const struct listener *l); static void l_vehicle_attitude(const struct listener *l); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 44fff24166..371f945c4c 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -798,7 +798,10 @@ MavlinkReceiver::receive_thread(void *arg) mavlink_message_t msg; - prctl(PR_SET_NAME, "mavlink_uart_rcv", getpid()); + /* Set thread name */ + char thread_name[18]; + sprintf(thread_name, "mavlink_uart_rcv_%d", _mavlink->get_channel()); + prctl(PR_SET_NAME, thread_name, getpid()); struct pollfd fds[1]; fds[0].fd = uart_fd; @@ -842,20 +845,19 @@ void MavlinkReceiver::print_status() void * MavlinkReceiver::start_helper(void *context) { - MavlinkReceiver *rcv = new MavlinkReceiver(((Mavlink *)context)); + MavlinkReceiver *rcv = new MavlinkReceiver((Mavlink*)context); return rcv->receive_thread(NULL); } pthread_t -MavlinkReceiver::receive_start(Mavlink *mavlink) +MavlinkReceiver::receive_start(Mavlink *parent) { - pthread_attr_t receiveloop_attr; pthread_attr_init(&receiveloop_attr); // set to non-blocking read - int flags = fcntl(mavlink->get_uart_fd(), F_GETFL, 0); - fcntl(mavlink->get_uart_fd(), F_SETFL, flags | O_NONBLOCK); + int flags = fcntl(parent->get_uart_fd(), F_GETFL, 0); + fcntl(parent->get_uart_fd(), F_SETFL, flags | O_NONBLOCK); struct sched_param param; (void)pthread_attr_getschedparam(&receiveloop_attr, ¶m); @@ -865,7 +867,7 @@ MavlinkReceiver::receive_start(Mavlink *mavlink) pthread_attr_setstacksize(&receiveloop_attr, 3000); pthread_t thread; - pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, mavlink); + pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, (void*)parent); pthread_attr_destroy(&receiveloop_attr); return thread; diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 6614e13f45..8e139e5e41 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -95,7 +95,7 @@ public: */ void print_status(); - static pthread_t receive_start(Mavlink* mavlink); + static pthread_t receive_start(Mavlink *parent); static void * start_helper(void *context); @@ -105,7 +105,6 @@ private: Mavlink* _mavlink; - void handle_message(mavlink_message_t *msg); void *receive_thread(void *arg); From 71cd1326635bead6c788b8967c6d5bcfd920b49c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 14 Feb 2014 14:24:26 +0100 Subject: [PATCH 067/381] Mavlink: Don't support mavlink stop so that QGC doesn't stop mavlink before it starts rc.usb --- src/modules/mavlink/mavlink_main.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 79aa6135b3..d2c94ca92b 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1842,7 +1842,7 @@ Mavlink::status() static void usage() { - errx(1, "usage: mavlink {start|stop|status} [-d device] [-b baudrate] [-o]"); + errx(1, "usage: mavlink {start|stop-all} [-d device] [-b baudrate] [-o]"); } int mavlink_main(int argc, char *argv[]) @@ -1892,6 +1892,10 @@ int mavlink_main(int argc, char *argv[]) return 0; } else if (!strcmp(argv[1], "stop")) { + warnx("mavlink stop is deprecated, use stop-all instead"); + usage(); + + } else if (!strcmp(argv[1], "stop-all")) { return Mavlink::destroy_all_instances(); // } else if (!strcmp(argv[1], "status")) { From 30dacbd1541073f9e35b8de0d1410e23f3aaccda Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 15 Feb 2014 11:29:36 +0100 Subject: [PATCH 068/381] Mavlink and navigator: handle current waypoint onboard --- src/modules/mavlink/mavlink_main.cpp | 102 +++++++++++--------- src/modules/mavlink/mavlink_main.h | 2 +- src/modules/navigator/navigator_main.cpp | 25 +++-- src/modules/navigator/navigator_mission.cpp | 67 +++++++++++-- src/modules/navigator/navigator_mission.h | 11 ++- src/modules/uORB/topics/mission_result.h | 3 +- 6 files changed, 142 insertions(+), 68 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index d2c94ca92b..38f753c396 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -59,11 +59,11 @@ #include #include #include -#include #include #include #include #include +#include #include #include #include @@ -958,7 +958,7 @@ void Mavlink::mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8 mavlink_msg_mission_ack_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpa); mavlink_missionlib_send_message(&msg); - if (verbose) warnx("Sent waypoint ack (%u) to ID %u", wpa.type, wpa.target_system); + if (_verbose) warnx("Sent waypoint ack (%u) to ID %u", wpa.type, wpa.target_system); } /* @@ -981,11 +981,11 @@ void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq) mavlink_msg_mission_current_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpc); mavlink_missionlib_send_message(&msg); - if (verbose) warnx("Broadcasted new current waypoint %u", wpc.seq); + if (_verbose) warnx("Broadcasted new current waypoint %u", wpc.seq); } else { mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds"); - if (verbose) warnx("ERROR: index out of bounds"); + if (_verbose) warnx("ERROR: index out of bounds"); } } @@ -1001,7 +1001,7 @@ void Mavlink::mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uin mavlink_msg_mission_count_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpc); mavlink_missionlib_send_message(&msg); - if (verbose) warnx("Sent waypoint count (%u) to ID %u", wpc.count, wpc.target_system); + if (_verbose) warnx("Sent waypoint count (%u) to ID %u", wpc.count, wpc.target_system); } void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq) @@ -1031,10 +1031,10 @@ void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t mavlink_msg_mission_item_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wp); mavlink_missionlib_send_message(&msg); - if (verbose) warnx("Sent waypoint %u to ID %u", wp.seq, wp.target_system); + if (_verbose) warnx("Sent waypoint %u to ID %u", wp.seq, wp.target_system); } else { mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR); - if (verbose) warnx("ERROR: could not read WP%u", seq); + if (_verbose) warnx("ERROR: could not read WP%u", seq); } } @@ -1049,11 +1049,11 @@ void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, u mavlink_msg_mission_request_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpr); mavlink_missionlib_send_message(&msg); - if (verbose) warnx("Sent waypoint request %u to ID %u", wpr.seq, wpr.target_system); + if (_verbose) warnx("Sent waypoint request %u to ID %u", wpr.seq, wpr.target_system); } else { mavlink_missionlib_send_gcs_string("ERROR: Waypoint index exceeds list capacity"); - if (verbose) warnx("ERROR: Waypoint index exceeds list capacity"); + if (_verbose) warnx("ERROR: Waypoint index exceeds list capacity"); } } @@ -1074,10 +1074,9 @@ void Mavlink::mavlink_wpm_send_waypoint_reached(uint16_t seq) mavlink_msg_mission_item_reached_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wp_reached); mavlink_missionlib_send_message(&msg); - if (verbose) warnx("Sent waypoint %u reached message", wp_reached.seq); + if (_verbose) warnx("Sent waypoint %u reached message", wp_reached.seq); } - void Mavlink::mavlink_waypoint_eventloop(uint64_t now) { /* check for timed-out operations */ @@ -1085,7 +1084,7 @@ void Mavlink::mavlink_waypoint_eventloop(uint64_t now) mavlink_missionlib_send_gcs_string("Operation timeout"); - if (verbose) warnx("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", wpm->current_state); + if (_verbose) warnx("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", wpm->current_state); wpm->current_state = MAVLINK_WPM_STATE_IDLE; wpm->current_partner_sysid = 0; @@ -1117,7 +1116,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch"); - if (verbose) warnx("REJ. WP CMD: curr partner id mismatch"); + if (_verbose) warnx("REJ. WP CMD: curr partner id mismatch"); } break; @@ -1140,18 +1139,18 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } else { mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list"); - if (verbose) warnx("IGN WP CURR CMD: Not in list"); + if (_verbose) warnx("IGN WP CURR CMD: Not in list"); } } else { mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy"); - if (verbose) warnx("IGN WP CURR CMD: Busy"); + if (_verbose) warnx("IGN WP CURR CMD: Busy"); } } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); - if (verbose) warnx("REJ. WP CMD: target id mismatch"); + if (_verbose) warnx("REJ. WP CMD: target id mismatch"); } break; @@ -1173,7 +1172,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) wpm->current_partner_compid = msg->compid; } else { - if (verbose) warnx("No waypoints send"); + if (_verbose) warnx("No waypoints send"); } wpm->current_count = wpm->size; @@ -1181,11 +1180,11 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } else { mavlink_missionlib_send_gcs_string("IGN REQUEST LIST: Busy"); - if (verbose) warnx("IGN REQUEST LIST: Busy"); + if (_verbose) warnx("IGN REQUEST LIST: Busy"); } } else { mavlink_missionlib_send_gcs_string("REJ. REQUEST LIST: target id mismatch"); - if (verbose) warnx("REJ. REQUEST LIST: target id mismatch"); + if (_verbose) warnx("REJ. REQUEST LIST: target id mismatch"); } break; @@ -1201,7 +1200,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (wpr.seq >= wpm->size) { mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list"); - if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.", wpr.seq); + if (_verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.", wpr.seq); break; } @@ -1212,11 +1211,11 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { if (wpr.seq == 0) { - if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); + if (_verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0"); - if (verbose) warnx("REJ. WP CMD: First id != 0"); + if (_verbose) warnx("REJ. WP CMD: First id != 0"); break; } @@ -1224,22 +1223,22 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (wpr.seq == wpm->current_wp_id) { - if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); + if (_verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } else if (wpr.seq == wpm->current_wp_id + 1) { - if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); + if (_verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected"); - if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).", wpr.seq, wpm->current_wp_id, wpm->current_wp_id + 1); + if (_verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).", wpr.seq, wpm->current_wp_id, wpm->current_wp_id + 1); break; } } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); - if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).", wpm->current_state); + if (_verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).", wpm->current_state); break; } @@ -1252,7 +1251,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } else { mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR); - if (verbose) warnx("ERROR: Waypoint %u out of bounds", wpr.seq); + if (_verbose) warnx("ERROR: Waypoint %u out of bounds", wpr.seq); } @@ -1261,12 +1260,12 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if ((wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) && !(msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid)) { mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); - if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.", msg->sysid, wpm->current_partner_sysid); + if (_verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.", msg->sysid, wpm->current_partner_sysid); } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); - if (verbose) warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); + if (_verbose) warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } } break; @@ -1282,18 +1281,18 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) { if (wpc.count > NUM_MISSIONS_SUPPORTED) { - if (verbose) warnx("Too many waypoints: %d, supported: %d", wpc.count, NUM_MISSIONS_SUPPORTED); + if (_verbose) warnx("Too many waypoints: %d, supported: %d", wpc.count, NUM_MISSIONS_SUPPORTED); mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_NO_SPACE); break; } if (wpc.count == 0) { mavlink_missionlib_send_gcs_string("COUNT 0"); - if (verbose) warnx("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE"); + if (_verbose) warnx("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE"); break; } - if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid); + if (_verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid); wpm->current_state = MAVLINK_WPM_STATE_GETLIST; wpm->current_wp_id = 0; @@ -1307,19 +1306,19 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (wpm->current_wp_id == 0) { mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN"); - if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) again from %u", wpc.count, msg->sysid); + if (_verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) again from %u", wpc.count, msg->sysid); } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); - if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.", wpm->current_wp_id); + if (_verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.", wpm->current_wp_id); } } else { mavlink_missionlib_send_gcs_string("IGN MISSION_COUNT CMD: Busy"); - if (verbose) warnx("IGN MISSION_COUNT CMD: Busy"); + if (_verbose) warnx("IGN MISSION_COUNT CMD: Busy"); } } else { mavlink_missionlib_send_gcs_string("REJ. WP COUNT CMD: target id mismatch"); - if (verbose) warnx("IGNORED WAYPOINT COUNT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); + if (_verbose) warnx("IGNORED WAYPOINT COUNT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } } break; @@ -1389,15 +1388,18 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) break; } - if (wp.current) { - mission.current_index = wp.seq; - } +// if (wp.current) { +// warnx("current is: %d", wp.seq); +// mission.current_index = wp.seq; +// } + // XXX ignore current set + mission.current_index = -1; wpm->current_wp_id = wp.seq + 1; if (wpm->current_wp_id == wpm->current_count && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { - if (verbose) warnx("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE", wpm->current_count); + if (_verbose) warnx("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE", wpm->current_count); mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED); @@ -1416,7 +1418,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); - if (verbose) warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); + if (_verbose) warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } break; @@ -1448,14 +1450,14 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } else { mavlink_missionlib_send_gcs_string("IGN WP CLEAR CMD: Busy"); - if (verbose) warnx("IGN WP CLEAR CMD: Busy"); + if (_verbose) warnx("IGN WP CLEAR CMD: Busy"); } } else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm->current_state != MAVLINK_WPM_STATE_IDLE) { mavlink_missionlib_send_gcs_string("REJ. WP CLERR CMD: target id mismatch"); - if (verbose) warnx("IGNORED WAYPOINT CLEAR COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); + if (_verbose) warnx("IGNORED WAYPOINT CLEAR COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } break; @@ -1550,6 +1552,9 @@ Mavlink::task_main(int argc, char *argv[]) _mode = MODE_ONBOARD; break; + case 'v': + _verbose = true; + default: usage(); break; @@ -1767,9 +1772,12 @@ Mavlink::task_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result); - if (mission_result.mission_reached) { - mavlink_wpm_send_waypoint_reached((uint16_t)mission_result.mission_index); - } + if (_verbose) warnx("Got mission result"); + + if (mission_result.mission_reached) + mavlink_wpm_send_waypoint_reached((uint16_t)mission_result.mission_index_reached); + + mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission); } mavlink_waypoint_eventloop(hrt_absolute_time()); @@ -1842,7 +1850,7 @@ Mavlink::status() static void usage() { - errx(1, "usage: mavlink {start|stop-all} [-d device] [-b baudrate] [-o]"); + errx(1, "usage: mavlink {start|stop-all} [-d device] [-b baudrate] [-o] [-v]"); } int mavlink_main(int argc, char *argv[]) diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index c667a41daa..9461a51f8f 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -293,7 +293,7 @@ private: mavlink_wpm_storage wpm_s; mavlink_wpm_storage *wpm; - bool verbose; + bool _verbose; int _uart; int _baudrate; bool gcs_link; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 260356eca9..e1dde35bff 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -65,7 +65,6 @@ #include #include #include -#include #include #include #include @@ -288,9 +287,9 @@ private: static void task_main_trampoline(int argc, char *argv[]); /** - * Main sensor collection task. + * Main task. */ - void task_main() __attribute__((noreturn)); + void task_main(); void publish_safepoints(unsigned points); @@ -384,7 +383,6 @@ Navigator::Navigator() : /* publications */ _pos_sp_triplet_pub(-1), - _mission_result_pub(-1), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), @@ -416,7 +414,6 @@ Navigator::Navigator() : _parameter_handles.rtl_land_delay = param_find("NAV_RTL_LAND_T"); memset(&_pos_sp_triplet, 0, sizeof(struct position_setpoint_triplet_s)); - memset(&_mission_result, 0, sizeof(struct mission_result_s)); memset(&_mission_item, 0, sizeof(struct mission_item_s)); memset(&nav_states_str, 0, sizeof(nav_states_str)); @@ -518,13 +515,16 @@ Navigator::offboard_mission_update(bool isrotaryWing) missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count, _geofence); _mission.set_offboard_dataman_id(offboard_mission.dataman_id); - _mission.set_current_offboard_mission_index(offboard_mission.current_index); + _mission.set_offboard_mission_count(offboard_mission.count); + _mission.set_current_offboard_mission_index(offboard_mission.current_index); } else { - _mission.set_current_offboard_mission_index(0); _mission.set_offboard_mission_count(0); + _mission.set_current_offboard_mission_index(0); } + + _mission.publish_mission_result(); } void @@ -534,12 +534,12 @@ Navigator::onboard_mission_update() if (orb_copy(ORB_ID(mission), _onboard_mission_sub, &onboard_mission) == OK) { - _mission.set_current_onboard_mission_index(onboard_mission.current_index); _mission.set_onboard_mission_count(onboard_mission.count); + _mission.set_current_onboard_mission_index(onboard_mission.current_index); } else { - _mission.set_current_onboard_mission_index(0); _mission.set_onboard_mission_count(0); + _mission.set_current_onboard_mission_index(0); } } @@ -1116,6 +1116,9 @@ Navigator::set_mission_item() ret = _mission.get_current_mission_item(&_mission_item, &onboard, &index); if (ret == OK) { + + _mission.report_current_mission_item(); + /* reset time counter for new item */ _time_first_inside_orbit = 0; @@ -1537,6 +1540,9 @@ void Navigator::on_mission_item_reached() { if (myState == NAV_STATE_MISSION) { + + _mission.report_mission_item_reached(); + if (_do_takeoff) { /* takeoff completed */ _do_takeoff = false; @@ -1589,6 +1595,7 @@ Navigator::on_mission_item_reached() mavlink_log_info(_mavlink_fd, "[navigator] landing completed"); dispatch(EVENT_READY_REQUESTED); } + _mission.publish_mission_result(); } void diff --git a/src/modules/navigator/navigator_mission.cpp b/src/modules/navigator/navigator_mission.cpp index e72caf98e9..fdc4ffff69 100644 --- a/src/modules/navigator/navigator_mission.cpp +++ b/src/modules/navigator/navigator_mission.cpp @@ -36,13 +36,12 @@ * Helper class to access missions */ -// #include -// #include -// #include -// #include - +#include #include #include +#include +#include +#include #include "navigator_mission.h" /* oddly, ERROR is not defined for c++ */ @@ -60,8 +59,11 @@ Mission::Mission() : _offboard_mission_item_count(0), _onboard_mission_item_count(0), _onboard_mission_allowed(false), - _current_mission_type(MISSION_TYPE_NONE) -{} + _current_mission_type(MISSION_TYPE_NONE), + _mission_result_pub(-1) +{ + memset(&_mission_result, 0, sizeof(struct mission_result_s)); +} Mission::~Mission() { @@ -78,8 +80,16 @@ void Mission::set_current_offboard_mission_index(int new_index) { if (new_index != -1) { + warnx("specifically set to %d", new_index); _current_offboard_mission_index = (unsigned)new_index; + } else { + + /* if less WPs available, reset to first WP */ + if (_current_offboard_mission_index >= _offboard_mission_item_count) { + _current_offboard_mission_index = 0; + } } + report_current_mission_item(); } void @@ -87,7 +97,15 @@ Mission::set_current_onboard_mission_index(int new_index) { if (new_index != -1) { _current_onboard_mission_index = (unsigned)new_index; + } else { + + /* if less WPs available, reset to first WP */ + if (_current_onboard_mission_index >= _onboard_mission_item_count) { + _current_onboard_mission_index = 0; + } } + // TODO: implement this for onboard missions as well + // report_current_mission_item(); } void @@ -266,4 +284,37 @@ Mission::move_to_next() default: break; } -} \ No newline at end of file +} + +void +Mission::report_mission_item_reached() +{ + if (_current_mission_type == MISSION_TYPE_OFFBOARD) { + _mission_result.mission_reached = true; + _mission_result.mission_index_reached = _current_offboard_mission_index; + } +} + +void +Mission::report_current_mission_item() +{ + if (_current_mission_type == MISSION_TYPE_OFFBOARD) { + _mission_result.index_current_mission = _current_offboard_mission_index; + } +} + +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.mission_reached = false; +} diff --git a/src/modules/navigator/navigator_mission.h b/src/modules/navigator/navigator_mission.h index 15d4e86bfb..845c16583d 100644 --- a/src/modules/navigator/navigator_mission.h +++ b/src/modules/navigator/navigator_mission.h @@ -40,6 +40,7 @@ #define NAVIGATOR_MISSION_H #include +#include class __EXPORT Mission @@ -71,7 +72,9 @@ public: void move_to_next(); - void add_home_pos(struct mission_item_s *new_mission_item); + void report_mission_item_reached(); + void report_current_mission_item(); + void publish_mission_result(); private: bool current_onboard_mission_available(); @@ -92,6 +95,10 @@ private: MISSION_TYPE_ONBOARD, MISSION_TYPE_OFFBOARD, } _current_mission_type; + + int _mission_result_pub; + + struct mission_result_s _mission_result; }; -#endif \ No newline at end of file +#endif diff --git a/src/modules/uORB/topics/mission_result.h b/src/modules/uORB/topics/mission_result.h index c99706b97a..7c3921198a 100644 --- a/src/modules/uORB/topics/mission_result.h +++ b/src/modules/uORB/topics/mission_result.h @@ -54,7 +54,8 @@ struct mission_result_s { bool mission_reached; /**< true if mission has been reached */ - unsigned mission_index; /**< index of the mission which has been reached */ + unsigned mission_index_reached; /**< index of the mission which has been reached */ + unsigned index_current_mission; /**< index of the current mission */ }; /** From b596bf6aa56b2039b51a03ab11acf2d7d719195d Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 16 Feb 2014 13:24:02 +0100 Subject: [PATCH 069/381] Mavlink: gotten rid of some warnings --- src/modules/mavlink/mavlink_main.cpp | 14 +++++++++----- src/modules/mavlink/mavlink_receiver.cpp | 13 +++++-------- src/modules/mavlink/mavlink_receiver.h | 2 +- 3 files changed, 15 insertions(+), 14 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 38f753c396..42504dea9f 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -52,7 +52,12 @@ #include #include #include +#include /* isinf / isnan checks */ + #include +#include +#include + #include #include #include @@ -64,6 +69,7 @@ #include #include #include + #include #include #include @@ -72,12 +78,10 @@ #include #include #include -#include -#include + #include #include "mavlink_bridge_header.h" -#include "math.h" /* isinf / isnan checks */ #include "mavlink_main.h" #include "mavlink_orb_listener.h" #include "mavlink_receiver.h" @@ -156,9 +160,9 @@ namespace mavlink Mavlink::Mavlink() : device_name("/dev/ttyS1"), - _mavlink_fd(-1), - _next(nullptr), _task_should_exit(false), + _next(nullptr), + _mavlink_fd(-1), thread_running(false), _mavlink_task(-1), _mavlink_incoming_fd(-1), diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 371f945c4c..d07de0f22f 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -106,7 +106,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : telemetry_status_pub(-1), lat0(0), lon0(0), - alt0(0) + alt0(0.0) { } @@ -605,8 +605,6 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed); } - uint64_t timestamp = hrt_absolute_time(); - // publish global position if (pub_hil_global_pos > 0) { orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos); @@ -628,7 +626,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) if (pub_hil_local_pos > 0) { float x; float y; - bool landed = hil_state.alt/1000.0f < (alt0 + 0.1); // XXX improve? + bool landed = (float)(hil_state.alt)/1000.0f < (alt0 + 0.1f); // XXX improve? double lat = hil_state.lat*1e-7; double lon = hil_state.lon*1e-7; map_projection_project(lat, lon, &x, &y); @@ -811,14 +809,13 @@ MavlinkReceiver::receive_thread(void *arg) while (!_mavlink->_task_should_exit) { if (poll(fds, 1, timeout) > 0) { - if (nread < sizeof(buf)) { + + /* non-blocking read. read may return negative values */ + if ((nread = read(uart_fd, buf, sizeof(buf))) < (ssize_t)sizeof(buf)) { /* to avoid reading very small chunks wait for data before reading */ usleep(1000); } - /* non-blocking read. read may return negative values */ - nread = read(uart_fd, buf, sizeof(buf)); - /* if read failed, this loop won't execute */ for (ssize_t i = 0; i < nread; i++) { if (mavlink_parse_char(_mavlink->get_chan(), buf[i], &msg, &status)) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 8e139e5e41..fca5de9179 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -140,6 +140,6 @@ private: orb_advert_t telemetry_status_pub; int32_t lat0; int32_t lon0; - double alt0; + float alt0; }; From 366f19c2c07f249a6ac51f16f08b86454e2e2da4 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 16 Feb 2014 13:24:56 +0100 Subject: [PATCH 070/381] Stack: lower stack of lpwork and hpwork --- nuttx-configs/px4fmu-v1/nsh/defconfig | 4 ++-- nuttx-configs/px4fmu-v2/nsh/defconfig | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index 2e82b8a2d9..7aa71f1826 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -720,11 +720,11 @@ CONFIG_SCHED_WORKQUEUE=y CONFIG_SCHED_HPWORK=y CONFIG_SCHED_WORKPRIORITY=192 CONFIG_SCHED_WORKPERIOD=5000 -CONFIG_SCHED_WORKSTACKSIZE=4000 +CONFIG_SCHED_WORKSTACKSIZE=2000 CONFIG_SCHED_LPWORK=y CONFIG_SCHED_LPWORKPRIORITY=50 CONFIG_SCHED_LPWORKPERIOD=50000 -CONFIG_SCHED_LPWORKSTACKSIZE=4000 +CONFIG_SCHED_LPWORKSTACKSIZE=2000 # CONFIG_LIB_KBDCODEC is not set # CONFIG_LIB_SLCDCODEC is not set diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 0508726777..d42b23ab0b 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -796,11 +796,11 @@ CONFIG_SCHED_WORKQUEUE=y CONFIG_SCHED_HPWORK=y CONFIG_SCHED_WORKPRIORITY=192 CONFIG_SCHED_WORKPERIOD=5000 -CONFIG_SCHED_WORKSTACKSIZE=4000 +CONFIG_SCHED_WORKSTACKSIZE=2000 CONFIG_SCHED_LPWORK=y CONFIG_SCHED_LPWORKPRIORITY=50 CONFIG_SCHED_LPWORKPERIOD=50000 -CONFIG_SCHED_LPWORKSTACKSIZE=4000 +CONFIG_SCHED_LPWORKSTACKSIZE=2000 # CONFIG_LIB_KBDCODEC is not set # CONFIG_LIB_SLCDCODEC is not set From c6f6eaf0cd99aadbc93264144cdab8bba81f1937 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Feb 2014 17:15:45 +0100 Subject: [PATCH 071/381] Minor init cleanup --- .../fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 294e1fc51d..829782d90e 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -363,10 +363,6 @@ void FixedwingEstimator::task_main() { - /* inform about start */ - warnx("Initializing.."); - fflush(stdout); - /* * do subscriptions */ @@ -647,7 +643,7 @@ FixedwingEstimator::task_main() * PART TWO: EXECUTE THE FILTER **/ - if (hrt_elapsed_time(&start_time) > 500000 && !_initialized && (GPSstatus == 3)) { + if (hrt_elapsed_time(&start_time) > 100000 && !_initialized && (GPSstatus == 3)) { InitialiseFilter(velNED); _initialized = true; From f6088c2f6ec29abafab07fe2e30aec943d09f46b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Feb 2014 17:38:33 +0100 Subject: [PATCH 072/381] Better initialization, removed unnecessary static variables, reduced scopes where feasible --- .../fw_att_pos_estimator/estimator.cpp | 105 ++++++++++-------- src/modules/fw_att_pos_estimator/estimator.h | 4 +- 2 files changed, 64 insertions(+), 45 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index 9be12e700f..bae4272340 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -1,6 +1,7 @@ #include "estimator.h" // For debugging only +#include #include // Global variables @@ -87,6 +88,20 @@ Vector3f Vector3f::zero(void) const return ret; } +Mat3f::Mat3f() { + x.x = 1.0f; + x.y = 0.0f; + x.z = 0.0f; + + y.x = 0.0f; + y.y = 1.0f; + y.z = 0.0f; + + z.x = 0.0f; + z.y = 0.0f; + z.z = 1.0f; +} + Mat3f Mat3f::transpose(void) const { Mat3f ret = *this; @@ -165,7 +180,6 @@ void swap_var(float &d1, float &d2) void UpdateStrapdownEquationsNED() { - static Vector3f prevDelAng; Vector3f delVelNav; float q00; float q11; @@ -177,15 +191,12 @@ void UpdateStrapdownEquationsNED() float q12; float q13; float q23; - static Mat3f Tbn; - static Mat3f Tnb; + Mat3f Tbn; + Mat3f Tnb; float rotationMag; - float rotScaler; float qUpdated[4]; float quatMag; - float quatMagInv; float deltaQuat[4]; - static float lastVelocity[3]; const Vector3f gravityNED = {0.0,0.0,GRAVITY_MSS}; // Remove sensor bias errors @@ -197,7 +208,7 @@ void UpdateStrapdownEquationsNED() dVelIMU.z = dVelIMU.z; // Save current measurements - prevDelAng = correctedDelAng; + Vector3f prevDelAng = correctedDelAng; // Apply corrections for earths rotation rate and coning errors // * and + operators have been overloaded @@ -215,7 +226,7 @@ void UpdateStrapdownEquationsNED() else { deltaQuat[0] = cos(0.5f*rotationMag); - rotScaler = (sin(0.5f*rotationMag))/rotationMag; + float rotScaler = (sin(0.5f*rotationMag))/rotationMag; deltaQuat[1] = correctedDelAng.x*rotScaler; deltaQuat[2] = correctedDelAng.y*rotScaler; deltaQuat[3] = correctedDelAng.z*rotScaler; @@ -232,7 +243,7 @@ void UpdateStrapdownEquationsNED() quatMag = sqrt(sq(qUpdated[0]) + sq(qUpdated[1]) + sq(qUpdated[2]) + sq(qUpdated[3])); if (quatMag > 1e-16f) { - quatMagInv = 1.0f/quatMag; + float quatMagInv = 1.0f/quatMag; states[0] = quatMagInv*qUpdated[0]; states[1] = quatMagInv*qUpdated[1]; states[2] = quatMagInv*qUpdated[2]; @@ -275,6 +286,7 @@ void UpdateStrapdownEquationsNED() accNavMag = delVelNav.length()/dtIMU; // If calculating position save previous velocity + float lastVelocity[3]; lastVelocity[0] = states[4]; lastVelocity[1] = states[5]; lastVelocity[2] = states[6]; @@ -296,7 +308,7 @@ void CovariancePrediction(float dt) // scalars float windVelSigma; float dAngBiasSigma; - float dVelBiasSigma; + // float dVelBiasSigma; float magEarthSigma; float magBodySigma; float daxCov; @@ -911,9 +923,9 @@ void FuseVelposNED() uint32_t gpsRetryTimeNoTAS = 5000; // retry time if no TAS measurement available uint32_t hgtRetryTime = 5000; // height measurement retry time uint32_t horizRetryTime; - static uint32_t velFailTime; - static uint32_t posFailTime; - static uint32_t hgtFailTime; + static uint32_t velFailTime = 0; + static uint32_t posFailTime = 0; + static uint32_t hgtFailTime = 0; bool velHealth; bool posHealth; bool hgtHealth; @@ -922,9 +934,9 @@ void FuseVelposNED() bool hgtTimeout; // declare variables used to check measurement errors - float velInnov[3] = {0.0,0.0,0.0}; - float posInnov[2] = {0.0,0.0}; - float hgtInnov = 0.0; + float velInnov[3] = {0.0f,0.0f,0.0f}; + float posInnov[2] = {0.0f,0.0f}; + float hgtInnov = 0.0f; // declare variables used to control access to arrays bool fuseData[6] = {false,false,false,false,false,false}; @@ -1133,32 +1145,19 @@ void FuseVelposNED() void FuseMagnetometer() { - - static float q0 = 1.0; - static float q1 = 0.0; - static float q2 = 0.0; - static float q3 = 0.0; - static float magN = 0.0; - static float magE = 0.0; - static float magD = 0.0; - static float magXbias = 0.0; - static float magYbias = 0.0; - static float magZbias = 0.0; - static uint8_t obsIndex; + uint8_t obsIndex; uint8_t indexLimit; float DCM[3][3] = { - {1.0,0.0,0.0} , - {0.0,1.0,0.0} , - {0.0,0.0,1.0} + {1.0f,0.0f,0.0f} , + {0.0f,1.0f,0.0f} , + {0.0f,0.0f,1.0f} }; - static float MagPred[3] = {0.0,0.0,0.0}; - static float R_MAG; - static float SH_MAG[9] = {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0}; - float H_MAG[21]; + float MagPred[3] = {0.0f,0.0f,0.0f}; float SK_MX[6]; float SK_MY[5]; float SK_MZ[6]; + float SH_MAG[9] = {0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f}; // Perform sequential fusion of Magnetometer measurements. // This assumes that the errors in the different components are @@ -1178,12 +1177,28 @@ void FuseMagnetometer() indexLimit = 12; } + static float q0 = 0.0f; + static float q1 = 0.0f; + static float q2 = 0.0f; + static float q3 = 1.0f; + static float magN = 0.4f; + static float magE = 0.0f; + static float magD = 0.3f; + + static float R_MAG = 0.0025f; + + float H_MAG[21] = {0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f}; + // Sequential fusion of XYZ components to spread processing load across // three prediction time steps. // Calculate observation jacobians and Kalman gains if (fuseMagData) { + static float magXbias = 0.0f; + static float magYbias = 0.0f; + static float magZbias = 0.0f; + // Copy required states to local variable names q0 = statesAtMagMeasTime[0]; q1 = statesAtMagMeasTime[1]; @@ -1224,6 +1239,7 @@ void FuseMagnetometer() SH_MAG[6] = sq(q0); SH_MAG[7] = 2*magN*q0; SH_MAG[8] = 2*magE*q3; + for (uint8_t i=0; i<=20; i++) H_MAG[i] = 0; H_MAG[0] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; H_MAG[1] = SH_MAG[0]; @@ -1232,7 +1248,7 @@ void FuseMagnetometer() H_MAG[15] = SH_MAG[5] - SH_MAG[4] - SH_MAG[3] + SH_MAG[6]; H_MAG[16] = 2*q0*q3 + 2*q1*q2; H_MAG[17] = 2*q1*q3 - 2*q0*q2; - H_MAG[18] = 1; + H_MAG[18] = 1.0f; // Calculate Kalman gain SK_MX[0] = 1/(P[18][18] + R_MAG + P[1][18]*SH_MAG[0] + P[3][18]*SH_MAG[2] - P[15][18]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) - (2*magD*q0 - 2*magE*q1 + 2*magN*q2)*(P[18][2] + P[1][2]*SH_MAG[0] + P[3][2]*SH_MAG[2] - P[15][2]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][2]*(2*q0*q3 + 2*q1*q2) - P[17][2]*(2*q0*q2 - 2*q1*q3) - P[2][2]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[18][0] + P[1][0]*SH_MAG[0] + P[3][0]*SH_MAG[2] - P[15][0]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][0]*(2*q0*q3 + 2*q1*q2) - P[17][0]*(2*q0*q2 - 2*q1*q3) - P[2][0]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[18][1] + P[1][1]*SH_MAG[0] + P[3][1]*SH_MAG[2] - P[15][1]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][1]*(2*q0*q3 + 2*q1*q2) - P[17][1]*(2*q0*q2 - 2*q1*q3) - P[2][1]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[2]*(P[18][3] + P[1][3]*SH_MAG[0] + P[3][3]*SH_MAG[2] - P[15][3]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][3]*(2*q0*q3 + 2*q1*q2) - P[17][3]*(2*q0*q2 - 2*q1*q3) - P[2][3]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6])*(P[18][15] + P[1][15]*SH_MAG[0] + P[3][15]*SH_MAG[2] - P[15][15]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][15]*(2*q0*q3 + 2*q1*q2) - P[17][15]*(2*q0*q2 - 2*q1*q3) - P[2][15]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][15]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[16][18]*(2*q0*q3 + 2*q1*q2) - P[17][18]*(2*q0*q2 - 2*q1*q3) - P[2][18]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + (2*q0*q3 + 2*q1*q2)*(P[18][16] + P[1][16]*SH_MAG[0] + P[3][16]*SH_MAG[2] - P[15][16]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][16]*(2*q0*q3 + 2*q1*q2) - P[17][16]*(2*q0*q2 - 2*q1*q3) - P[2][16]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q2 - 2*q1*q3)*(P[18][17] + P[1][17]*SH_MAG[0] + P[3][17]*SH_MAG[2] - P[15][17]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][17]*(2*q0*q3 + 2*q1*q2) - P[17][17]*(2*q0*q2 - 2*q1*q3) - P[2][17]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[0][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); @@ -1272,7 +1288,7 @@ void FuseMagnetometer() else if (obsIndex == 1) // we are now fusing the Y measurement { // Calculate observation jacobians - for (uint8_t i=0; i<=20; i++) H_MAG[i] = 0; + for (unsigned int i=0; i 1e-12f) { for (uint8_t j= 0; j<=3; j++) @@ -1592,6 +1607,7 @@ float sq(float valIn) // Store states in a history array along with time stamp void StoreStates(uint64_t timestamp_ms) { + /* static to keep the store index */ static uint8_t storeIndex = 0; for (unsigned i=0; i Date: Sun, 16 Feb 2014 18:21:30 +0100 Subject: [PATCH 073/381] Debug hackery. We finally got something that is kind of close to an actual attitude estimate. --- .../fw_att_pos_estimator_main.cpp | 284 +++++++++++++----- 1 file changed, 211 insertions(+), 73 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 829782d90e..02b58224e1 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -357,7 +357,14 @@ FixedwingEstimator::task_main_trampoline(int argc, char *argv[]) estimator::g_estimator->task_main(); } -static float dt = 0.0f; // time lapsed since last covariance prediction +float dt = 0.0f; // time lapsed since last covariance prediction + +// Estimated time delays (msec) +uint32_t msecVelDelay = 230; +uint32_t msecPosDelay = 210; +uint32_t msecHgtDelay = 350; +uint32_t msecMagDelay = 30; +uint32_t msecTasDelay = 210; void FixedwingEstimator::task_main() @@ -403,7 +410,13 @@ FixedwingEstimator::task_main() /* initialize measurement data */ VtasMeas = 0.0f; Vector3f lastAngRate = {0.0f, 0.0f, 0.0f}; - Vector3f lastAccel = {0.0f, 0.0f, 0.0f}; + Vector3f lastAccel = {0.0f, 0.0f, -9.81f}; + dVelIMU.x = 0.0f; + dVelIMU.y = 0.0f; + dVelIMU.z = 0.0f; + dAngIMU.x = 0.0f; + dAngIMU.y = 0.0f; + dAngIMU.z = 0.0f; /* wakeup source(s) */ struct pollfd fds[2]; @@ -421,6 +434,10 @@ FixedwingEstimator::task_main() hrt_abstime start_time = hrt_absolute_time(); + bool newDataGps = false; + bool newAdsData = false; + bool newDataMag = false; + while (!_task_should_exit) { /* wait for up to 500ms for data */ @@ -548,6 +565,9 @@ FixedwingEstimator::task_main() if (last_mag != _sensor_combined.magnetometer_timestamp) { mag_updated = true; + newDataMag = true; + } else { + newDataMag = false; } last_mag = _sensor_combined.magnetometer_timestamp; @@ -560,6 +580,9 @@ FixedwingEstimator::task_main() perf_count(_perf_airspeed); VtasMeas = _airspeed.true_airspeed_m_s; + newAdsData = true; + } else { + newAdsData = false; } bool gps_updated; @@ -570,6 +593,7 @@ FixedwingEstimator::task_main() if (_gps.fix_type < 3) { gps_updated = false; + newDataGps = false; } else { /* fuse GPS updates */ @@ -584,6 +608,7 @@ FixedwingEstimator::task_main() gpsLat = math::radians(_gps.lat / (double)1e7); gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI; gpsHgt = _gps.alt / 1e3f; + newDataGps = true; } @@ -636,6 +661,10 @@ FixedwingEstimator::task_main() #endif + newDataMag = true; + + } else { + newDataMag = false; } @@ -643,101 +672,210 @@ FixedwingEstimator::task_main() * PART TWO: EXECUTE THE FILTER **/ - if (hrt_elapsed_time(&start_time) > 100000 && !_initialized && (GPSstatus == 3)) { - InitialiseFilter(velNED); - _initialized = true; + if (/*(IMUmsec > msecAlignTime) &&*/ !statesInitialised && (GPSstatus == 3)) + { + velNED[0] = _gps.vel_n_m_s; + velNED[1] = _gps.vel_e_m_s; + velNED[2] = _gps.vel_d_m_s; + InitialiseFilter(velNED); + } - warnx("init done."); - } + // If valid IMU data and states initialised, predict states and covariances + if (statesInitialised) + { + // Run the strapdown INS equations every IMU update + 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 + StoreStates(IMUmsec); + // Check if on ground - status is used by covariance prediction + OnGroundCheck(); + onGround = false; + // sum delta angles and time used by covariance prediction + summedDelAng = summedDelAng + correctedDelAng; + summedDelVel = summedDelVel + dVelIMU; + dt += dtIMU; + // perform a covariance prediction if the total delta angle has exceeded the limit + // or the time limit will be exceeded at the next IMU update + if ((dt >= (covTimeStepMax - dtIMU)) || (summedDelAng.length() > covDelAngMax)) + { + CovariancePrediction(dt); + summedDelAng = summedDelAng.zero(); + summedDelVel = summedDelVel.zero(); + dt = 0.0f; + } - if (_initialized) { + _initialized = true; + } - /* predict states and covariances */ + // Fuse GPS Measurements + if (newDataGps && statesInitialised) + { + // Convert GPS measurements to Pos NE, hgt and Vel NED + velNED[0] = _gps.vel_n_m_s; + velNED[1] = _gps.vel_e_m_s; + velNED[2] = _gps.vel_d_m_s; + calcposNED(posNED, gpsLat, gpsLon, gpsHgt, latRef, lonRef, hgtRef); - /* run the strapdown INS every sensor update */ - UpdateStrapdownEquationsNED(); + posNE[0] = posNED[0]; + posNE[1] = posNED[1]; + // set fusion flags + fuseVelData = true; + fusePosData = true; + // recall states stored at time of measurement after adjusting for delays + RecallStates(statesAtVelTime, (IMUmsec - msecVelDelay)); + RecallStates(statesAtPosTime, (IMUmsec - msecPosDelay)); + // run the fusion step + FuseVelposNED(); + } + else + { + fuseVelData = false; + fusePosData = false; + } - /* store the predictions */ - StoreStates(IMUmsec); + if (newAdsData && statesInitialised) + { + // Could use a blend of GPS and baro alt data if desired + hgtMea = 1.0f*baroHgt + 0.0f*gpsHgt; + fuseHgtData = true; + // recall states stored at time of measurement after adjusting for delays + RecallStates(statesAtHgtTime, (IMUmsec - msecHgtDelay)); + // run the fusion step + FuseVelposNED(); + } + else + { + fuseHgtData = false; + } - /* evaluate if on ground */ - // OnGroundCheck(); - onGround = false; + // Fuse Magnetometer Measurements + if (newDataMag && statesInitialised) + { + fuseMagData = true; + RecallStates(statesAtMagMeasTime, (IMUmsec - msecMagDelay)); // Assume 50 msec avg delay for magnetometer data + } + else + { + fuseMagData = false; + } + if (statesInitialised) FuseMagnetometer(); - /* prepare the delta angles and time used by the covariance prediction */ - summedDelAng = summedDelAng + correctedDelAng; - summedDelVel = summedDelVel + correctedDelVel; + // Fuse Airspeed Measurements + if (newAdsData && statesInitialised && VtasMeas > 8.0f) + { + fuseVtasData = true; + RecallStates(statesAtVtasMeasTime, (IMUmsec - msecTasDelay)); // assume 100 msec avg delay for airspeed data + FuseAirspeed(); + } + else + { + fuseVtasData = false; + } + + // if (hrt_elapsed_time(&start_time) > 100000 && !_initialized && (GPSstatus == 3)) { + // InitialiseFilter(velNED); + // _initialized = true; + + // warnx("init done."); + // } + + // if (_initialized) { + + // /* predict states and covariances */ + + // /* run the strapdown INS every sensor update */ + // UpdateStrapdownEquationsNED(); + + // /* store the predictions */ + // StoreStates(IMUmsec); + + // /* evaluate if on ground */ + // // OnGroundCheck(); + // onGround = false; + + // /* prepare the delta angles and time used by the covariance prediction */ + // summedDelAng = summedDelAng + correctedDelAng; + // summedDelVel = summedDelVel + correctedDelVel; - dt += dtIMU; + // dt += dtIMU; - /* predict the covairance if the total delta angle has exceeded the threshold - * or the time limit will be exceeded on the next measurement update - */ - if ((dt >= (covTimeStepMax - dtIMU)) || (summedDelAng.length() > covDelAngMax)) { - CovariancePrediction(dt); - summedDelAng = summedDelAng.zero(); - summedDelVel = summedDelVel.zero(); - dt = 0.0f; - } + // /* predict the covairance if the total delta angle has exceeded the threshold + // * or the time limit will be exceeded on the next measurement update + // */ + // if ((dt >= (covTimeStepMax - dtIMU)) || (summedDelAng.length() > covDelAngMax)) { + // CovariancePrediction(dt); + // summedDelAng = summedDelAng.zero(); + // summedDelVel = summedDelVel.zero(); + // dt = 0.0f; + // } - } + // } - if (gps_updated && _initialized) { + // if (gps_updated && _initialized) { - /* convert GPS measurements to horizontal NE, altitude and 3D velocity NED */ - calcposNED(posNED, gpsLat, gpsLon, gpsHgt, latRef, lonRef, hgtRef); + // /* convert GPS measurements to horizontal NE, altitude and 3D velocity NED */ + // calcposNED(posNED, gpsLat, gpsLon, gpsHgt, latRef, lonRef, hgtRef); - posNE[0] = posNED[0]; - posNE[1] = posNED[1]; + // posNE[0] = posNED[0]; + // posNE[1] = posNED[1]; - // set flags for further processing - fuseVelData = true; - fusePosData = true; + // // set flags for further processing + // fuseVelData = true; + // fusePosData = true; - /* recall states after adjusting for delays */ - RecallStates(statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); - RecallStates(statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); + // /* recall states after adjusting for delays */ + // RecallStates(statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); + // RecallStates(statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); - /* run the actual fusion */ - FuseVelposNED(); - } else { - fuseVelData = false; - fusePosData = false; - } + // /* run the actual fusion */ + // FuseVelposNED(); + // } else { + // fuseVelData = false; + // fusePosData = false; + // } - if (baro_updated && _initialized) { + // if (baro_updated && _initialized) { - fuseHgtData = true; - // recall states stored at time of measurement after adjusting for delays - RecallStates(statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms)); - // run the fusion step - FuseVelposNED(); - } else { - fuseHgtData = false; - } + // fuseHgtData = true; + // // recall states stored at time of measurement after adjusting for delays + // RecallStates(statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms)); + // // run the fusion step + // FuseVelposNED(); + // } else { + // fuseHgtData = false; + // } - if (mag_updated && _initialized) { - fuseMagData = true; - RecallStates(statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); + // if (mag_updated && _initialized) { + // fuseMagData = true; + // RecallStates(statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); - } else { - fuseMagData = false; - } + // } else { + // fuseMagData = false; + // } - if (_initialized) { - FuseMagnetometer(); - } + // if (_initialized) { + // FuseMagnetometer(); + // } - if (airspeed_updated && _initialized - && _airspeed.true_airspeed_m_s > 6.0f /* XXX magic number */) { + // if (airspeed_updated && _initialized + // && _airspeed.true_airspeed_m_s > 6.0f /* XXX magic number */) { - fuseVtasData = true; - RecallStates(statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data - FuseAirspeed(); - } else { - fuseVtasData = false; - } + // fuseVtasData = true; + // RecallStates(statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data + // FuseAirspeed(); + // } else { + // fuseVtasData = false; + // } // Publish results if (_initialized) { From 7e9fcac057616ba535e09d49f7d0f47f5ce9977b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 18 Feb 2014 08:24:30 +0100 Subject: [PATCH 074/381] Pure code style formatting --- .../fw_att_pos_estimator_main.cpp | 278 +++++++++--------- 1 file changed, 142 insertions(+), 136 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 02b58224e1..4f5e3c9b48 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -129,9 +129,9 @@ private: int _gyro_sub; /**< gyro sensor subscription */ int _accel_sub; /**< accel sensor subscription */ int _mag_sub; /**< mag sensor subscription */ - #else +#else int _sensor_combined_sub; - #endif +#endif int _airspeed_sub; /**< airspeed subscription */ int _baro_sub; /**< barometer subscription */ int _gps_sub; /**< GPS subscription */ @@ -233,13 +233,13 @@ FixedwingEstimator::FixedwingEstimator() : _estimator_task(-1), /* subscriptions */ - #ifndef SENSOR_COMBINED_SUB +#ifndef SENSOR_COMBINED_SUB _gyro_sub(-1), _accel_sub(-1), _mag_sub(-1), - #else +#else _sensor_combined_sub(-1), - #endif +#endif _airspeed_sub(-1), _baro_sub(-1), _gps_sub(-1), @@ -382,7 +382,7 @@ FixedwingEstimator::task_main() /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vstatus_sub, 200); - #ifndef SENSOR_COMBINED_SUB +#ifndef SENSOR_COMBINED_SUB _gyro_sub = orb_subscribe(ORB_ID(sensor_gyro)); _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); @@ -391,11 +391,11 @@ FixedwingEstimator::task_main() /* rate limit gyro updates to 50 Hz */ /* XXX remove this!, BUT increase the data buffer size! */ orb_set_interval(_gyro_sub, 4); - #else +#else _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); /* XXX remove this!, BUT increase the data buffer size! */ orb_set_interval(_sensor_combined_sub, 4); - #endif +#endif parameters_update(); @@ -424,13 +424,13 @@ FixedwingEstimator::task_main() /* Setup of loop */ fds[0].fd = _params_sub; fds[0].events = POLLIN; - #ifndef SENSOR_COMBINED_SUB +#ifndef SENSOR_COMBINED_SUB fds[1].fd = _gyro_sub; fds[1].events = POLLIN; - #else +#else fds[1].fd = _sensor_combined_sub; fds[1].events = POLLIN; - #endif +#endif hrt_abstime start_time = hrt_absolute_time(); @@ -483,9 +483,9 @@ FixedwingEstimator::task_main() hrt_abstime last_sensor_timestamp; /* load local copies */ - #ifndef SENSOR_COMBINED_SUB +#ifndef SENSOR_COMBINED_SUB orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro); - + orb_check(_accel_sub, &accel_updated); @@ -523,7 +523,7 @@ FixedwingEstimator::task_main() lastAccel = accel; - #else +#else orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); static hrt_abstime last_accel = 0; @@ -532,6 +532,7 @@ FixedwingEstimator::task_main() if (last_accel != _sensor_combined.accelerometer_timestamp) { accel_updated = true; } + last_accel = _sensor_combined.accelerometer_timestamp; @@ -566,27 +567,32 @@ FixedwingEstimator::task_main() if (last_mag != _sensor_combined.magnetometer_timestamp) { mag_updated = true; newDataMag = true; + } else { newDataMag = false; } + last_mag = _sensor_combined.magnetometer_timestamp; - #endif +#endif bool airspeed_updated; orb_check(_airspeed_sub, &airspeed_updated); + if (airspeed_updated) { orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); perf_count(_perf_airspeed); VtasMeas = _airspeed.true_airspeed_m_s; newAdsData = true; + } else { newAdsData = false; } bool gps_updated; orb_check(_gps_sub, &gps_updated); + if (gps_updated) { orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps); perf_count(_perf_gps); @@ -594,6 +600,7 @@ FixedwingEstimator::task_main() if (_gps.fix_type < 3) { gps_updated = false; newDataGps = false; + } else { /* fuse GPS updates */ @@ -616,23 +623,25 @@ FixedwingEstimator::task_main() bool baro_updated; orb_check(_baro_sub, &baro_updated); + if (baro_updated) { orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); baroHgt = _baro.altitude; // Could use a blend of GPS and baro alt data if desired - hgtMea = 1.0f*baroHgt + 0.0f*gpsHgt; + hgtMea = 1.0f * baroHgt + 0.0f * gpsHgt; } - #ifndef SENSOR_COMBINED_SUB +#ifndef SENSOR_COMBINED_SUB orb_check(_mag_sub, &mag_updated); - #endif +#endif + if (mag_updated) { perf_count(_perf_mag); - #ifndef SENSOR_COMBINED_SUB +#ifndef SENSOR_COMBINED_SUB orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag); // XXX we compensate the offsets upfront - should be close to zero. @@ -645,8 +654,8 @@ FixedwingEstimator::task_main() magData.z = _mag.z; magBias.z = 0.000001f; // _mag_offsets.y_offset - - #else + +#else // XXX we compensate the offsets upfront - should be close to zero. // 0.001f @@ -659,7 +668,7 @@ FixedwingEstimator::task_main() magData.z = _sensor_combined.magnetometer_ga[2]; magBias.z = 0.000001f; // _mag_offsets.y_offset - #endif +#endif newDataMag = true; @@ -672,114 +681,111 @@ FixedwingEstimator::task_main() * PART TWO: EXECUTE THE FILTER **/ - if (/*(IMUmsec > msecAlignTime) &&*/ !statesInitialised && (GPSstatus == 3)) - { - velNED[0] = _gps.vel_n_m_s; - velNED[1] = _gps.vel_e_m_s; - velNED[2] = _gps.vel_d_m_s; - InitialiseFilter(velNED); - } + if (/*(IMUmsec > msecAlignTime) &&*/ !statesInitialised && (GPSstatus == 3)) { + velNED[0] = _gps.vel_n_m_s; + velNED[1] = _gps.vel_e_m_s; + velNED[2] = _gps.vel_d_m_s; + InitialiseFilter(velNED); + } - // If valid IMU data and states initialised, predict states and covariances - if (statesInitialised) - { - // Run the strapdown INS equations every IMU update - 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 - StoreStates(IMUmsec); - // Check if on ground - status is used by covariance prediction - OnGroundCheck(); - onGround = false; - // sum delta angles and time used by covariance prediction - summedDelAng = summedDelAng + correctedDelAng; - summedDelVel = summedDelVel + dVelIMU; - dt += dtIMU; - // perform a covariance prediction if the total delta angle has exceeded the limit - // or the time limit will be exceeded at the next IMU update - if ((dt >= (covTimeStepMax - dtIMU)) || (summedDelAng.length() > covDelAngMax)) - { - CovariancePrediction(dt); - summedDelAng = summedDelAng.zero(); - summedDelVel = summedDelVel.zero(); - dt = 0.0f; - } + // If valid IMU data and states initialised, predict states and covariances + if (statesInitialised) { + // Run the strapdown INS equations every IMU update + UpdateStrapdownEquationsNED(); +#if 0 + // debug code - could be tunred into a filter mnitoring/watchdog function + float tempQuat[4]; - _initialized = true; - } + for (uint8_t j = 0; j <= 3; j++) tempQuat[j] = states[j]; - // Fuse GPS Measurements - if (newDataGps && statesInitialised) - { - // Convert GPS measurements to Pos NE, hgt and Vel NED - velNED[0] = _gps.vel_n_m_s; - velNED[1] = _gps.vel_e_m_s; - velNED[2] = _gps.vel_d_m_s; - calcposNED(posNED, gpsLat, gpsLon, gpsHgt, latRef, lonRef, hgtRef); + quat2eul(eulerEst, tempQuat); - posNE[0] = posNED[0]; - posNE[1] = posNED[1]; - // set fusion flags - fuseVelData = true; - fusePosData = true; - // recall states stored at time of measurement after adjusting for delays - RecallStates(statesAtVelTime, (IMUmsec - msecVelDelay)); - RecallStates(statesAtPosTime, (IMUmsec - msecPosDelay)); - // run the fusion step - FuseVelposNED(); - } - else - { - fuseVelData = false; - fusePosData = false; - } + for (uint8_t j = 0; j <= 2; j++) eulerDif[j] = eulerEst[j] - ahrsEul[j]; - if (newAdsData && statesInitialised) - { - // Could use a blend of GPS and baro alt data if desired - hgtMea = 1.0f*baroHgt + 0.0f*gpsHgt; - fuseHgtData = true; - // recall states stored at time of measurement after adjusting for delays - RecallStates(statesAtHgtTime, (IMUmsec - msecHgtDelay)); - // run the fusion step - FuseVelposNED(); - } - else - { - fuseHgtData = false; - } + if (eulerDif[2] > pi) eulerDif[2] -= 2 * pi; - // Fuse Magnetometer Measurements - if (newDataMag && statesInitialised) - { - fuseMagData = true; - RecallStates(statesAtMagMeasTime, (IMUmsec - msecMagDelay)); // Assume 50 msec avg delay for magnetometer data - } - else - { - fuseMagData = false; - } - if (statesInitialised) FuseMagnetometer(); + if (eulerDif[2] < -pi) eulerDif[2] += 2 * pi; - // Fuse Airspeed Measurements - if (newAdsData && statesInitialised && VtasMeas > 8.0f) - { - fuseVtasData = true; - RecallStates(statesAtVtasMeasTime, (IMUmsec - msecTasDelay)); // assume 100 msec avg delay for airspeed data - FuseAirspeed(); - } - else - { - fuseVtasData = false; - } +#endif + // store the predicted states for subsequent use by measurement fusion + StoreStates(IMUmsec); + // Check if on ground - status is used by covariance prediction + OnGroundCheck(); + onGround = false; + // sum delta angles and time used by covariance prediction + summedDelAng = summedDelAng + correctedDelAng; + summedDelVel = summedDelVel + dVelIMU; + dt += dtIMU; + + // perform a covariance prediction if the total delta angle has exceeded the limit + // or the time limit will be exceeded at the next IMU update + if ((dt >= (covTimeStepMax - dtIMU)) || (summedDelAng.length() > covDelAngMax)) { + CovariancePrediction(dt); + summedDelAng = summedDelAng.zero(); + summedDelVel = summedDelVel.zero(); + dt = 0.0f; + } + + _initialized = true; + } + + // Fuse GPS Measurements + if (newDataGps && statesInitialised) { + // Convert GPS measurements to Pos NE, hgt and Vel NED + velNED[0] = _gps.vel_n_m_s; + velNED[1] = _gps.vel_e_m_s; + velNED[2] = _gps.vel_d_m_s; + calcposNED(posNED, gpsLat, gpsLon, gpsHgt, latRef, lonRef, hgtRef); + + posNE[0] = posNED[0]; + posNE[1] = posNED[1]; + // set fusion flags + fuseVelData = true; + fusePosData = true; + // recall states stored at time of measurement after adjusting for delays + RecallStates(statesAtVelTime, (IMUmsec - msecVelDelay)); + RecallStates(statesAtPosTime, (IMUmsec - msecPosDelay)); + // run the fusion step + FuseVelposNED(); + + } else { + fuseVelData = false; + fusePosData = false; + } + + if (newAdsData && statesInitialised) { + // Could use a blend of GPS and baro alt data if desired + hgtMea = 1.0f * baroHgt + 0.0f * gpsHgt; + fuseHgtData = true; + // recall states stored at time of measurement after adjusting for delays + RecallStates(statesAtHgtTime, (IMUmsec - msecHgtDelay)); + // run the fusion step + FuseVelposNED(); + + } else { + fuseHgtData = false; + } + + // Fuse Magnetometer Measurements + if (newDataMag && statesInitialised) { + fuseMagData = true; + RecallStates(statesAtMagMeasTime, (IMUmsec - msecMagDelay)); // Assume 50 msec avg delay for magnetometer data + + } else { + fuseMagData = false; + } + + if (statesInitialised) FuseMagnetometer(); + + // Fuse Airspeed Measurements + if (newAdsData && statesInitialised && VtasMeas > 8.0f) { + fuseVtasData = true; + RecallStates(statesAtVtasMeasTime, (IMUmsec - msecTasDelay)); // assume 100 msec avg delay for airspeed data + FuseAirspeed(); + + } else { + fuseVtasData = false; + } // if (hrt_elapsed_time(&start_time) > 100000 && !_initialized && (GPSstatus == 3)) { // InitialiseFilter(velNED); @@ -805,7 +811,7 @@ FixedwingEstimator::task_main() // /* prepare the delta angles and time used by the covariance prediction */ // summedDelAng = summedDelAng + correctedDelAng; // summedDelVel = summedDelVel + correctedDelVel; - + // dt += dtIMU; // /* predict the covairance if the total delta angle has exceeded the threshold @@ -829,7 +835,7 @@ FixedwingEstimator::task_main() // posNE[0] = posNED[0]; // posNE[1] = posNED[1]; - // // set flags for further processing + // // set flags for further processing // fuseVelData = true; // fusePosData = true; @@ -896,7 +902,7 @@ FixedwingEstimator::task_main() math::Vector<3> euler = R.to_euler(); for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) - _att.R[i][j] = R(i, j); + _att.R[i][j] = R(i, j); _att.timestamp = last_sensor_timestamp; _att.q[0] = states[0]; @@ -1005,7 +1011,7 @@ void print_status() math::Vector<3> euler = R.to_euler(); printf("attitude: roll: %8.4f, pitch %8.4f, yaw: %8.4f degrees\n", - (double)math::degrees(euler(0)), (double)math::degrees(euler(1)), (double)math::degrees(euler(2))); + (double)math::degrees(euler(0)), (double)math::degrees(euler(1)), (double)math::degrees(euler(2))); // State vector: // 0-3: quaternions (q0, q1, q2, q3) @@ -1027,15 +1033,15 @@ void print_status() printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)states[15], (double)states[16], (double)states[17]); printf("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f\n", (double)states[18], (double)states[19], (double)states[20]); printf("states: %s %s %s %s %s %s %s %s %s\n", - (statesInitialised) ? "INITIALIZED" : "NON_INIT", - (onGround) ? "ON_GROUND" : "AIRBORNE", - (fuseVelData) ? "FUSE_VEL" : "INH_VEL", - (fusePosData) ? "FUSE_POS" : "INH_POS", - (fuseHgtData) ? "FUSE_HGT" : "INH_HGT", - (fuseMagData) ? "FUSE_MAG" : "INH_MAG", - (fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS", - (useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD", - (useCompass) ? "USE_COMPASS" : "IGN_COMPASS"); + (statesInitialised) ? "INITIALIZED" : "NON_INIT", + (onGround) ? "ON_GROUND" : "AIRBORNE", + (fuseVelData) ? "FUSE_VEL" : "INH_VEL", + (fusePosData) ? "FUSE_POS" : "INH_POS", + (fuseHgtData) ? "FUSE_HGT" : "INH_HGT", + (fuseMagData) ? "FUSE_MAG" : "INH_MAG", + (fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS", + (useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD", + (useCompass) ? "USE_COMPASS" : "IGN_COMPASS"); } int fw_att_pos_estimator_main(int argc, char *argv[]) From 447d159964e3af6c4ac2004d3a9e3217c0f269b2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 18 Feb 2014 08:33:58 +0100 Subject: [PATCH 075/381] Initialize the filter immediately, re-init once GPS becomes available (needs in-flight testing) --- .../fw_att_pos_estimator_main.cpp | 46 ++++++++++++++++--- 1 file changed, 40 insertions(+), 6 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 4f5e3c9b48..2b7cdc1189 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -681,11 +681,26 @@ FixedwingEstimator::task_main() * PART TWO: EXECUTE THE FILTER **/ - if (/*(IMUmsec > msecAlignTime) &&*/ !statesInitialised && (GPSstatus == 3)) { - velNED[0] = _gps.vel_n_m_s; - velNED[1] = _gps.vel_e_m_s; - velNED[2] = _gps.vel_d_m_s; - InitialiseFilter(velNED); + // Wait long enough to ensure all sensors updated once + // XXX we rather want to check all updated + + + if (hrt_elapsed_time(&start_time) > 100000) { + + if (!_gps_initialized && (GPSstatus == 3)) { + velNED[0] = _gps.vel_n_m_s; + velNED[1] = _gps.vel_e_m_s; + velNED[2] = _gps.vel_d_m_s; + InitialiseFilter(velNED); + + _gps_initialized = true; + + } else if (!statesInitialised) { + velNED[0] = _gps.vel_n_m_s; + velNED[1] = _gps.vel_e_m_s; + velNED[2] = _gps.vel_d_m_s; + InitialiseFilter(velNED); + } } // If valid IMU data and states initialised, predict states and covariances @@ -730,7 +745,7 @@ FixedwingEstimator::task_main() } // Fuse GPS Measurements - if (newDataGps && statesInitialised) { + if (newDataGps && _gps_initialized) { // Convert GPS measurements to Pos NE, hgt and Vel NED velNED[0] = _gps.vel_n_m_s; velNED[1] = _gps.vel_e_m_s; @@ -748,6 +763,25 @@ FixedwingEstimator::task_main() // run the fusion step FuseVelposNED(); + } else if (statesInitialised) { + // Convert GPS measurements to Pos NE, hgt and Vel NED + velNED[0] = 0.0f; + velNED[1] = 0.0f; + velNED[2] = 0.0f; + posNED[0] = 0.0f; + posNED[1] = 0.0f; + posNED[2] = 0.0f; + + posNE[0] = posNED[0]; + posNE[1] = posNED[1]; + // set fusion flags + fuseVelData = true; + fusePosData = true; + // recall states stored at time of measurement after adjusting for delays + RecallStates(statesAtVelTime, (IMUmsec - msecVelDelay)); + RecallStates(statesAtPosTime, (IMUmsec - msecPosDelay)); + // run the fusion step + FuseVelposNED(); } else { fuseVelData = false; fusePosData = false; From a7ee2e71a0a855f6206f6f9009c5893e942e9bed Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Tue, 18 Feb 2014 21:36:55 +0100 Subject: [PATCH 076/381] Recognize @unit parameter documentation tag. Not used in any output yet. --- Tools/px4params/srcparser.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/Tools/px4params/srcparser.py b/Tools/px4params/srcparser.py index 1b2d301108..9e202d3b87 100644 --- a/Tools/px4params/srcparser.py +++ b/Tools/px4params/srcparser.py @@ -44,6 +44,7 @@ class Parameter(object): "default": 6, "min": 5, "max": 4, + "unit": 3, # all others == 0 (sorted alphabetically) } @@ -86,7 +87,7 @@ class Parser(object): re_is_a_number = re.compile(r'^-?[0-9\.]') re_remove_dots = re.compile(r'\.+$') - valid_tags = set(["min", "max", "group"]) + valid_tags = set(["group", "min", "max", "unit"]) # Order of parameter groups priority = { From e9feef8ff6ef1f4dcc7b55a75301e2275e662dae Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Tue, 18 Feb 2014 22:00:10 +0100 Subject: [PATCH 077/381] Simplify file extension check. --- Tools/px4params/scanner.py | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/Tools/px4params/scanner.py b/Tools/px4params/scanner.py index 8779b7bbf6..43fd35e600 100644 --- a/Tools/px4params/scanner.py +++ b/Tools/px4params/scanner.py @@ -8,20 +8,15 @@ class Scanner(object): to the Parser. """ - re_file_extension = re.compile(r'\.([^\.]+)$') - def ScanDir(self, srcdir, parser): """ Scans provided path and passes all found contents to the parser using parser.Parse method. """ - extensions = set(parser.GetSupportedExtensions()) + extensions = tuple(parser.GetSupportedExtensions()) for dirname, dirnames, filenames in os.walk(srcdir): for filename in filenames: - m = self.re_file_extension.search(filename) - if m: - ext = m.group(1) - if ext in extensions: + if filename.endswith(extensions): path = os.path.join(dirname, filename) self.ScanFile(path, parser) From 55e66f5ad1fecb02bcb9782265303e20756b5cb9 Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Tue, 18 Feb 2014 22:07:42 +0100 Subject: [PATCH 078/381] Rename Scanner and Parser classes to SourceScanner and SourceParser to avoid naming conflicts. --- Tools/px4params/px_process_params.py | 10 +++++----- Tools/px4params/srcparser.py | 2 +- Tools/px4params/{scanner.py => srcscanner.py} | 2 +- 3 files changed, 7 insertions(+), 7 deletions(-) rename Tools/px4params/{scanner.py => srcscanner.py} (96%) diff --git a/Tools/px4params/px_process_params.py b/Tools/px4params/px_process_params.py index 7799f63484..8503a2f4ce 100755 --- a/Tools/px4params/px_process_params.py +++ b/Tools/px4params/px_process_params.py @@ -39,19 +39,19 @@ # definitions, and outputs list of parameters in XML and DokuWiki formats. # -import scanner +import srcscanner import srcparser import output_xml import output_dokuwiki_tables import output_dokuwiki_listings # Initialize parser -prs = srcparser.Parser() +parser = srcparser.SourceParser() # Scan directories, and parse the files -sc = scanner.Scanner() -sc.ScanDir("../../src", prs) -groups = prs.GetParamGroups() +scanner = srcscanner.SourceScanner() +scanner.ScanDir("../../src", parser) +groups = parser.GetParamGroups() # Output into XML out = output_xml.XMLOutput(groups) diff --git a/Tools/px4params/srcparser.py b/Tools/px4params/srcparser.py index 9e202d3b87..0a4d21d264 100644 --- a/Tools/px4params/srcparser.py +++ b/Tools/px4params/srcparser.py @@ -72,7 +72,7 @@ class Parameter(object): """ return self.fields.get(code) -class Parser(object): +class SourceParser(object): """ Parses provided data and stores all found parameters internally. """ diff --git a/Tools/px4params/scanner.py b/Tools/px4params/srcscanner.py similarity index 96% rename from Tools/px4params/scanner.py rename to Tools/px4params/srcscanner.py index 43fd35e600..d7eca72d72 100644 --- a/Tools/px4params/scanner.py +++ b/Tools/px4params/srcscanner.py @@ -2,7 +2,7 @@ import os import re import codecs -class Scanner(object): +class SourceScanner(object): """ Traverses directory tree, reads all source files, and passes their contents to the Parser. From 8d41155fb66ce04834141151c4a0fa54de0b4b31 Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Tue, 18 Feb 2014 22:34:52 +0100 Subject: [PATCH 079/381] Rework px_process_params.py into a fully featured program. --- Tools/.gitignore | 2 + Tools/px4params/.gitignore | 4 - Tools/px4params/README.md | 10 +- Tools/px4params/__init__.py | 1 + ...tput_dokuwiki_tables.py => dokuwikiout.py} | 27 ---- Tools/px4params/dokuwikirpc.py | 16 ++ Tools/px4params/output_dokuwiki_listings.py | 31 ---- Tools/px4params/px_process_params.py | 67 --------- Tools/px4params/{output_xml.py => xmlout.py} | 0 Tools/px4params/xmlrpc.sh | 5 - Tools/px_process_params.py | 140 ++++++++++++++++++ Tools/px_update_wiki.sh | 2 + 12 files changed, 162 insertions(+), 143 deletions(-) create mode 100644 Tools/.gitignore delete mode 100644 Tools/px4params/.gitignore create mode 100644 Tools/px4params/__init__.py rename Tools/px4params/{output_dokuwiki_tables.py => dokuwikiout.py} (71%) create mode 100644 Tools/px4params/dokuwikirpc.py delete mode 100644 Tools/px4params/output_dokuwiki_listings.py delete mode 100755 Tools/px4params/px_process_params.py rename Tools/px4params/{output_xml.py => xmlout.py} (100%) delete mode 100644 Tools/px4params/xmlrpc.sh create mode 100644 Tools/px_process_params.py create mode 100644 Tools/px_update_wiki.sh diff --git a/Tools/.gitignore b/Tools/.gitignore new file mode 100644 index 0000000000..7628bda821 --- /dev/null +++ b/Tools/.gitignore @@ -0,0 +1,2 @@ +parameters.wiki +parameters.xml diff --git a/Tools/px4params/.gitignore b/Tools/px4params/.gitignore deleted file mode 100644 index d78b71a6e8..0000000000 --- a/Tools/px4params/.gitignore +++ /dev/null @@ -1,4 +0,0 @@ -parameters.wiki -parameters.xml -parameters.wikirpc.xml -cookies.txt \ No newline at end of file diff --git a/Tools/px4params/README.md b/Tools/px4params/README.md index a23b447995..50dcd2e296 100644 --- a/Tools/px4params/README.md +++ b/Tools/px4params/README.md @@ -1,9 +1 @@ -h1. PX4 Parameters Processor - -It's designed to scan PX4 source codes, find declarations of tunable parameters, -and generate the list in various formats. - -Currently supported formats are: - -* XML for the parametric UI generator -* Human-readable description in DokuWiki format +This folder contains a python library used by px_process_params.py diff --git a/Tools/px4params/__init__.py b/Tools/px4params/__init__.py new file mode 100644 index 0000000000..3a9f1e2c6d --- /dev/null +++ b/Tools/px4params/__init__.py @@ -0,0 +1 @@ +__all__ = ["srcscanner", "srcparser", "xmlout", "dokuwikiout", "dokuwikirpc"] \ No newline at end of file diff --git a/Tools/px4params/output_dokuwiki_tables.py b/Tools/px4params/dokuwikiout.py similarity index 71% rename from Tools/px4params/output_dokuwiki_tables.py rename to Tools/px4params/dokuwikiout.py index aa04304df7..705b0bb5fb 100644 --- a/Tools/px4params/output_dokuwiki_tables.py +++ b/Tools/px4params/dokuwikiout.py @@ -47,30 +47,3 @@ class DokuWikiTablesOutput(): def Save(self, filename): with codecs.open(filename, 'w', 'utf-8') as f: f.write(self.output) - - def SaveRpc(self, filename): - with codecs.open(filename, 'w', 'utf-8') as f: - f.write(""" - - wiki.putPage - - - - :firmware:parameters - - - - - """) - f.write(escape(self.output)) - f.write(""" - - - - - sum - Updated parameters automagically from code. - - - -""") diff --git a/Tools/px4params/dokuwikirpc.py b/Tools/px4params/dokuwikirpc.py new file mode 100644 index 0000000000..407d306fda --- /dev/null +++ b/Tools/px4params/dokuwikirpc.py @@ -0,0 +1,16 @@ +try: + import xmlrpclib +except ImportError: + import xmlrpc.client as xmlrpclib + +# See https://www.dokuwiki.org/devel:xmlrpc for a list of available functions! +# Usage example: +# xmlrpc = dokuwikirpc.get_xmlrpc(url, username, password) +# print(xmlrpc.dokuwiki.getVersion()) + +def get_xmlrpc(url, username, password): + #proto, url = url.split("://") + #url = proto + "://" + username + ":" + password + "@" + url + "/lib/exe/xmlrpc.php" + url += "/lib/exe/xmlrpc.php?u=" + username + "&p=" + password + + return xmlrpclib.ServerProxy(url) diff --git a/Tools/px4params/output_dokuwiki_listings.py b/Tools/px4params/output_dokuwiki_listings.py deleted file mode 100644 index 83c50ae15d..0000000000 --- a/Tools/px4params/output_dokuwiki_listings.py +++ /dev/null @@ -1,31 +0,0 @@ -import codecs - -class DokuWikiListingsOutput(): - def __init__(self, groups): - result = "" - for group in groups: - result += "==== %s ====\n\n" % group.GetName() - for param in group.GetParams(): - code = param.GetFieldValue("code") - name = param.GetFieldValue("short_desc") - if code != name: - name = "%s (%s)" % (name, code) - result += "=== %s ===\n\n" % name - long_desc = param.GetFieldValue("long_desc") - if long_desc is not None: - result += "%s\n\n" % long_desc - min_val = param.GetFieldValue("min") - if min_val is not None: - result += "* Minimal value: %s\n" % min_val - max_val = param.GetFieldValue("max") - if max_val is not None: - result += "* Maximal value: %s\n" % max_val - def_val = param.GetFieldValue("default") - if def_val is not None: - result += "* Default value: %s\n" % def_val - result += "\n" - self.output = result - - def Save(self, filename): - with codecs.open(filename, 'w', 'utf-8') as f: - f.write(self.output) diff --git a/Tools/px4params/px_process_params.py b/Tools/px4params/px_process_params.py deleted file mode 100755 index 8503a2f4ce..0000000000 --- a/Tools/px4params/px_process_params.py +++ /dev/null @@ -1,67 +0,0 @@ -#!/usr/bin/env python -############################################################################ -# -# 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. -# -############################################################################ - -# -# PX4 paramers processor (main executable file) -# -# It scans src/ subdirectory of the project, collects all parameters -# definitions, and outputs list of parameters in XML and DokuWiki formats. -# - -import srcscanner -import srcparser -import output_xml -import output_dokuwiki_tables -import output_dokuwiki_listings - -# Initialize parser -parser = srcparser.SourceParser() - -# Scan directories, and parse the files -scanner = srcscanner.SourceScanner() -scanner.ScanDir("../../src", parser) -groups = parser.GetParamGroups() - -# Output into XML -out = output_xml.XMLOutput(groups) -out.Save("parameters.xml") - -# Output to DokuWiki listings -#out = output_dokuwiki_listings.DokuWikiListingsOutput(groups) -#out.Save("parameters.wiki") - -# Output to DokuWiki tables -out = output_dokuwiki_tables.DokuWikiTablesOutput(groups) -out.Save("parameters.wiki") -out.SaveRpc("parameters.wikirpc.xml") diff --git a/Tools/px4params/output_xml.py b/Tools/px4params/xmlout.py similarity index 100% rename from Tools/px4params/output_xml.py rename to Tools/px4params/xmlout.py diff --git a/Tools/px4params/xmlrpc.sh b/Tools/px4params/xmlrpc.sh deleted file mode 100644 index efd177f46a..0000000000 --- a/Tools/px4params/xmlrpc.sh +++ /dev/null @@ -1,5 +0,0 @@ -python px_process_params.py - -rm cookies.txt -curl --cookie cookies.txt --cookie-jar cookies.txt --user-agent Mozilla/4.0 --data "u=$XMLRPCUSER&p=$XMLRPCPASS" https://pixhawk.org/start?do=login -curl -k --cookie cookies.txt -H "Content-Type: application/xml" -X POST --data-binary @parameters.wikirpc.xml "https://pixhawk.org/lib/exe/xmlrpc.php" diff --git a/Tools/px_process_params.py b/Tools/px_process_params.py new file mode 100644 index 0000000000..12128a997e --- /dev/null +++ b/Tools/px_process_params.py @@ -0,0 +1,140 @@ +#!/usr/bin/env python +############################################################################ +# +# 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. +# +############################################################################ + +# +# PX4 paramers processor (main executable file) +# +# This tool scans the PX4 source code for declarations of tunable parameters +# and outputs the list in various formats. +# +# Currently supported formats are: +# * XML for the parametric UI generator +# * Human-readable description in DokuWiki page format +# +# This tool also allows to automatically upload the human-readable version +# to the DokuWiki installation via XML-RPC. +# + +from __future__ import print_function +import sys +import os +import argparse +from px4params import srcscanner, srcparser, xmlout, dokuwikiout, dokuwikirpc + +def main(): + # Parse command line arguments + parser = argparse.ArgumentParser(description="Process parameter documentation.") + parser.add_argument("-s", "--src-path", + default="../src", + metavar="PATH", + help="path to source files to scan for parameters") + parser.add_argument("-x", "--xml", + nargs='?', + const="parameters.xml", + metavar="FILENAME", + help="Create XML file" + " (default FILENAME: parameters.xml)") + parser.add_argument("-w", "--wiki", + nargs='?', + const="parameters.wiki", + metavar="FILENAME", + help="Create DokuWiki file" + " (default FILENAME: parameters.wiki)") + parser.add_argument("-u", "--wiki-update", + nargs='?', + const="firmware:parameters", + metavar="PAGENAME", + help="Update DokuWiki page" + " (default PAGENAME: firmware:parameters)") + parser.add_argument("--wiki-url", + default="https://pixhawk.org", + metavar="URL", + help="DokuWiki URL" + " (default: https://pixhawk.org)") + parser.add_argument("--wiki-user", + default=os.environ.get('XMLRPCUSER', None), + metavar="USERNAME", + help="DokuWiki XML-RPC user name" + " (default: $XMLRPCUSER environment variable)") + parser.add_argument("--wiki-pass", + default=os.environ.get('XMLRPCPASS', None), + metavar="PASSWORD", + help="DokuWiki XML-RPC user password" + " (default: $XMLRPCUSER environment variable)") + parser.add_argument("--wiki-summary", + metavar="SUMMARY", + default="Automagically updated parameter documentation from code.", + help="DokuWiki page edit summary") + args = parser.parse_args() + + # Check for valid command + if not (args.xml or args.wiki or args.wiki_update): + print("Error: You need to specify at least one output method!\n") + parser.print_usage() + sys.exit(1) + + # Initialize source scanner and parser + scanner = srcscanner.SourceScanner() + parser = srcparser.SourceParser() + + # Scan directories, and parse the files + print("Scanning source path " + args.src_path) + scanner.ScanDir(args.src_path, parser) + param_groups = parser.GetParamGroups() + + # Output to XML file + if args.xml: + print("Creating XML file " + args.xml) + out = xmlout.XMLOutput(param_groups) + out.Save(args.xml) + + # Output to DokuWiki tables + if args.wiki or args.wiki_update: + out = dokuwikiout.DokuWikiTablesOutput(param_groups) + if args.wiki: + print("Creating wiki file " + args.wiki) + out.Save(args.wiki) + if args.wiki_update: + if args.wiki_user and args.wiki_pass: + print("Updating wiki page " + args.wiki_update) + xmlrpc = dokuwikirpc.get_xmlrpc(args.wiki_url, args.wiki_user, args.wiki_pass) + xmlrpc.wiki.putPage(args.wiki_update, out.output, {'sum': args.wiki_summary}) + else: + print("Error: You need to specify DokuWiki XML-RPC username and password!") + + print("All done!") + + +if __name__ == "__main__": + main() diff --git a/Tools/px_update_wiki.sh b/Tools/px_update_wiki.sh new file mode 100644 index 0000000000..d66bb9e10b --- /dev/null +++ b/Tools/px_update_wiki.sh @@ -0,0 +1,2 @@ +# Remember to set the XMLRPCUSER and XMLRPCPASS environment variables +python px_process_params.py --wiki-update From ec78fcf2b9e75c6ca9ab6cd0772ee2b7beef4300 Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Wed, 19 Feb 2014 01:54:01 +0100 Subject: [PATCH 080/381] Improve DokuWiki formatting. --- Tools/px4params/dokuwikiout.py | 41 +++++++++++++++------------------- 1 file changed, 18 insertions(+), 23 deletions(-) diff --git a/Tools/px4params/dokuwikiout.py b/Tools/px4params/dokuwikiout.py index 705b0bb5fb..77e0ef53db 100644 --- a/Tools/px4params/dokuwikiout.py +++ b/Tools/px4params/dokuwikiout.py @@ -3,11 +3,14 @@ import codecs class DokuWikiTablesOutput(): def __init__(self, groups): - result = "====== Parameter Reference ======\nThis list is auto-generated every few minutes and contains the most recent parameter names and default values.\n\n" + result = ("====== Parameter Reference ======\n" + "**This list is auto-generated from the source code** and contains the most recent parameter documentation.\n" + "\n") for group in groups: result += "==== %s ====\n\n" % group.GetName() - result += "|< 100% 20% 20% 10% 10% 10% 30%>|\n" - result += "^ Name ^ Description ^ Min ^ Max ^ Default ^ Comment ^\n" + result += "|< 100% 25% 45% 10% 10% 10% >|\n" + result += "^ Name ^ Description ^ Min ^ Max ^ Default ^\n" + result += "^ ::: ^ Comment ^^^^\n" for param in group.GetParams(): code = param.GetFieldValue("code") name = param.GetFieldValue("short_desc") @@ -16,31 +19,23 @@ class DokuWikiTablesOutput(): def_val = param.GetFieldValue("default") long_desc = param.GetFieldValue("long_desc") - name = name.replace("\n", " ") - result += "| %s | %s |" % (code, name) - - if min_val is not None: - result += " %s |" % min_val + if name == code: + name = "" else: - result += " |" + name = name.replace("\n", " ") + name = name.replace("|", "%%|%%") + name = name.replace("^", "%%^%%") - if max_val is not None: - result += " %s |" % max_val - else: - result += " |" - - if def_val is not None: - result += " %s |" % def_val - else: - result += " |" + result += "| **%s** |" % code + result += " %s |" % name + result += " %s |" % (min_val or "") + result += " %s |" % (max_val or "") + result += " %s |" % (def_val or "") + result += "\n" if long_desc is not None: - long_desc = long_desc.replace("\n", " ") - result += " %s |" % long_desc - else: - result += " |" + result += "| ::: |
%s
||||\n" % long_desc - result += "\n" result += "\n" self.output = result; From c5311b18fef0e215c019a4686ca9697224d1cd31 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 21 Feb 2014 13:22:04 +0100 Subject: [PATCH 081/381] Build fixes --- src/modules/fw_att_pos_estimator/estimator.cpp | 6 ------ .../fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 4 +++- 2 files changed, 3 insertions(+), 7 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index bae4272340..0f2559c58b 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -1,9 +1,5 @@ #include "estimator.h" -// For debugging only -#include -#include - // Global variables float KH[n_states][n_states]; // intermediate result used for covariance updates float KHP[n_states][n_states]; // intermediate result used for covariance updates @@ -1840,8 +1836,6 @@ void InitialiseFilter(float (&initvelNED)[3]) Vector3f initMagXYZ; initMagXYZ = magData - magBias; AttitudeInit(accel.x, accel.y, accel.z, initMagXYZ.x, initMagXYZ.y, initMagXYZ.z, initQuat); - printf("initializing: accel: %8.4f %8.4f %8.4f, mag: %8.4f %8.4f %8.4f q: %8.4f %8.4f %8.4f %8.4f\n", - accel.x, accel.y, accel.z, initMagXYZ.x, initMagXYZ.y, initMagXYZ.z, initQuat[0], initQuat[1], initQuat[2], initQuat[3]); // Calculate initial Tbn matrix and rotate Mag measurements into NED // to set initial NED magnetic field states diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 2b7cdc1189..688a44c470 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -172,6 +172,7 @@ private: perf_counter_t _perf_airspeed; /// Date: Fri, 21 Feb 2014 13:44:34 +0100 Subject: [PATCH 082/381] GPS reinit completed, put in NaN catcher --- src/modules/fw_att_pos_estimator/estimator.h | 4 ++-- .../fw_att_pos_estimator_main.cpp | 20 ++++++++++++++++--- 2 files changed, 19 insertions(+), 5 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/estimator.h b/src/modules/fw_att_pos_estimator/estimator.h index bc71810183..ff31a84acc 100644 --- a/src/modules/fw_att_pos_estimator/estimator.h +++ b/src/modules/fw_att_pos_estimator/estimator.h @@ -122,8 +122,8 @@ extern float baroHgt; extern bool statesInitialised; -const float covTimeStepMax = 0.2f; // maximum time allowed between covariance predictions -const float covDelAngMax = 0.05f; // maximum delta angle between covariance predictions +const float covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions +const float covDelAngMax = 0.02f; // maximum delta angle between covariance predictions void UpdateStrapdownEquationsNED(); diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 688a44c470..95549ad219 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -694,13 +694,20 @@ FixedwingEstimator::task_main() velNED[1] = _gps.vel_e_m_s; velNED[2] = _gps.vel_d_m_s; InitialiseFilter(velNED); + warnx("GPS REINIT"); _gps_initialized = true; } else if (!statesInitialised) { - velNED[0] = _gps.vel_n_m_s; - velNED[1] = _gps.vel_e_m_s; - velNED[2] = _gps.vel_d_m_s; + velNED[0] = 0.0f; + velNED[1] = 0.0f; + velNED[2] = 0.0f; + posNED[0] = 0.0f; + posNED[1] = 0.0f; + posNED[2] = 0.0f; + + posNE[0] = posNED[0]; + posNE[1] = posNED[1]; InitialiseFilter(velNED); } } @@ -970,7 +977,14 @@ FixedwingEstimator::task_main() /* advertise and publish */ _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att); } + } + if (!isfinite(states[0])) { + print_status(); + _exit(0); + } + + if (_gps_initialized) { _local_pos.timestamp = last_sensor_timestamp; _local_pos.x = states[7]; _local_pos.y = states[8]; From 73edc020162841f250cf8ba26c6bdc40837a0e4f Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 23 Feb 2014 13:42:15 +0100 Subject: [PATCH 083/381] mavlink: fix verbose mode and actually publish mission --- src/modules/mavlink/mavlink_main.cpp | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 42504dea9f..15f2fd2ca4 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -169,8 +169,10 @@ Mavlink::Mavlink() : /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mavlink")), - _mavlink_hil_enabled(false) + _mavlink_hil_enabled(false), // _params_sub(-1) + + mission_pub(-1) { wpm = &wpm_s; fops.ioctl = (int (*)(file*, int, long unsigned int))&mavlink_dev_ioctl; @@ -987,6 +989,10 @@ void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq) if (_verbose) warnx("Broadcasted new current waypoint %u", wpc.seq); + } else if (seq == 0 && wpm->size == 0) { + + /* don't broadcast if no WPs */ + } else { mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds"); if (_verbose) warnx("ERROR: index out of bounds"); @@ -1534,7 +1540,7 @@ Mavlink::task_main(int argc, char *argv[]) argc -= 2; argv += 2; - while ((ch = getopt(argc, argv, "b:d:eo")) != EOF) { + while ((ch = getopt(argc, argv, "b:d:eov")) != EOF) { switch (ch) { case 'b': _baudrate = strtoul(optarg, NULL, 10); @@ -1558,6 +1564,7 @@ Mavlink::task_main(int argc, char *argv[]) case 'v': _verbose = true; + break; default: usage(); @@ -1778,8 +1785,9 @@ Mavlink::task_main(int argc, char *argv[]) if (_verbose) warnx("Got mission result"); - if (mission_result.mission_reached) + if (mission_result.mission_reached) { mavlink_wpm_send_waypoint_reached((uint16_t)mission_result.mission_index_reached); + } mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission); } From b36bf7b17e4827465d1d543a00ea3898813c77ea Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Feb 2014 15:59:56 -0800 Subject: [PATCH 084/381] Removed gravity and on ground check hardcoded testing values --- src/modules/fw_att_pos_estimator/estimator.h | 2 +- src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/estimator.h b/src/modules/fw_att_pos_estimator/estimator.h index ff31a84acc..d65be322f7 100644 --- a/src/modules/fw_att_pos_estimator/estimator.h +++ b/src/modules/fw_att_pos_estimator/estimator.h @@ -3,7 +3,7 @@ #pragma once -#define GRAVITY_MSS 9.76f//9.80665f +#define GRAVITY_MSS 9.80665f #define deg2rad 0.017453292f #define rad2deg 57.295780f #define pi 3.141592657f diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 95549ad219..38162959d3 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -735,7 +735,6 @@ FixedwingEstimator::task_main() StoreStates(IMUmsec); // Check if on ground - status is used by covariance prediction OnGroundCheck(); - onGround = false; // sum delta angles and time used by covariance prediction summedDelAng = summedDelAng + correctedDelAng; summedDelVel = summedDelVel + dVelIMU; From acb1bc1383399465f3183df6a61698dbc9f5e958 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Feb 2014 18:31:44 -0800 Subject: [PATCH 085/381] Disable time compensation which gets us reasonable results --- src/modules/fw_att_pos_estimator/estimator.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index 0f2559c58b..a7c2cb0e35 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -1629,14 +1629,14 @@ void RecallStates(float (&statesForFusion)[n_states], uint32_t msec) bestTimeDelta = timeDelta; } } - if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error - { - for (uint8_t i=0; i < n_states; i++) statesForFusion[i] = storedStates[i][bestStoreIndex]; - } - else // otherwise output current state - { + // if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error + // { + // for (uint8_t i=0; i < n_states; i++) statesForFusion[i] = storedStates[i][bestStoreIndex]; + // } + // else // otherwise output current state + // { for (uint8_t i=0; i < n_states; i++) statesForFusion[i] = states[i]; - } + // } } void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]) From b4c1713b96d452efa8c2285537f0969d1e6dcb77 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Feb 2014 18:55:06 -0800 Subject: [PATCH 086/381] Fix up time delay compensation loading --- .../fw_att_pos_estimator/estimator.cpp | 30 +++++++++---------- src/modules/fw_att_pos_estimator/estimator.h | 2 +- 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index a7c2cb0e35..e449a66028 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -6,8 +6,6 @@ float KHP[n_states][n_states]; // intermediate result used for covariance update float P[n_states][n_states]; // covariance matrix float Kfusion[n_states]; // Kalman gains float states[n_states]; // state matrix -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 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) Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad) @@ -55,6 +53,9 @@ float gpsLon; float gpsHgt; uint8_t GPSstatus; +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 + // Baro input float baroHgt; @@ -1614,14 +1615,13 @@ void StoreStates(uint64_t timestamp_ms) } // Output the state vector stored at the time that best matches that specified by msec -void RecallStates(float (&statesForFusion)[n_states], uint32_t msec) +void RecallStates(float (&statesForFusion)[n_states], uint64_t msec) { long int bestTimeDelta = 200; - uint8_t storeIndex; - uint8_t bestStoreIndex = 0; - for (storeIndex=0; storeIndex < data_buffer_size; storeIndex++) + unsigned bestStoreIndex = 0; + for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++) { - long int timeDelta = msec - statetimeStamp[storeIndex]; + int64_t timeDelta = (int64_t)msec - statetimeStamp[storeIndex]; if (timeDelta < 0) timeDelta = -timeDelta; if (timeDelta < bestTimeDelta) { @@ -1629,14 +1629,14 @@ void RecallStates(float (&statesForFusion)[n_states], uint32_t msec) bestTimeDelta = timeDelta; } } - // if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error - // { - // for (uint8_t i=0; i < n_states; i++) statesForFusion[i] = storedStates[i][bestStoreIndex]; - // } - // else // otherwise output current state - // { + if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error + { + for (uint8_t i=0; i < n_states; i++) statesForFusion[i] = storedStates[i][bestStoreIndex]; + } + else // otherwise output current state + { for (uint8_t i=0; i < n_states; i++) statesForFusion[i] = states[i]; - // } + } } void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]) @@ -1825,7 +1825,7 @@ void InitialiseFilter(float (&initvelNED)[3]) for (unsigned i = 0; i < data_buffer_size; i++) { for (unsigned j = 0; j < n_states; j++) { - + storedStates[j][i] = 0.0f; } statetimeStamp[i] = 0; diff --git a/src/modules/fw_att_pos_estimator/estimator.h b/src/modules/fw_att_pos_estimator/estimator.h index d65be322f7..d68ac1d34e 100644 --- a/src/modules/fw_att_pos_estimator/estimator.h +++ b/src/modules/fw_att_pos_estimator/estimator.h @@ -147,7 +147,7 @@ void quatNorm(float (&quatOut)[4], const float quatIn[4]); void StoreStates(uint64_t timestamp_ms); // recall stste vector stored at closest time to the one specified by msec -void RecallStates(float (&statesForFusion)[n_states], uint32_t msec); +void RecallStates(float (&statesForFusion)[n_states], uint64_t msec); void quat2Tbn(Mat3f &Tbn, const float (&quat)[4]); From 76ac14d3c4e44c22b04b312acfa34186dfb028d0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Feb 2014 19:05:32 -0800 Subject: [PATCH 087/381] Use right combination of casts --- src/modules/fw_att_pos_estimator/estimator.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index e449a66028..c9e809280d 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -1617,11 +1617,11 @@ void StoreStates(uint64_t timestamp_ms) // Output the state vector stored at the time that best matches that specified by msec void RecallStates(float (&statesForFusion)[n_states], uint64_t msec) { - long int bestTimeDelta = 200; + int64_t bestTimeDelta = 200; unsigned bestStoreIndex = 0; for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++) { - int64_t timeDelta = (int64_t)msec - statetimeStamp[storeIndex]; + int64_t timeDelta = (int)msec - statetimeStamp[storeIndex]; if (timeDelta < 0) timeDelta = -timeDelta; if (timeDelta < bestTimeDelta) { From 926c4701c71fb2689025decbc454d14c6df85e76 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 24 Feb 2014 15:17:13 +0100 Subject: [PATCH 088/381] mavlink: set current WP working as expected, report current WP with 0.5 Hz --- src/modules/mavlink/mavlink_main.cpp | 19 +++++++++++-------- src/modules/navigator/navigator_main.cpp | 2 +- src/modules/navigator/navigator_mission.cpp | 8 +++----- src/modules/navigator/navigator_mission.h | 2 +- 4 files changed, 16 insertions(+), 15 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 15f2fd2ca4..1ce467a7b8 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -987,8 +987,6 @@ void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq) mavlink_msg_mission_current_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpc); mavlink_missionlib_send_message(&msg); - if (_verbose) warnx("Broadcasted new current waypoint %u", wpc.seq); - } else if (seq == 0 && wpm->size == 0) { /* don't broadcast if no WPs */ @@ -1145,7 +1143,8 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) mission.current_index = wpc.seq; publish_mission(); - mavlink_wpm_send_waypoint_current(wpc.seq); + /* don't answer yet, wait for the navigator to respond, then publish the mission_result */ +// mavlink_wpm_send_waypoint_current(wpc.seq); } else { mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list"); @@ -1708,8 +1707,7 @@ Mavlink::task_main(int argc, char *argv[]) thread_running = true; - /* arm counter to go off immediately */ - unsigned lowspeed_counter = 10; + unsigned lowspeed_counter = 0; /* wakeup source(s) */ struct pollfd fds[1]; @@ -1739,7 +1737,7 @@ Mavlink::task_main(int argc, char *argv[]) } /* 1 Hz */ - if (lowspeed_counter == 10) { + if (lowspeed_counter % 10 == 0) { mavlink_update_system(); /* translate the current system state to mavlink state and mode */ @@ -1772,7 +1770,12 @@ Mavlink::task_main(int argc, char *argv[]) v_status.errors_count2, v_status.errors_count3, v_status.errors_count4); - lowspeed_counter = 0; + } + + /* 0.5 Hz */ + if (lowspeed_counter % 20 == 0) { + + mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission); } lowspeed_counter++; @@ -1783,7 +1786,7 @@ Mavlink::task_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result); - if (_verbose) warnx("Got mission result"); + if (_verbose) warnx("Got mission result: new current: %d", mission_result.index_current_mission); if (mission_result.mission_reached) { mavlink_wpm_send_waypoint_reached((uint16_t)mission_result.mission_index_reached); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 5559d7b562..4d135a0ef0 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1105,7 +1105,7 @@ Navigator::set_mission_item() if (ret == OK) { - _mission.report_current_mission_item(); + _mission.report_current_offboard_mission_item(); /* reset time counter for new item */ _time_first_inside_orbit = 0; diff --git a/src/modules/navigator/navigator_mission.cpp b/src/modules/navigator/navigator_mission.cpp index fdc4ffff69..72dddebfee 100644 --- a/src/modules/navigator/navigator_mission.cpp +++ b/src/modules/navigator/navigator_mission.cpp @@ -89,7 +89,7 @@ Mission::set_current_offboard_mission_index(int new_index) _current_offboard_mission_index = 0; } } - report_current_mission_item(); + report_current_offboard_mission_item(); } void @@ -296,11 +296,9 @@ Mission::report_mission_item_reached() } void -Mission::report_current_mission_item() +Mission::report_current_offboard_mission_item() { - if (_current_mission_type == MISSION_TYPE_OFFBOARD) { - _mission_result.index_current_mission = _current_offboard_mission_index; - } + _mission_result.index_current_mission = _current_offboard_mission_index; } void diff --git a/src/modules/navigator/navigator_mission.h b/src/modules/navigator/navigator_mission.h index 845c16583d..2bd4da82eb 100644 --- a/src/modules/navigator/navigator_mission.h +++ b/src/modules/navigator/navigator_mission.h @@ -73,7 +73,7 @@ public: void move_to_next(); void report_mission_item_reached(); - void report_current_mission_item(); + void report_current_offboard_mission_item(); void publish_mission_result(); private: From d8fdade6aba5e0bd2b56c206d09da4a92fda5fa0 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 24 Feb 2014 23:46:58 +0400 Subject: [PATCH 089/381] mavlink: major rewrite, prepare for dynamic mavlink streams configuration, WIP --- src/modules/mavlink/mavlink_orb_listener.cpp | 1192 +++++++---------- src/modules/mavlink/mavlink_orb_listener.h | 95 +- .../mavlink/mavlink_orb_subscription.cpp | 47 + .../mavlink/mavlink_orb_subscription.h | 31 + src/modules/mavlink/mavlink_stream.cpp | 48 + src/modules/mavlink/mavlink_stream.h | 33 + src/modules/mavlink/module.mk | 4 +- 7 files changed, 718 insertions(+), 732 deletions(-) create mode 100644 src/modules/mavlink/mavlink_orb_subscription.cpp create mode 100644 src/modules/mavlink/mavlink_orb_subscription.h create mode 100644 src/modules/mavlink/mavlink_stream.cpp create mode 100644 src/modules/mavlink/mavlink_stream.h diff --git a/src/modules/mavlink/mavlink_orb_listener.cpp b/src/modules/mavlink/mavlink_orb_listener.cpp index fdc196371b..ea1e3a8bb9 100644 --- a/src/modules/mavlink/mavlink_orb_listener.cpp +++ b/src/modules/mavlink/mavlink_orb_listener.cpp @@ -57,17 +57,14 @@ #include #include - #include #include "mavlink_orb_listener.h" #include "mavlink_main.h" - uint16_t cm_uint16_from_m_float(float m); - uint16_t cm_uint16_from_m_float(float m) { @@ -85,212 +82,195 @@ MavlinkOrbListener::MavlinkOrbListener(Mavlink* parent) : _loop_perf(perf_alloc(PC_ELAPSED, "mavlink orb")), _mavlink(parent), - _listeners(nullptr), - _n_listeners(0) + _subscriptions(nullptr), + _streams(nullptr) { - add_listener(MavlinkOrbListener::l_sensor_combined, &(_mavlink->get_subs()->sensor_sub), 0); - add_listener(MavlinkOrbListener::l_vehicle_attitude, &(_mavlink->get_subs()->att_sub), 0); - add_listener(MavlinkOrbListener::l_vehicle_gps_position, &(_mavlink->get_subs()->gps_sub), 0); - add_listener(MavlinkOrbListener::l_vehicle_status, &(_mavlink->get_subs()->status_sub), 0); - add_listener(MavlinkOrbListener::l_home, &(_mavlink->get_subs()->home_sub), 0); - add_listener(MavlinkOrbListener::l_rc_channels, &(_mavlink->get_subs()->rc_sub), 0); - add_listener(MavlinkOrbListener::l_input_rc, &(_mavlink->get_subs()->input_rc_sub), 0); - add_listener(MavlinkOrbListener::l_global_position, &(_mavlink->get_subs()->global_pos_sub), 0); - add_listener(MavlinkOrbListener::l_local_position, &(_mavlink->get_subs()->local_pos_sub), 0); - add_listener(MavlinkOrbListener::l_global_position_setpoint, &(_mavlink->get_subs()->triplet_sub), 0); - add_listener(MavlinkOrbListener::l_local_position_setpoint, &(_mavlink->get_subs()->spl_sub), 0); - add_listener(MavlinkOrbListener::l_attitude_setpoint, &(_mavlink->get_subs()->spa_sub), 0); - add_listener(MavlinkOrbListener::l_actuator_outputs, &(_mavlink->get_subs()->act_0_sub), 0); - add_listener(MavlinkOrbListener::l_actuator_outputs, &(_mavlink->get_subs()->act_1_sub), 1); - add_listener(MavlinkOrbListener::l_actuator_outputs, &(_mavlink->get_subs()->act_2_sub), 2); - add_listener(MavlinkOrbListener::l_actuator_outputs, &(_mavlink->get_subs()->act_3_sub), 3); - add_listener(MavlinkOrbListener::l_actuator_armed, &(_mavlink->get_subs()->armed_sub), 0); - add_listener(MavlinkOrbListener::l_manual_control_setpoint, &(_mavlink->get_subs()->man_control_sp_sub), 0); - add_listener(MavlinkOrbListener::l_vehicle_attitude_controls, &(_mavlink->get_subs()->actuators_sub), 0); - add_listener(MavlinkOrbListener::l_debug_key_value, &(_mavlink->get_subs()->debug_key_value), 0); - add_listener(MavlinkOrbListener::l_optical_flow, &(_mavlink->get_subs()->optical_flow), 0); - add_listener(MavlinkOrbListener::l_vehicle_rates_setpoint, &(_mavlink->get_subs()->rates_setpoint_sub), 0); - add_listener(MavlinkOrbListener::l_airspeed, &(_mavlink->get_subs()->airspeed_sub), 0); - add_listener(MavlinkOrbListener::l_nav_cap, &(_mavlink->get_subs()->navigation_capabilities_sub), 0); } -void MavlinkOrbListener::add_listener(void (*callback)(const struct listener *l), int *subp, uintptr_t arg) +MavlinkOrbListener::~MavlinkOrbListener() { - struct listener *nl = new listener; - nl->callback = callback; - nl->subp = subp; - nl->arg = arg; - nl->next = nullptr; - nl->mavlink = _mavlink; - nl->listener = this; - - if (_listeners == nullptr) { - _listeners = nl; - } else { - struct listener *next = _listeners; - while (next->next != nullptr) { - next = next->next; - } - next->next = nl; - } - _n_listeners++; } -void -MavlinkOrbListener::l_sensor_combined(const struct listener *l) +MavlinkOrbSubscription *MavlinkOrbListener::add_subscription(const struct orb_metadata *meta, const size_t size, const MavlinkStream *stream, const unsigned int interval) { - struct sensor_combined_s raw; + /* check if already subscribed to this topic */ + MavlinkOrbSubscription *sub; - /* copy sensors raw data into local buffer */ - orb_copy(ORB_ID(sensor_combined), l->mavlink->get_subs()->sensor_sub, &raw); - - /* mark individual fields as _mavlink->get_chan()ged */ - uint16_t fields_updated = 0; - - // if (accel_counter != raw.accelerometer_counter) { - // /* mark first three dimensions as _mavlink->get_chan()ged */ - // fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); - // accel_counter = raw.accelerometer_counter; - // } - - // if (gyro_counter != raw.gyro_counter) { - // /* mark second group dimensions as _mavlink->get_chan()ged */ - // fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); - // gyro_counter = raw.gyro_counter; - // } - - // if (mag_counter != raw.magnetometer_counter) { - // /* mark third group dimensions as _mavlink->get_chan()ged */ - // fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); - // mag_counter = raw.magnetometer_counter; - // } - - // if (baro_counter != raw.baro_counter) { - // /* mark last group dimensions as _mavlink->get_chan()ged */ - // fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); - // baro_counter = raw.baro_counter; - // } - - if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) - mavlink_msg_highres_imu_send(l->mavlink->get_chan(), l->listener->last_sensor_timestamp, - raw.accelerometer_m_s2[0], raw.accelerometer_m_s2[1], - raw.accelerometer_m_s2[2], raw.gyro_rad_s[0], - raw.gyro_rad_s[1], raw.gyro_rad_s[2], - raw.magnetometer_ga[0], - raw.magnetometer_ga[1], raw.magnetometer_ga[2], - raw.baro_pres_mbar, raw.differential_pressure_pa, - raw.baro_alt_meter, raw.baro_temp_celcius, - fields_updated); - - l->listener->sensors_raw_counter++; -} - -void -MavlinkOrbListener::l_vehicle_attitude(const struct listener *l) -{ - /* copy attitude data into local buffer */ - orb_copy(ORB_ID(vehicle_attitude), l->mavlink->get_subs()->att_sub, &l->listener->att); - - if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) { - /* send sensor values */ - mavlink_msg_attitude_send(l->mavlink->get_chan(), - l->listener->last_sensor_timestamp / 1000, - l->listener->att.roll, - l->listener->att.pitch, - l->listener->att.yaw, - l->listener->att.rollspeed, - l->listener->att.pitchspeed, - l->listener->att.yawspeed); - - /* limit VFR message rate to 10Hz */ - hrt_abstime t = hrt_absolute_time(); - if (t >= l->listener->last_sent_vfr + 100000) { - l->listener->last_sent_vfr = t; - float groundspeed = sqrtf(l->listener->global_pos.vel_n * l->listener->global_pos.vel_n + l->listener->global_pos.vel_e * l->listener->global_pos.vel_e); - uint16_t heading = _wrap_2pi(l->listener->att.yaw) * M_RAD_TO_DEG_F; - float throttle = l->listener->armed.armed ? l->listener->actuators_0.control[3] * 100.0f : 0.0f; - mavlink_msg_vfr_hud_send(l->mavlink->get_chan(), l->listener->airspeed.true_airspeed_m_s, groundspeed, heading, throttle, l->listener->global_pos.alt, -l->listener->global_pos.vel_d); - } - - /* send quaternion values if it exists */ - if(l->listener->att.q_valid) { - mavlink_msg_attitude_quaternion_send(l->mavlink->get_chan(), - l->listener->last_sensor_timestamp / 1000, - l->listener->att.q[0], - l->listener->att.q[1], - l->listener->att.q[2], - l->listener->att.q[3], - l->listener->att.rollspeed, - l->listener->att.pitchspeed, - l->listener->att.yawspeed); + LL_FOREACH(_subscriptions, sub) { + if (sub->meta == meta) { + /* already subscribed */ + if (sub->interval > interval) { + /* subscribed with bigger interval, change interval */ + sub->set_interval(interval); + } + return sub; } } - l->listener->attitude_counter++; + /* add new subscription */ + MavlinkOrbSubscription *sub_new = new MavlinkOrbSubscription(meta, size); + + sub_new->set_interval(interval); + + LL_APPEND(_subscriptions, sub_new); + + return sub_new; } -void -MavlinkOrbListener::l_vehicle_gps_position(const struct listener *l) +void MavlinkOrbListener::add_stream(void (*callback)(const MavlinkStream *), const unsigned int subs_n, const struct orb_metadata **metas, const size_t *sizes, const uintptr_t arg, const unsigned int interval) { - struct vehicle_gps_position_s gps; + MavlinkStream *stream = new MavlinkStream(this, callback, subs_n, metas, sizes, arg, interval); - /* copy gps data into local buffer */ - orb_copy(ORB_ID(vehicle_gps_position), l->mavlink->get_subs()->gps_sub, &gps); + stream->mavlink = _mavlink; - /* GPS COG is 0..2PI in degrees * 1e2 */ - float cog_deg = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F; - - /* GPS position */ - mavlink_msg_gps_raw_int_send(l->mavlink->get_chan(), - gps.timestamp_position, - gps.fix_type, - gps.lat, - gps.lon, - gps.alt, - cm_uint16_from_m_float(gps.eph_m), - cm_uint16_from_m_float(gps.epv_m), - gps.vel_m_s * 1e2f, // from m/s to cm/s - cog_deg * 1e2f, // from deg to deg * 100 - gps.satellites_visible); - - /* update SAT info every 10 seconds */ - if (gps.satellite_info_available && (l->listener->gps_counter % 50 == 0)) { - mavlink_msg_gps_status_send(l->mavlink->get_chan(), - gps.satellites_visible, - gps.satellite_prn, - gps.satellite_used, - gps.satellite_elevation, - gps.satellite_azimuth, - gps.satellite_snr); - } - - l->listener->gps_counter++; + LL_APPEND(_streams, stream); } +//void +//MavlinkOrbListener::l_sensor_combined(const struct listener *l) +//{ +// struct sensor_combined_s raw; +// +// /* copy sensors raw data into local buffer */ +// orb_copy(ORB_ID(sensor_combined), l->mavlink->get_subs()->sensor_sub, &raw); +// +// /* mark individual fields as _mavlink->get_chan()ged */ +// uint16_t fields_updated = 0; +// +// // if (accel_counter != raw.accelerometer_counter) { +// // /* mark first three dimensions as _mavlink->get_chan()ged */ +// // fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); +// // accel_counter = raw.accelerometer_counter; +// // } +// +// // if (gyro_counter != raw.gyro_counter) { +// // /* mark second group dimensions as _mavlink->get_chan()ged */ +// // fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); +// // gyro_counter = raw.gyro_counter; +// // } +// +// // if (mag_counter != raw.magnetometer_counter) { +// // /* mark third group dimensions as _mavlink->get_chan()ged */ +// // fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); +// // mag_counter = raw.magnetometer_counter; +// // } +// +// // if (baro_counter != raw.baro_counter) { +// // /* mark last group dimensions as _mavlink->get_chan()ged */ +// // fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); +// // baro_counter = raw.baro_counter; +// // } +// +// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) +// mavlink_msg_highres_imu_send(l->mavlink->get_chan(), l->listener->last_sensor_timestamp, +// raw.accelerometer_m_s2[0], raw.accelerometer_m_s2[1], +// raw.accelerometer_m_s2[2], raw.gyro_rad_s[0], +// raw.gyro_rad_s[1], raw.gyro_rad_s[2], +// raw.magnetometer_ga[0], +// raw.magnetometer_ga[1], raw.magnetometer_ga[2], +// raw.baro_pres_mbar, raw.differential_pressure_pa, +// raw.baro_alt_meter, raw.baro_temp_celcius, +// fields_updated); +// +// l->listener->sensors_raw_counter++; +//} +// +//void +//MavlinkOrbListener::l_vehicle_attitude(const struct listener *l) +//{ +// /* copy attitude data into local buffer */ +// orb_copy(ORB_ID(vehicle_attitude), l->mavlink->get_subs()->att_sub, &l->listener->att); +// +// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) { +// /* send sensor values */ +// mavlink_msg_attitude_send(l->mavlink->get_chan(), +// l->listener->last_sensor_timestamp / 1000, +// l->listener->att.roll, +// l->listener->att.pitch, +// l->listener->att.yaw, +// l->listener->att.rollspeed, +// l->listener->att.pitchspeed, +// l->listener->att.yawspeed); +// +// /* limit VFR message rate to 10Hz */ +// hrt_abstime t = hrt_absolute_time(); +// if (t >= l->listener->last_sent_vfr + 100000) { +// l->listener->last_sent_vfr = t; +// float groundspeed = sqrtf(l->listener->global_pos.vel_n * l->listener->global_pos.vel_n + l->listener->global_pos.vel_e * l->listener->global_pos.vel_e); +// uint16_t heading = _wrap_2pi(l->listener->att.yaw) * M_RAD_TO_DEG_F; +// float throttle = l->listener->armed.armed ? l->listener->actuators_0.control[3] * 100.0f : 0.0f; +// mavlink_msg_vfr_hud_send(l->mavlink->get_chan(), l->listener->airspeed.true_airspeed_m_s, groundspeed, heading, throttle, l->listener->global_pos.alt, -l->listener->global_pos.vel_d); +// } +// +// /* send quaternion values if it exists */ +// if(l->listener->att.q_valid) { +// mavlink_msg_attitude_quaternion_send(l->mavlink->get_chan(), +// l->listener->last_sensor_timestamp / 1000, +// l->listener->att.q[0], +// l->listener->att.q[1], +// l->listener->att.q[2], +// l->listener->att.q[3], +// l->listener->att.rollspeed, +// l->listener->att.pitchspeed, +// l->listener->att.yawspeed); +// } +// } +// +// l->listener->attitude_counter++; +//} +// +//void +//MavlinkOrbListener::l_vehicle_gps_position(const struct listener *l) +//{ +// struct vehicle_gps_position_s gps; +// +// /* copy gps data into local buffer */ +// orb_copy(ORB_ID(vehicle_gps_position), l->mavlink->get_subs()->gps_sub, &gps); +// +// /* GPS COG is 0..2PI in degrees * 1e2 */ +// float cog_deg = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F; +// +// /* GPS position */ +// mavlink_msg_gps_raw_int_send(l->mavlink->get_chan(), +// gps.timestamp_position, +// gps.fix_type, +// gps.lat, +// gps.lon, +// gps.alt, +// cm_uint16_from_m_float(gps.eph_m), +// cm_uint16_from_m_float(gps.epv_m), +// gps.vel_m_s * 1e2f, // from m/s to cm/s +// cog_deg * 1e2f, // from deg to deg * 100 +// gps.satellites_visible); +// +// /* update SAT info every 10 seconds */ +// if (gps.satellite_info_available && (l->listener->gps_counter % 50 == 0)) { +// mavlink_msg_gps_status_send(l->mavlink->get_chan(), +// gps.satellites_visible, +// gps.satellite_prn, +// gps.satellite_used, +// gps.satellite_elevation, +// gps.satellite_azimuth, +// gps.satellite_snr); +// } +// +// l->listener->gps_counter++; +//} +// + void -MavlinkOrbListener::l_vehicle_status(const struct listener *l) +MavlinkOrbListener::msg_heartbeat(const MavlinkStream *stream) { - /* immediately communicate state _mavlink->get_chan()ges back to user */ - orb_copy(ORB_ID(vehicle_status), l->mavlink->get_subs()->status_sub, &(l->mavlink->v_status)); - orb_copy(ORB_ID(actuator_armed), l->mavlink->get_subs()->armed_sub, &l->listener->armed); - orb_copy(ORB_ID(position_setpoint_triplet), l->mavlink->get_subs()->position_setpoint_triplet_sub, &(l->mavlink->pos_sp_triplet)); - - /* enable or disable HIL */ - if (l->listener->v_status.hil_state == HIL_STATE_ON) - l->mavlink->set_hil_enabled(true); - else if (l->listener->v_status.hil_state == HIL_STATE_OFF) - l->mavlink->set_hil_enabled(false); - /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state = 0; uint8_t mavlink_base_mode = 0; uint32_t mavlink_custom_mode = 0; - l->mavlink->get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); + //l->mavlink->get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); /* send heartbeat */ - mavlink_msg_heartbeat_send(l->mavlink->get_chan(), + mavlink_msg_heartbeat_send(stream->mavlink->get_chan(), mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_base_mode, @@ -298,409 +278,378 @@ MavlinkOrbListener::l_vehicle_status(const struct listener *l) mavlink_state); } -void -MavlinkOrbListener::l_rc_channels(const struct listener *l) -{ - /* copy rc channels into local buffer */ - orb_copy(ORB_ID(rc_channels), l->mavlink->get_subs()->rc_sub, &l->listener->rc); - // XXX Add RC channels scaled message here -} - -void -MavlinkOrbListener::l_input_rc(const struct listener *l) -{ - /* copy rc _mavlink->get_chan()nels into local buffer */ - orb_copy(ORB_ID(input_rc), l->mavlink->get_subs()->input_rc_sub, &l->listener->rc_raw); - - if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) { - - const unsigned port_width = 8; - - for (unsigned i = 0; (i * port_width) < l->listener->rc_raw.channel_count; i++) { - /* Channels are sent in MAVLink main loop at a fixed interval */ - mavlink_msg_rc_channels_raw_send(l->mavlink->get_chan(), - l->listener->rc_raw.timestamp_publication / 1000, - i, - (l->listener->rc_raw.channel_count > (i * port_width) + 0) ? l->listener->rc_raw.values[(i * port_width) + 0] : UINT16_MAX, - (l->listener->rc_raw.channel_count > (i * port_width) + 1) ? l->listener->rc_raw.values[(i * port_width) + 1] : UINT16_MAX, - (l->listener->rc_raw.channel_count > (i * port_width) + 2) ? l->listener->rc_raw.values[(i * port_width) + 2] : UINT16_MAX, - (l->listener->rc_raw.channel_count > (i * port_width) + 3) ? l->listener->rc_raw.values[(i * port_width) + 3] : UINT16_MAX, - (l->listener->rc_raw.channel_count > (i * port_width) + 4) ? l->listener->rc_raw.values[(i * port_width) + 4] : UINT16_MAX, - (l->listener->rc_raw.channel_count > (i * port_width) + 5) ? l->listener->rc_raw.values[(i * port_width) + 5] : UINT16_MAX, - (l->listener->rc_raw.channel_count > (i * port_width) + 6) ? l->listener->rc_raw.values[(i * port_width) + 6] : UINT16_MAX, - (l->listener->rc_raw.channel_count > (i * port_width) + 7) ? l->listener->rc_raw.values[(i * port_width) + 7] : UINT16_MAX, - l->listener->rc_raw.rssi); - } - } -} - -void -MavlinkOrbListener::l_global_position(const struct listener *l) -{ - /* copy global position data into local buffer */ - orb_copy(ORB_ID(vehicle_global_position), l->mavlink->get_subs()->global_pos_sub, &l->listener->global_pos); - - mavlink_msg_global_position_int_send(l->mavlink->get_chan(), - l->listener->global_pos.timestamp / 1000, - l->listener->global_pos.lat * 1e7, - l->listener->global_pos.lon * 1e7, - l->listener->global_pos.alt * 1000.0f, - (l->listener->global_pos.alt - l->listener->home.alt) * 1000.0f, - l->listener->global_pos.vel_n * 100.0f, - l->listener->global_pos.vel_e * 100.0f, - l->listener->global_pos.vel_d * 100.0f, - _wrap_2pi(l->listener->global_pos.yaw) * M_RAD_TO_DEG_F * 100.0f); -} - -void -MavlinkOrbListener::l_local_position(const struct listener *l) -{ - /* copy local position data into local buffer */ - orb_copy(ORB_ID(vehicle_local_position), l->mavlink->get_subs()->local_pos_sub, &l->listener->local_pos); - - if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) - mavlink_msg_local_position_ned_send(l->mavlink->get_chan(), - l->listener->local_pos.timestamp / 1000, - l->listener->local_pos.x, - l->listener->local_pos.y, - l->listener->local_pos.z, - l->listener->local_pos.vx, - l->listener->local_pos.vy, - l->listener->local_pos.vz); -} - -void -MavlinkOrbListener::l_global_position_setpoint(const struct listener *l) -{ - struct position_setpoint_triplet_s triplet; - orb_copy(ORB_ID(position_setpoint_triplet), l->mavlink->get_subs()->triplet_sub, &triplet); - - if (!triplet.current.valid) - return; - - if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) - mavlink_msg_global_position_setpoint_int_send(l->mavlink->get_chan(), - MAV_FRAME_GLOBAL, - (int32_t)(triplet.current.lat * 1e7d), - (int32_t)(triplet.current.lon * 1e7d), - (int32_t)(triplet.current.alt * 1e3f), - (int16_t)(triplet.current.yaw * M_RAD_TO_DEG_F * 1e2f)); -} - -void -MavlinkOrbListener::l_local_position_setpoint(const struct listener *l) -{ - struct vehicle_local_position_setpoint_s local_sp; - - /* copy local position data into local buffer */ - orb_copy(ORB_ID(vehicle_local_position_setpoint), l->mavlink->get_subs()->spl_sub, &local_sp); - - if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) - mavlink_msg_local_position_setpoint_send(l->mavlink->get_chan(), - MAV_FRAME_LOCAL_NED, - local_sp.x, - local_sp.y, - local_sp.z, - local_sp.yaw); -} - -void -MavlinkOrbListener::l_attitude_setpoint(const struct listener *l) -{ - struct vehicle_attitude_setpoint_s att_sp; - - /* copy local position data into local buffer */ - orb_copy(ORB_ID(vehicle_attitude_setpoint), l->mavlink->get_subs()->spa_sub, &att_sp); - - if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) - mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(l->mavlink->get_chan(), - att_sp.timestamp / 1000, - att_sp.roll_body, - att_sp.pitch_body, - att_sp.yaw_body, - att_sp.thrust); -} - -void -MavlinkOrbListener::l_vehicle_rates_setpoint(const struct listener *l) -{ - struct vehicle_rates_setpoint_s rates_sp; - - /* copy local position data into local buffer */ - orb_copy(ORB_ID(vehicle_rates_setpoint), l->mavlink->get_subs()->rates_setpoint_sub, &rates_sp); - - if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) - mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(l->mavlink->get_chan(), - rates_sp.timestamp / 1000, - rates_sp.roll, - rates_sp.pitch, - rates_sp.yaw, - rates_sp.thrust); -} - -void -MavlinkOrbListener::l_actuator_outputs(const struct listener *l) -{ - struct actuator_outputs_s act_outputs; - - orb_id_t ids[] = { - ORB_ID(actuator_outputs_0), - ORB_ID(actuator_outputs_1), - ORB_ID(actuator_outputs_2), - ORB_ID(actuator_outputs_3) - }; - - /* copy actuator data into local buffer */ - orb_copy(ids[l->arg], *l->subp, &act_outputs); - - if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) { - mavlink_msg_servo_output_raw_send(l->mavlink->get_chan(), l->listener->last_sensor_timestamp / 1000, - l->arg /* port number - needs GCS support */, - /* QGC has port number support already */ - act_outputs.output[0], - act_outputs.output[1], - act_outputs.output[2], - act_outputs.output[3], - act_outputs.output[4], - act_outputs.output[5], - act_outputs.output[6], - act_outputs.output[7]); - - /* only send in HIL mode and only send first group for HIL */ - if (l->mavlink->get_hil_enabled() && l->listener->armed.armed && ids[l->arg] == ORB_ID(actuator_outputs_0)) { - - /* translate the current syste state to mavlink state and mode */ - uint8_t mavlink_state = 0; - uint8_t mavlink_base_mode = 0; - uint32_t mavlink_custom_mode = 0; - l->mavlink->get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); - - /* HIL message as per MAVLink spec */ - - /* scale / assign outputs depending on system type */ - - if (mavlink_system.type == MAV_TYPE_QUADROTOR) { - mavlink_msg_hil_controls_send(l->mavlink->get_chan(), - hrt_absolute_time(), - ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f, - -1, - -1, - -1, - -1, - mavlink_base_mode, - 0); - - } else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) { - mavlink_msg_hil_controls_send(l->mavlink->get_chan(), - hrt_absolute_time(), - ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f, - -1, - -1, - mavlink_base_mode, - 0); - - } else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) { - mavlink_msg_hil_controls_send(l->mavlink->get_chan(), - hrt_absolute_time(), - ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[6] - 900.0f) / 600.0f) / 2.0f, - ((act_outputs.output[7] - 900.0f) / 600.0f) / 2.0f, - mavlink_base_mode, - 0); - - } else { - mavlink_msg_hil_controls_send(l->mavlink->get_chan(), - hrt_absolute_time(), - (act_outputs.output[0] - 1500.0f) / 500.0f, - (act_outputs.output[1] - 1500.0f) / 500.0f, - (act_outputs.output[2] - 1500.0f) / 500.0f, - (act_outputs.output[3] - 1000.0f) / 1000.0f, - (act_outputs.output[4] - 1500.0f) / 500.0f, - (act_outputs.output[5] - 1500.0f) / 500.0f, - (act_outputs.output[6] - 1500.0f) / 500.0f, - (act_outputs.output[7] - 1500.0f) / 500.0f, - mavlink_base_mode, - 0); - } - } - } -} - -void -MavlinkOrbListener::l_actuator_armed(const struct listener *l) -{ - orb_copy(ORB_ID(actuator_armed), l->mavlink->get_subs()->armed_sub, &l->listener->armed); -} - -void -MavlinkOrbListener::l_manual_control_setpoint(const struct listener *l) -{ - struct manual_control_setpoint_s man_control; - - /* copy manual control data into local buffer */ - orb_copy(ORB_ID(manual_control_setpoint), l->mavlink->get_subs()->man_control_sp_sub, &man_control); - - if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) - mavlink_msg_manual_control_send(l->mavlink->get_chan(), - mavlink_system.sysid, - man_control.roll * 1000, - man_control.pitch * 1000, - man_control.yaw * 1000, - man_control.throttle * 1000, - 0); -} - -void -MavlinkOrbListener::l_vehicle_attitude_controls(const struct listener *l) -{ - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, l->mavlink->get_subs()->actuators_sub, &l->listener->actuators_0); - - if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) { - /* send, add spaces so that string buffer is at least 10 chars long */ - mavlink_msg_named_value_float_send(l->mavlink->get_chan(), - l->listener->last_sensor_timestamp / 1000, - "ctrl0 ", - l->listener->actuators_0.control[0]); - mavlink_msg_named_value_float_send(l->mavlink->get_chan(), - l->listener->last_sensor_timestamp / 1000, - "ctrl1 ", - l->listener->actuators_0.control[1]); - mavlink_msg_named_value_float_send(l->mavlink->get_chan(), - l->listener->last_sensor_timestamp / 1000, - "ctrl2 ", - l->listener->actuators_0.control[2]); - mavlink_msg_named_value_float_send(l->mavlink->get_chan(), - l->listener->last_sensor_timestamp / 1000, - "ctrl3 ", - l->listener->actuators_0.control[3]); - } -} - -void -MavlinkOrbListener::l_debug_key_value(const struct listener *l) -{ - struct debug_key_value_s debug; - - orb_copy(ORB_ID(debug_key_value), l->mavlink->get_subs()->debug_key_value, &debug); - - /* Enforce null termination */ - debug.key[sizeof(debug.key) - 1] = '\0'; - - mavlink_msg_named_value_float_send(l->mavlink->get_chan(), - l->listener->last_sensor_timestamp / 1000, - debug.key, - debug.value); -} - -void -MavlinkOrbListener::l_optical_flow(const struct listener *l) -{ - struct optical_flow_s flow; - - orb_copy(ORB_ID(optical_flow), l->mavlink->get_subs()->optical_flow, &flow); - - mavlink_msg_optical_flow_send(l->mavlink->get_chan(), flow.timestamp, flow.sensor_id, flow.flow_raw_x, flow.flow_raw_y, - flow.flow_comp_x_m, flow.flow_comp_y_m, flow.quality, flow.ground_distance_m); -} - -void -MavlinkOrbListener::l_home(const struct listener *l) -{ - orb_copy(ORB_ID(home_position), l->mavlink->get_subs()->home_sub, &l->listener->home); - - mavlink_msg_gps_global_origin_send(l->mavlink->get_chan(), (int32_t)(l->listener->home.lat*1e7d), (int32_t)(l->listener->home.lon*1e7d), (int32_t)(l->listener->home.alt)*1e3f); -} - -void -MavlinkOrbListener::l_airspeed(const struct listener *l) -{ - orb_copy(ORB_ID(airspeed), l->mavlink->get_subs()->airspeed_sub, &l->listener->airspeed); -} - -void -MavlinkOrbListener::l_nav_cap(const struct listener *l) -{ - - orb_copy(ORB_ID(navigation_capabilities), l->mavlink->get_subs()->navigation_capabilities_sub, &l->listener->nav_cap); - - mavlink_msg_named_value_float_send(l->mavlink->get_chan(), - hrt_absolute_time() / 1000, - "turn dist", - l->listener->nav_cap.turn_distance); - -} +// +//void +//MavlinkOrbListener::l_rc_channels(const struct listener *l) +//{ +// /* copy rc channels into local buffer */ +// orb_copy(ORB_ID(rc_channels), l->mavlink->get_subs()->rc_sub, &l->listener->rc); +// // XXX Add RC channels scaled message here +//} +// +//void +//MavlinkOrbListener::l_input_rc(const struct listener *l) +//{ +// /* copy rc _mavlink->get_chan()nels into local buffer */ +// orb_copy(ORB_ID(input_rc), l->mavlink->get_subs()->input_rc_sub, &l->listener->rc_raw); +// +// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) { +// +// const unsigned port_width = 8; +// +// for (unsigned i = 0; (i * port_width) < l->listener->rc_raw.channel_count; i++) { +// /* Channels are sent in MAVLink main loop at a fixed interval */ +// mavlink_msg_rc_channels_raw_send(l->mavlink->get_chan(), +// l->listener->rc_raw.timestamp_publication / 1000, +// i, +// (l->listener->rc_raw.channel_count > (i * port_width) + 0) ? l->listener->rc_raw.values[(i * port_width) + 0] : UINT16_MAX, +// (l->listener->rc_raw.channel_count > (i * port_width) + 1) ? l->listener->rc_raw.values[(i * port_width) + 1] : UINT16_MAX, +// (l->listener->rc_raw.channel_count > (i * port_width) + 2) ? l->listener->rc_raw.values[(i * port_width) + 2] : UINT16_MAX, +// (l->listener->rc_raw.channel_count > (i * port_width) + 3) ? l->listener->rc_raw.values[(i * port_width) + 3] : UINT16_MAX, +// (l->listener->rc_raw.channel_count > (i * port_width) + 4) ? l->listener->rc_raw.values[(i * port_width) + 4] : UINT16_MAX, +// (l->listener->rc_raw.channel_count > (i * port_width) + 5) ? l->listener->rc_raw.values[(i * port_width) + 5] : UINT16_MAX, +// (l->listener->rc_raw.channel_count > (i * port_width) + 6) ? l->listener->rc_raw.values[(i * port_width) + 6] : UINT16_MAX, +// (l->listener->rc_raw.channel_count > (i * port_width) + 7) ? l->listener->rc_raw.values[(i * port_width) + 7] : UINT16_MAX, +// l->listener->rc_raw.rssi); +// } +// } +//} +// +//void +//MavlinkOrbListener::l_global_position(const struct listener *l) +//{ +// /* copy global position data into local buffer */ +// orb_copy(ORB_ID(vehicle_global_position), l->mavlink->get_subs()->global_pos_sub, &l->listener->global_pos); +// +// mavlink_msg_global_position_int_send(l->mavlink->get_chan(), +// l->listener->global_pos.timestamp / 1000, +// l->listener->global_pos.lat * 1e7, +// l->listener->global_pos.lon * 1e7, +// l->listener->global_pos.alt * 1000.0f, +// (l->listener->global_pos.alt - l->listener->home.alt) * 1000.0f, +// l->listener->global_pos.vel_n * 100.0f, +// l->listener->global_pos.vel_e * 100.0f, +// l->listener->global_pos.vel_d * 100.0f, +// _wrap_2pi(l->listener->global_pos.yaw) * M_RAD_TO_DEG_F * 100.0f); +//} +// +//void +//MavlinkOrbListener::l_local_position(const struct listener *l) +//{ +// /* copy local position data into local buffer */ +// orb_copy(ORB_ID(vehicle_local_position), l->mavlink->get_subs()->local_pos_sub, &l->listener->local_pos); +// +// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) +// mavlink_msg_local_position_ned_send(l->mavlink->get_chan(), +// l->listener->local_pos.timestamp / 1000, +// l->listener->local_pos.x, +// l->listener->local_pos.y, +// l->listener->local_pos.z, +// l->listener->local_pos.vx, +// l->listener->local_pos.vy, +// l->listener->local_pos.vz); +//} +// +//void +//MavlinkOrbListener::l_global_position_setpoint(const struct listener *l) +//{ +// struct position_setpoint_triplet_s triplet; +// orb_copy(ORB_ID(position_setpoint_triplet), l->mavlink->get_subs()->triplet_sub, &triplet); +// +// if (!triplet.current.valid) +// return; +// +// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) +// mavlink_msg_global_position_setpoint_int_send(l->mavlink->get_chan(), +// MAV_FRAME_GLOBAL, +// (int32_t)(triplet.current.lat * 1e7d), +// (int32_t)(triplet.current.lon * 1e7d), +// (int32_t)(triplet.current.alt * 1e3f), +// (int16_t)(triplet.current.yaw * M_RAD_TO_DEG_F * 1e2f)); +//} +// +//void +//MavlinkOrbListener::l_local_position_setpoint(const struct listener *l) +//{ +// struct vehicle_local_position_setpoint_s local_sp; +// +// /* copy local position data into local buffer */ +// orb_copy(ORB_ID(vehicle_local_position_setpoint), l->mavlink->get_subs()->spl_sub, &local_sp); +// +// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) +// mavlink_msg_local_position_setpoint_send(l->mavlink->get_chan(), +// MAV_FRAME_LOCAL_NED, +// local_sp.x, +// local_sp.y, +// local_sp.z, +// local_sp.yaw); +//} +// +//void +//MavlinkOrbListener::l_attitude_setpoint(const struct listener *l) +//{ +// struct vehicle_attitude_setpoint_s att_sp; +// +// /* copy local position data into local buffer */ +// orb_copy(ORB_ID(vehicle_attitude_setpoint), l->mavlink->get_subs()->spa_sub, &att_sp); +// +// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) +// mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(l->mavlink->get_chan(), +// att_sp.timestamp / 1000, +// att_sp.roll_body, +// att_sp.pitch_body, +// att_sp.yaw_body, +// att_sp.thrust); +//} +// +//void +//MavlinkOrbListener::l_vehicle_rates_setpoint(const struct listener *l) +//{ +// struct vehicle_rates_setpoint_s rates_sp; +// +// /* copy local position data into local buffer */ +// orb_copy(ORB_ID(vehicle_rates_setpoint), l->mavlink->get_subs()->rates_setpoint_sub, &rates_sp); +// +// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) +// mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(l->mavlink->get_chan(), +// rates_sp.timestamp / 1000, +// rates_sp.roll, +// rates_sp.pitch, +// rates_sp.yaw, +// rates_sp.thrust); +//} +// +//void +//MavlinkOrbListener::l_actuator_outputs(const struct listener *l) +//{ +// struct actuator_outputs_s act_outputs; +// +// orb_id_t ids[] = { +// ORB_ID(actuator_outputs_0), +// ORB_ID(actuator_outputs_1), +// ORB_ID(actuator_outputs_2), +// ORB_ID(actuator_outputs_3) +// }; +// +// /* copy actuator data into local buffer */ +// orb_copy(ids[l->arg], *l->subp, &act_outputs); +// +// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) { +// mavlink_msg_servo_output_raw_send(l->mavlink->get_chan(), l->listener->last_sensor_timestamp / 1000, +// l->arg /* port number - needs GCS support */, +// /* QGC has port number support already */ +// act_outputs.output[0], +// act_outputs.output[1], +// act_outputs.output[2], +// act_outputs.output[3], +// act_outputs.output[4], +// act_outputs.output[5], +// act_outputs.output[6], +// act_outputs.output[7]); +// +// /* only send in HIL mode and only send first group for HIL */ +// if (l->mavlink->get_hil_enabled() && l->listener->armed.armed && ids[l->arg] == ORB_ID(actuator_outputs_0)) { +// +// /* translate the current syste state to mavlink state and mode */ +// uint8_t mavlink_state = 0; +// uint8_t mavlink_base_mode = 0; +// uint32_t mavlink_custom_mode = 0; +// l->mavlink->get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); +// +// /* HIL message as per MAVLink spec */ +// +// /* scale / assign outputs depending on system type */ +// +// if (mavlink_system.type == MAV_TYPE_QUADROTOR) { +// mavlink_msg_hil_controls_send(l->mavlink->get_chan(), +// hrt_absolute_time(), +// ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, +// ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, +// ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f, +// ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f, +// -1, +// -1, +// -1, +// -1, +// mavlink_base_mode, +// 0); +// +// } else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) { +// mavlink_msg_hil_controls_send(l->mavlink->get_chan(), +// hrt_absolute_time(), +// ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, +// ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, +// ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f, +// ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f, +// ((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f, +// ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f, +// -1, +// -1, +// mavlink_base_mode, +// 0); +// +// } else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) { +// mavlink_msg_hil_controls_send(l->mavlink->get_chan(), +// hrt_absolute_time(), +// ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, +// ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, +// ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f, +// ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f, +// ((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f, +// ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f, +// ((act_outputs.output[6] - 900.0f) / 600.0f) / 2.0f, +// ((act_outputs.output[7] - 900.0f) / 600.0f) / 2.0f, +// mavlink_base_mode, +// 0); +// +// } else { +// mavlink_msg_hil_controls_send(l->mavlink->get_chan(), +// hrt_absolute_time(), +// (act_outputs.output[0] - 1500.0f) / 500.0f, +// (act_outputs.output[1] - 1500.0f) / 500.0f, +// (act_outputs.output[2] - 1500.0f) / 500.0f, +// (act_outputs.output[3] - 1000.0f) / 1000.0f, +// (act_outputs.output[4] - 1500.0f) / 500.0f, +// (act_outputs.output[5] - 1500.0f) / 500.0f, +// (act_outputs.output[6] - 1500.0f) / 500.0f, +// (act_outputs.output[7] - 1500.0f) / 500.0f, +// mavlink_base_mode, +// 0); +// } +// } +// } +//} +// +//void +//MavlinkOrbListener::l_actuator_armed(const struct listener *l) +//{ +// orb_copy(ORB_ID(actuator_armed), l->mavlink->get_subs()->armed_sub, &l->listener->armed); +//} +// +//void +//MavlinkOrbListener::l_manual_control_setpoint(const struct listener *l) +//{ +// struct manual_control_setpoint_s man_control; +// +// /* copy manual control data into local buffer */ +// orb_copy(ORB_ID(manual_control_setpoint), l->mavlink->get_subs()->man_control_sp_sub, &man_control); +// +// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) +// mavlink_msg_manual_control_send(l->mavlink->get_chan(), +// mavlink_system.sysid, +// man_control.roll * 1000, +// man_control.pitch * 1000, +// man_control.yaw * 1000, +// man_control.throttle * 1000, +// 0); +//} +// +//void +//MavlinkOrbListener::l_vehicle_attitude_controls(const struct listener *l) +//{ +// orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, l->mavlink->get_subs()->actuators_sub, &l->listener->actuators_0); +// +// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) { +// /* send, add spaces so that string buffer is at least 10 chars long */ +// mavlink_msg_named_value_float_send(l->mavlink->get_chan(), +// l->listener->last_sensor_timestamp / 1000, +// "ctrl0 ", +// l->listener->actuators_0.control[0]); +// mavlink_msg_named_value_float_send(l->mavlink->get_chan(), +// l->listener->last_sensor_timestamp / 1000, +// "ctrl1 ", +// l->listener->actuators_0.control[1]); +// mavlink_msg_named_value_float_send(l->mavlink->get_chan(), +// l->listener->last_sensor_timestamp / 1000, +// "ctrl2 ", +// l->listener->actuators_0.control[2]); +// mavlink_msg_named_value_float_send(l->mavlink->get_chan(), +// l->listener->last_sensor_timestamp / 1000, +// "ctrl3 ", +// l->listener->actuators_0.control[3]); +// } +//} +// +//void +//MavlinkOrbListener::l_debug_key_value(const struct listener *l) +//{ +// struct debug_key_value_s debug; +// +// orb_copy(ORB_ID(debug_key_value), l->mavlink->get_subs()->debug_key_value, &debug); +// +// /* Enforce null termination */ +// debug.key[sizeof(debug.key) - 1] = '\0'; +// +// mavlink_msg_named_value_float_send(l->mavlink->get_chan(), +// l->listener->last_sensor_timestamp / 1000, +// debug.key, +// debug.value); +//} +// +//void +//MavlinkOrbListener::l_optical_flow(const struct listener *l) +//{ +// struct optical_flow_s flow; +// +// orb_copy(ORB_ID(optical_flow), l->mavlink->get_subs()->optical_flow, &flow); +// +// mavlink_msg_optical_flow_send(l->mavlink->get_chan(), flow.timestamp, flow.sensor_id, flow.flow_raw_x, flow.flow_raw_y, +// flow.flow_comp_x_m, flow.flow_comp_y_m, flow.quality, flow.ground_distance_m); +//} +// +//void +//MavlinkOrbListener::l_home(const struct listener *l) +//{ +// orb_copy(ORB_ID(home_position), l->mavlink->get_subs()->home_sub, &l->listener->home); +// +// mavlink_msg_gps_global_origin_send(l->mavlink->get_chan(), (int32_t)(l->listener->home.lat*1e7d), (int32_t)(l->listener->home.lon*1e7d), (int32_t)(l->listener->home.alt)*1e3f); +//} +// +//void +//MavlinkOrbListener::l_airspeed(const struct listener *l) +//{ +// orb_copy(ORB_ID(airspeed), l->mavlink->get_subs()->airspeed_sub, &l->listener->airspeed); +//} +// +//void +//MavlinkOrbListener::l_nav_cap(const struct listener *l) +//{ +// +// orb_copy(ORB_ID(navigation_capabilities), l->mavlink->get_subs()->navigation_capabilities_sub, &l->listener->nav_cap); +// +// mavlink_msg_named_value_float_send(l->mavlink->get_chan(), +// hrt_absolute_time() / 1000, +// "turn dist", +// l->listener->nav_cap.turn_distance); +// +//} void * MavlinkOrbListener::uorb_receive_thread(void *arg) { - /* Set thread name */ + /* set thread name */ char thread_name[18]; sprintf(thread_name, "mavlink_uorb_rcv_%d", _mavlink->get_channel()); prctl(PR_SET_NAME, thread_name, getpid()); - /* - * set up poll to block for new data, - * wait for a maximum of 1000 ms (1 second) - */ - const int timeout = 1000; + /* add mavlink streams */ + /* common buffer for topics and data sizes */ + const struct orb_metadata *topics[1]; + size_t sizes[1]; - /* - * Initialise listener array. - * - * Might want to invoke each listener once to set initial state. - */ - struct pollfd fds[_max_listeners]; - - struct listener* next = _listeners; - unsigned i = 0; - while (next != nullptr) { - - fds[i].fd = *next->subp; - fds[i].events = POLLIN; - - next = next->next; - i++; - } - /* Invoke callback to set initial state */ - //listeners[i].callback(&listener[i]); + /* --- HEARTBEAT --- */ + topics[0] = ORB_ID(vehicle_status); + sizes[0] = sizeof(vehicle_status_s); + add_stream(msg_heartbeat, 1, topics, sizes, 0, 500); while (!_mavlink->_task_should_exit) { - - int poll_ret = poll(fds, _n_listeners, timeout); - - /* handle the poll result */ - if (poll_ret == 0) { - /* silent */ - - } else if (poll_ret < 0) { - //mavlink_missionlib_send_gcs_string("[mavlink] ERROR reading uORB data"); - - } else { - - i = 0; - struct listener* cb = _listeners; - while (cb != nullptr) { - - if (fds[i].revents & POLLIN) - cb->callback(cb); - - cb = cb->next; - i++; - } + /* check all streams each 1ms */ + hrt_abstime t = hrt_absolute_time(); + MavlinkStream *stream; + LL_FOREACH(_streams, stream) { + stream->update(t); } + usleep(1000); } return NULL; @@ -715,105 +664,6 @@ void * MavlinkOrbListener::uorb_start_helper(void *context) pthread_t MavlinkOrbListener::uorb_receive_start(Mavlink* mavlink) { - - /* --- SENSORS RAW VALUE --- */ - mavlink->get_subs()->sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); - /* rate limit set externally based on interface speed, set a basic default here */ - orb_set_interval(mavlink->get_subs()->sensor_sub, 200); /* 5Hz updates */ - - /* --- ATTITUDE VALUE --- */ - mavlink->get_subs()->att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - /* rate limit set externally based on interface speed, set a basic default here */ - orb_set_interval(mavlink->get_subs()->att_sub, 200); /* 5Hz updates */ - - /* --- GPS VALUE --- */ - mavlink->get_subs()->gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); - orb_set_interval(mavlink->get_subs()->gps_sub, 200); /* 5Hz updates */ - - /* --- HOME POSITION --- */ - mavlink->get_subs()->home_sub = orb_subscribe(ORB_ID(home_position)); - orb_set_interval(mavlink->get_subs()->home_sub, 1000); /* 1Hz updates */ - - /* --- SYSTEM STATE --- */ - mavlink->get_subs()->status_sub = orb_subscribe(ORB_ID(vehicle_status)); - orb_set_interval(mavlink->get_subs()->status_sub, 300); /* max 3.33 Hz updates */ - - /* --- POSITION SETPOINT TRIPLET --- */ - mavlink->get_subs()->position_setpoint_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); - orb_set_interval(mavlink->get_subs()->position_setpoint_triplet_sub, 0); /* not polled, don't limit */ - - /* --- RC CHANNELS VALUE --- */ - mavlink->get_subs()->rc_sub = orb_subscribe(ORB_ID(rc_channels)); - orb_set_interval(mavlink->get_subs()->rc_sub, 100); /* 10Hz updates */ - - /* --- RC RAW VALUE --- */ - mavlink->get_subs()->input_rc_sub = orb_subscribe(ORB_ID(input_rc)); - orb_set_interval(mavlink->get_subs()->input_rc_sub, 100); - - /* --- GLOBAL POS VALUE --- */ - mavlink->get_subs()->global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - orb_set_interval(mavlink->get_subs()->global_pos_sub, 100); /* 10 Hz active updates */ - - /* --- LOCAL POS VALUE --- */ - mavlink->get_subs()->local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); - orb_set_interval(mavlink->get_subs()->local_pos_sub, 1000); /* 1Hz active updates */ - - /* --- GLOBAL SETPOINT VALUE --- */ - mavlink->get_subs()->triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); - orb_set_interval(mavlink->get_subs()->triplet_sub, 2000); /* 0.5 Hz updates */ - - /* --- LOCAL SETPOINT VALUE --- */ - mavlink->get_subs()->spl_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); - orb_set_interval(mavlink->get_subs()->spl_sub, 2000); /* 0.5 Hz updates */ - - /* --- ATTITUDE SETPOINT VALUE --- */ - mavlink->get_subs()->spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - orb_set_interval(mavlink->get_subs()->spa_sub, 2000); /* 0.5 Hz updates */ - - /* --- RATES SETPOINT VALUE --- */ - mavlink->get_subs()->rates_setpoint_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); - orb_set_interval(mavlink->get_subs()->rates_setpoint_sub, 2000); /* 0.5 Hz updates */ - - /* --- ACTUATOR OUTPUTS --- */ - mavlink->get_subs()->act_0_sub = orb_subscribe(ORB_ID(actuator_outputs_0)); - mavlink->get_subs()->act_1_sub = orb_subscribe(ORB_ID(actuator_outputs_1)); - mavlink->get_subs()->act_2_sub = orb_subscribe(ORB_ID(actuator_outputs_2)); - mavlink->get_subs()->act_3_sub = orb_subscribe(ORB_ID(actuator_outputs_3)); - /* rate limits set externally based on interface speed, set a basic default here */ - orb_set_interval(mavlink->get_subs()->act_0_sub, 100); /* 10Hz updates */ - orb_set_interval(mavlink->get_subs()->act_1_sub, 100); /* 10Hz updates */ - orb_set_interval(mavlink->get_subs()->act_2_sub, 100); /* 10Hz updates */ - orb_set_interval(mavlink->get_subs()->act_3_sub, 100); /* 10Hz updates */ - - /* --- ACTUATOR ARMED VALUE --- */ - mavlink->get_subs()->armed_sub = orb_subscribe(ORB_ID(actuator_armed)); - orb_set_interval(mavlink->get_subs()->armed_sub, 100); /* 10Hz updates */ - - /* --- MAPPED MANUAL CONTROL INPUTS --- */ - mavlink->get_subs()->man_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - /* rate limits set externally based on interface speed, set a basic default here */ - orb_set_interval(mavlink->get_subs()->man_control_sp_sub, 100); /* 10Hz updates */ - - /* --- ACTUATOR CONTROL VALUE --- */ - mavlink->get_subs()->actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); - orb_set_interval(mavlink->get_subs()->actuators_sub, 100); /* 10Hz updates */ - - /* --- DEBUG VALUE OUTPUT --- */ - mavlink->get_subs()->debug_key_value = orb_subscribe(ORB_ID(debug_key_value)); - orb_set_interval(mavlink->get_subs()->debug_key_value, 100); /* 10Hz updates */ - - /* --- FLOW SENSOR --- */ - mavlink->get_subs()->optical_flow = orb_subscribe(ORB_ID(optical_flow)); - orb_set_interval(mavlink->get_subs()->optical_flow, 200); /* 5Hz updates */ - - /* --- AIRSPEED --- */ - mavlink->get_subs()->airspeed_sub = orb_subscribe(ORB_ID(airspeed)); - orb_set_interval(mavlink->get_subs()->airspeed_sub, 200); /* 5Hz updates */ - - /* --- NAVIGATION CAPABILITIES --- */ - mavlink->get_subs()->navigation_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); - orb_set_interval(mavlink->get_subs()->navigation_capabilities_sub, 500); /* 2Hz updates */ - /* start the listener loop */ pthread_attr_t uorb_attr; pthread_attr_init(&uorb_attr); diff --git a/src/modules/mavlink/mavlink_orb_listener.h b/src/modules/mavlink/mavlink_orb_listener.h index 26a2e832f7..1a103e43d3 100644 --- a/src/modules/mavlink/mavlink_orb_listener.h +++ b/src/modules/mavlink/mavlink_orb_listener.h @@ -40,9 +40,12 @@ */ #include +#include #pragma once +#include + #include #include #include @@ -68,11 +71,14 @@ #include #include #include -#include #include #include +#include "mavlink_orb_subscription.h" +#include "mavlink_stream.h" + class Mavlink; +class MavlinkStream; class MavlinkOrbListener { @@ -83,22 +89,14 @@ public: MavlinkOrbListener(Mavlink* parent); /** - * Destructor, also kills the mavlinks task. + * Destructor, closes all subscriptions. */ ~MavlinkOrbListener(); static pthread_t uorb_receive_start(Mavlink *mavlink); - struct listener { - void (*callback)(const struct listener *l); - int *subp; - uintptr_t arg; - struct listener *next; - Mavlink *mavlink; - MavlinkOrbListener* listener; - }; - - void add_listener(void (*callback)(const struct listener *l), int *subp, uintptr_t arg); + MavlinkOrbSubscription *add_subscription(const struct orb_metadata *meta, size_t size, const MavlinkStream *stream, const unsigned int interval); + void add_stream(void (*callback)(const MavlinkStream *), const unsigned int subs_n, const struct orb_metadata **metas, const size_t *sizes, const uintptr_t arg, const unsigned int interval); static void * uorb_start_helper(void *context); private: @@ -107,57 +105,34 @@ private: Mavlink* _mavlink; - struct listener *_listeners; - unsigned _n_listeners; - static const unsigned _max_listeners = 32; + MavlinkOrbSubscription *_subscriptions; + static const unsigned _max_subscriptions = 32; + MavlinkStream *_streams; void *uorb_receive_thread(void *arg); - static void l_sensor_combined(const struct listener *l); - static void l_vehicle_attitude(const struct listener *l); - static void l_vehicle_gps_position(const struct listener *l); - static void l_vehicle_status(const struct listener *l); - static void l_rc_channels(const struct listener *l); - static void l_input_rc(const struct listener *l); - static void l_global_position(const struct listener *l); - static void l_local_position(const struct listener *l); - static void l_global_position_setpoint(const struct listener *l); - static void l_local_position_setpoint(const struct listener *l); - static void l_attitude_setpoint(const struct listener *l); - static void l_actuator_outputs(const struct listener *l); - static void l_actuator_armed(const struct listener *l); - static void l_manual_control_setpoint(const struct listener *l); - static void l_vehicle_attitude_controls(const struct listener *l); - static void l_debug_key_value(const struct listener *l); - static void l_optical_flow(const struct listener *l); - static void l_vehicle_rates_setpoint(const struct listener *l); - static void l_home(const struct listener *l); - static void l_airspeed(const struct listener *l); - static void l_nav_cap(const struct listener *l); + static void msg_heartbeat(const MavlinkStream *stream); - struct vehicle_global_position_s global_pos; - struct vehicle_local_position_s local_pos; - struct navigation_capabilities_s nav_cap; - struct vehicle_status_s v_status; - struct rc_channels_s rc; - struct rc_input_values rc_raw; - struct actuator_armed_s armed; - struct actuator_controls_s actuators_0; - struct vehicle_attitude_s att; - struct airspeed_s airspeed; - struct home_position_s home; - - unsigned int sensors_raw_counter; - unsigned int attitude_counter; - unsigned int gps_counter; - - /* - * Last sensor loop time - * some outputs are better timestamped - * with this "global" reference. - */ - uint64_t last_sensor_timestamp; - - hrt_abstime last_sent_vfr; +// static void l_sensor_combined(const struct listener *l); +// static void l_vehicle_attitude(const struct listener *l); +// static void l_vehicle_gps_position(const struct listener *l); +// static void l_vehicle_status(const struct listener *l); +// static void l_rc_channels(const struct listener *l); +// static void l_input_rc(const struct listener *l); +// static void l_global_position(const struct listener *l); +// static void l_local_position(const struct listener *l); +// static void l_global_position_setpoint(const struct listener *l); +// static void l_local_position_setpoint(const struct listener *l); +// static void l_attitude_setpoint(const struct listener *l); +// static void l_actuator_outputs(const struct listener *l); +// static void l_actuator_armed(const struct listener *l); +// static void l_manual_control_setpoint(const struct listener *l); +// static void l_vehicle_attitude_controls(const struct listener *l); +// static void l_debug_key_value(const struct listener *l); +// static void l_optical_flow(const struct listener *l); +// static void l_vehicle_rates_setpoint(const struct listener *l); +// static void l_home(const struct listener *l); +// static void l_airspeed(const struct listener *l); +// static void l_nav_cap(const struct listener *l); }; diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp new file mode 100644 index 0000000000..182407a5eb --- /dev/null +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -0,0 +1,47 @@ +/* + * mavlink_orb_subscription.cpp + * + * Created on: 23.02.2014 + * Author: ton + */ + + +#include +#include +#include +#include + +#include "mavlink_orb_subscription.h" + +MavlinkOrbSubscription::MavlinkOrbSubscription(const struct orb_metadata *meta, size_t size) +{ + this->meta = meta; + this->data = malloc(size); + memset(this->data, 0, size); + this->fd = orb_subscribe(meta); + this->last_update = 0; + this->interval = 0; +} + +MavlinkOrbSubscription::~MavlinkOrbSubscription() +{ + close(fd); + free(data); +} + +int MavlinkOrbSubscription::set_interval(const unsigned int interval) +{ + this->interval = interval; + return orb_set_interval(fd, interval); +} + +int MavlinkOrbSubscription::update(const hrt_abstime t) +{ + if (last_update != t) { + bool updated; + orb_check(fd, &updated); + if (updated) + return orb_copy(meta, fd, &data); + } + return OK; +} diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h new file mode 100644 index 0000000000..2c72abaeff --- /dev/null +++ b/src/modules/mavlink/mavlink_orb_subscription.h @@ -0,0 +1,31 @@ +/* + * mavlink_orb_subscription.h + * + * Created on: 23.02.2014 + * Author: ton + */ + +#ifndef MAVLINK_ORB_SUBSCRIPTION_H_ +#define MAVLINK_ORB_SUBSCRIPTION_H_ + +#include +#include + +class MavlinkOrbSubscription { +public: + MavlinkOrbSubscription(const struct orb_metadata *meta, size_t size); + ~MavlinkOrbSubscription(); + + int set_interval(const unsigned int interval); + int update(const hrt_abstime t); + + const struct orb_metadata *meta; + int fd; + void *data; + hrt_abstime last_update; + unsigned int interval; + MavlinkOrbSubscription *next; +}; + + +#endif /* MAVLINK_ORB_SUBSCRIPTION_H_ */ diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp new file mode 100644 index 0000000000..2a6728fdb2 --- /dev/null +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -0,0 +1,48 @@ +/* + * mavlink_stream.cpp + * + * Created on: 24.02.2014 + * Author: ton + */ + +#include + +#include "mavlink_orb_listener.h" + +MavlinkStream::MavlinkStream(MavlinkOrbListener *listener, void (*callback)(const MavlinkStream *), const unsigned int subs_n, const struct orb_metadata **metas, const size_t *sizes, const uintptr_t arg, const unsigned int interval) +{ + this->callback = callback; + this->arg = arg; + this->interval = interval * 1000; + this->mavlink = mavlink; + this->listener = listener; + this->subscriptions_n = subs_n; + this->subscriptions = (MavlinkOrbSubscription **) malloc(subs_n * sizeof(MavlinkOrbSubscription *)); + + for (int i = 0; i < subs_n; i++) { + this->subscriptions[i] = listener->add_subscription(metas[i], sizes[i], this, interval); + } +} + +MavlinkStream::~MavlinkStream() +{ + free(subscriptions); +} + +/** + * Update mavlink stream, i.e. update subscriptions and send message if necessary + */ +int MavlinkStream::update(const hrt_abstime t) +{ + uint64_t dt = t - last_sent; + if (dt > 0 && dt >= interval) { + /* interval expired, update all subscriptions */ + for (unsigned int i = 0; i < subscriptions_n; i++) { + subscriptions[i]->update(t); + } + + /* format and send mavlink message */ + callback(this); + last_sent = t; + } +} diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h new file mode 100644 index 0000000000..0f959d7206 --- /dev/null +++ b/src/modules/mavlink/mavlink_stream.h @@ -0,0 +1,33 @@ +/* + * mavlink_stream.h + * + * Created on: 24.02.2014 + * Author: ton + */ + +#ifndef MAVLINK_STREAM_H_ +#define MAVLINK_STREAM_H_ + +class Mavlink; +class MavlinkOrbListener; +class MavlinkOrbSubscription; + +class MavlinkStream { +public: + void (*callback)(const MavlinkStream *); + uintptr_t arg; + unsigned int subscriptions_n; + MavlinkOrbSubscription **subscriptions; + hrt_abstime last_sent; + unsigned int interval; + MavlinkStream *next; + Mavlink *mavlink; + MavlinkOrbListener* listener; + + MavlinkStream(MavlinkOrbListener *listener, void (*callback)(const MavlinkStream *), const unsigned int subs_n, const struct orb_metadata **metas, const size_t *sizes, const uintptr_t arg, const unsigned int interval); + ~MavlinkStream(); + int update(const hrt_abstime t); +}; + + +#endif /* MAVLINK_STREAM_H_ */ diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index 76798eb124..dc8d5586f4 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -39,6 +39,8 @@ MODULE_COMMAND = mavlink SRCS += mavlink_main.cpp \ mavlink.c \ mavlink_receiver.cpp \ - mavlink_orb_listener.cpp + mavlink_orb_listener.cpp \ + mavlink_orb_subscription.cpp \ + mavlink_stream.cpp INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink From cf7ac7e660b4be82d98b881fea6cbc718978605b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 25 Feb 2014 00:04:44 +0400 Subject: [PATCH 090/381] mavlink_orb_subscription: bug fixed --- src/modules/mavlink/mavlink_orb_subscription.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp index 182407a5eb..16044ef72a 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.cpp +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -41,7 +41,7 @@ int MavlinkOrbSubscription::update(const hrt_abstime t) bool updated; orb_check(fd, &updated); if (updated) - return orb_copy(meta, fd, &data); + return orb_copy(meta, fd, data); } return OK; } From 4e27fd9a381bd32ba5b79d275528ac19d1fb9442 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 26 Feb 2014 00:22:19 +0400 Subject: [PATCH 091/381] commander/px4_custom_mode.h: added missing include --- src/modules/commander/px4_custom_mode.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index b60a7e0b9c..2144d34607 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -8,6 +8,8 @@ #ifndef PX4_CUSTOM_MODE_H_ #define PX4_CUSTOM_MODE_H_ +#include + enum PX4_CUSTOM_MAIN_MODE { PX4_CUSTOM_MAIN_MODE_MANUAL = 1, PX4_CUSTOM_MAIN_MODE_SEATBELT, From e291af990fd9a4f447cbad2416b78d031cd33f5c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 26 Feb 2014 00:24:14 +0400 Subject: [PATCH 092/381] mavlink: adding message stream by name implemnted, mavlink streams definitions and formatters moved to mavlink_messages.h/cpp, mavlink_orb_listener class and thread removed --- src/modules/mavlink/mavlink_main.cpp | 442 ++++++------------ src/modules/mavlink/mavlink_main.h | 88 +--- ..._orb_listener.cpp => mavlink_messages.cpp} | 294 +++++------- src/modules/mavlink/mavlink_messages.h | 28 ++ src/modules/mavlink/mavlink_orb_listener.h | 138 ------ .../mavlink/mavlink_orb_subscription.cpp | 23 +- .../mavlink/mavlink_orb_subscription.h | 6 +- src/modules/mavlink/mavlink_stream.cpp | 8 +- src/modules/mavlink/mavlink_stream.h | 10 +- src/modules/mavlink/module.mk | 2 +- 10 files changed, 326 insertions(+), 713 deletions(-) rename src/modules/mavlink/{mavlink_orb_listener.cpp => mavlink_messages.cpp} (77%) create mode 100644 src/modules/mavlink/mavlink_messages.h delete mode 100644 src/modules/mavlink/mavlink_orb_listener.h diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 1ce467a7b8..4a75b00ab9 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -79,11 +79,9 @@ #include #include -#include - #include "mavlink_bridge_header.h" #include "mavlink_main.h" -#include "mavlink_orb_listener.h" +#include "mavlink_messages.h" #include "mavlink_receiver.h" /* oddly, ERROR is not defined for c++ */ @@ -92,6 +90,8 @@ #endif static const int ERROR = -1; +#define MAIN_LOOP_DELAY 10000 // 100 Hz + static Mavlink* _head = nullptr; /* TODO: if this is a class member it crashes */ @@ -170,7 +170,8 @@ Mavlink::Mavlink() : /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mavlink")), _mavlink_hil_enabled(false), - // _params_sub(-1) + _subscriptions(nullptr), + _streams(nullptr), mission_pub(-1) { @@ -325,17 +326,6 @@ int Mavlink::get_channel() return (int)_chan; } -void -Mavlink::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)); - -} - /**************************************************************************** * MAVLink text message logger ****************************************************************************/ @@ -532,13 +522,13 @@ Mavlink::set_hil_enabled(bool hil_enabled) hil_rate_interval = 5; } - orb_set_interval(subs.spa_sub, hil_rate_interval); - set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, hil_rate_interval); +// orb_set_interval(subs.spa_sub, hil_rate_interval); +// set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, hil_rate_interval); } if (!hil_enabled && _mavlink_hil_enabled) { _mavlink_hil_enabled = false; - orb_set_interval(subs.spa_sub, 200); +// orb_set_interval(subs.spa_sub, 200); } else { ret = ERROR; @@ -547,159 +537,8 @@ Mavlink::set_hil_enabled(bool hil_enabled) return ret; } -void -Mavlink::get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode) -{ - /* reset MAVLink mode bitfield */ - *mavlink_base_mode = 0; - *mavlink_custom_mode = 0; - - /** - * Set mode flags - **/ - - /* HIL */ - if (v_status.hil_state == HIL_STATE_ON) { - *mavlink_base_mode |= MAV_MODE_FLAG_HIL_ENABLED; - } - - /* arming state */ - if (v_status.arming_state == ARMING_STATE_ARMED - || v_status.arming_state == ARMING_STATE_ARMED_ERROR) { - *mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; - } - - /* main state */ - *mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; - union px4_custom_mode custom_mode; - custom_mode.data = 0; - if (pos_sp_triplet.nav_state == NAV_STATE_NONE) { - /* use main state when navigator is not active */ - if (v_status.main_state == MAIN_STATE_MANUAL) { - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (v_status.is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; - } else if (v_status.main_state == MAIN_STATE_SEATBELT) { - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT; - } else if (v_status.main_state == MAIN_STATE_EASY) { - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY; - } else if (v_status.main_state == MAIN_STATE_AUTO) { - *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; - } - } else { - /* use navigation state when navigator is active */ - *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - if (pos_sp_triplet.nav_state == NAV_STATE_READY) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; - } else if (pos_sp_triplet.nav_state == NAV_STATE_LOITER) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; - } else if (pos_sp_triplet.nav_state == NAV_STATE_MISSION) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; - } else if (pos_sp_triplet.nav_state == NAV_STATE_RTL) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; - } else if (pos_sp_triplet.nav_state == NAV_STATE_LAND) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; - } - } - *mavlink_custom_mode = custom_mode.data; - - /** - * Set mavlink state - **/ - - /* set calibration state */ - if (v_status.arming_state == ARMING_STATE_INIT - || v_status.arming_state == ARMING_STATE_IN_AIR_RESTORE - || v_status.arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review - *mavlink_state = MAV_STATE_UNINIT; - } else if (v_status.arming_state == ARMING_STATE_ARMED) { - *mavlink_state = MAV_STATE_ACTIVE; - } else if (v_status.arming_state == ARMING_STATE_ARMED_ERROR) { - *mavlink_state = MAV_STATE_CRITICAL; - } else if (v_status.arming_state == ARMING_STATE_STANDBY) { - *mavlink_state = MAV_STATE_STANDBY; - } else if (v_status.arming_state == ARMING_STATE_REBOOT) { - *mavlink_state = MAV_STATE_POWEROFF; - } else { - warnx("Unknown mavlink state"); - *mavlink_state = MAV_STATE_CRITICAL; - } -} - - -int Mavlink::set_mavlink_interval_limit(int mavlink_msg_id, int min_interval) -{ - int ret = OK; - - switch (mavlink_msg_id) { - case MAVLINK_MSG_ID_SCALED_IMU: - /* sensor sub triggers scaled IMU */ - orb_set_interval(subs.sensor_sub, min_interval); - break; - - case MAVLINK_MSG_ID_HIGHRES_IMU: - /* sensor sub triggers highres IMU */ - orb_set_interval(subs.sensor_sub, min_interval); - break; - - case MAVLINK_MSG_ID_RAW_IMU: - /* sensor sub triggers RAW IMU */ - orb_set_interval(subs.sensor_sub, min_interval); - break; - - case MAVLINK_MSG_ID_ATTITUDE: - /* attitude sub triggers attitude */ - orb_set_interval(subs.att_sub, min_interval); - break; - - case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW: - /* actuator_outputs triggers this message */ - orb_set_interval(subs.act_0_sub, min_interval); - orb_set_interval(subs.act_1_sub, min_interval); - orb_set_interval(subs.act_2_sub, min_interval); - orb_set_interval(subs.act_3_sub, min_interval); - orb_set_interval(subs.actuators_sub, min_interval); - orb_set_interval(subs.actuators_effective_sub, min_interval); - orb_set_interval(subs.spa_sub, min_interval); - orb_set_interval(subs.rates_setpoint_sub, min_interval); - break; - - case MAVLINK_MSG_ID_MANUAL_CONTROL: - /* manual_control_setpoint triggers this message */ - orb_set_interval(subs.man_control_sp_sub, min_interval); - break; - - case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT: - orb_set_interval(subs.debug_key_value, min_interval); - break; - - default: - /* not found */ - ret = ERROR; - break; - } - - return ret; -} - extern mavlink_system_t mavlink_system; -void Mavlink::mavlink_pm_callback(void *arg, param_t param) -{ - //mavlink_pm_send_param(param); - usleep(*(unsigned int *)arg); -} - -void Mavlink::mavlink_pm_send_all_params(unsigned int delay) -{ - unsigned int dbuf = delay; - param_foreach(&mavlink_pm_callback, &dbuf, false); -} - int Mavlink::mavlink_pm_queued_send() { if (mavlink_param_queue_index < param_count()) { @@ -1519,6 +1358,51 @@ Mavlink::mavlink_missionlib_send_gcs_string(const char *string) } } +MavlinkOrbSubscription *Mavlink::add_orb_subscription(const struct orb_metadata *topic, const size_t size) +{ + /* check if already subscribed to this topic */ + MavlinkOrbSubscription *sub; + + LL_FOREACH(_subscriptions, sub) { + if (sub->topic == topic) { + /* already subscribed */ + return sub; + } + } + + /* add new subscription */ + MavlinkOrbSubscription *sub_new = new MavlinkOrbSubscription(topic, size); + + LL_APPEND(_subscriptions, sub_new); + + return sub_new; +} + +int +Mavlink::add_stream(const char *stream_name, const unsigned int interval) +{ + uintptr_t arg = 0; + + unsigned int i = 0; + /* search for message with specified name */ + while (msgs_list[i].name != nullptr) { + if (strcmp(stream_name, msgs_list[i].name) == 0) { + /* count topics, array is nullptr-terminated */ + unsigned int topics_n; + for (topics_n = 0; topics_n < MAX_TOPICS_PER_MAVLINK_STREAM; topics_n++) { + if (msgs_list[i].topics[topics_n] == nullptr) { + break; + } + } + MavlinkStream *stream = new MavlinkStream(this, msgs_list[i].callback, topics_n, msgs_list[i].topics, msgs_list[i].sizes, arg, interval); + LL_APPEND(_streams, stream); + return OK; + } + i++; + } + return ERROR; +} + int Mavlink::task_main(int argc, char *argv[]) { @@ -1639,67 +1523,64 @@ Mavlink::task_main(int argc, char *argv[]) /* start the MAVLink receiver */ receive_thread = MavlinkReceiver::receive_start(this); - /* start the ORB receiver */ - uorb_receive_thread = MavlinkOrbListener::uorb_receive_start(this); - /* initialize waypoint manager */ mavlink_wpm_init(wpm); /* all subscriptions are now active, set up initial guess about rate limits */ - if (_baudrate >= 230400) { - /* 200 Hz / 5 ms */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_HIGHRES_IMU, 20); - set_mavlink_interval_limit(MAVLINK_MSG_ID_RAW_IMU, 20); - /* 50 Hz / 20 ms */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 30); - /* 20 Hz / 50 ms */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 10); - set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50); - /* 10 Hz */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_GPS_RAW_INT, 100); - /* 10 Hz */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_MANUAL_CONTROL, 100); - - } else if (_baudrate >= 115200) { - /* 20 Hz / 50 ms */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_HIGHRES_IMU, 50); - set_mavlink_interval_limit(MAVLINK_MSG_ID_RAW_IMU, 50); - set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 50); - set_mavlink_interval_limit(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50); - /* 5 Hz / 200 ms */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200); - /* 5 Hz / 200 ms */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_GPS_RAW_INT, 200); - /* 2 Hz */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_MANUAL_CONTROL, 500); - - } else if (_baudrate >= 57600) { - /* 10 Hz / 100 ms */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_RAW_IMU, 300); - set_mavlink_interval_limit(MAVLINK_MSG_ID_HIGHRES_IMU, 300); - /* 10 Hz / 100 ms ATTITUDE */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 200); - /* 5 Hz / 200 ms */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200); - /* 5 Hz / 200 ms */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 500); - /* 2 Hz */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_MANUAL_CONTROL, 500); - /* 2 Hz */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_GPS_RAW_INT, 500); - - } else { - /* very low baud rate, limit to 1 Hz / 1000 ms */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_RAW_IMU, 1000); - set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 1000); - set_mavlink_interval_limit(MAVLINK_MSG_ID_HIGHRES_IMU, 1000); - /* 1 Hz / 1000 ms */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 1000); - /* 0.5 Hz */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 2000); - /* 0.1 Hz */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_MANUAL_CONTROL, 10000); - } +// if (_baudrate >= 230400) { +// /* 200 Hz / 5 ms */ +// set_mavlink_interval_limit(MAVLINK_MSG_ID_HIGHRES_IMU, 20); +// set_mavlink_interval_limit(MAVLINK_MSG_ID_RAW_IMU, 20); +// /* 50 Hz / 20 ms */ +// set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 30); +// /* 20 Hz / 50 ms */ +// set_mavlink_interval_limit(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 10); +// set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50); +// /* 10 Hz */ +// set_mavlink_interval_limit(MAVLINK_MSG_ID_GPS_RAW_INT, 100); +// /* 10 Hz */ +// set_mavlink_interval_limit(MAVLINK_MSG_ID_MANUAL_CONTROL, 100); +// +// } else if (_baudrate >= 115200) { +// /* 20 Hz / 50 ms */ +// set_mavlink_interval_limit(MAVLINK_MSG_ID_HIGHRES_IMU, 50); +// set_mavlink_interval_limit(MAVLINK_MSG_ID_RAW_IMU, 50); +// set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 50); +// set_mavlink_interval_limit(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50); +// /* 5 Hz / 200 ms */ +// set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200); +// /* 5 Hz / 200 ms */ +// set_mavlink_interval_limit(MAVLINK_MSG_ID_GPS_RAW_INT, 200); +// /* 2 Hz */ +// set_mavlink_interval_limit(MAVLINK_MSG_ID_MANUAL_CONTROL, 500); +// +// } else if (_baudrate >= 57600) { +// /* 10 Hz / 100 ms */ +// set_mavlink_interval_limit(MAVLINK_MSG_ID_RAW_IMU, 300); +// set_mavlink_interval_limit(MAVLINK_MSG_ID_HIGHRES_IMU, 300); +// /* 10 Hz / 100 ms ATTITUDE */ +// set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 200); +// /* 5 Hz / 200 ms */ +// set_mavlink_interval_limit(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200); +// /* 5 Hz / 200 ms */ +// set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 500); +// /* 2 Hz */ +// set_mavlink_interval_limit(MAVLINK_MSG_ID_MANUAL_CONTROL, 500); +// /* 2 Hz */ +// set_mavlink_interval_limit(MAVLINK_MSG_ID_GPS_RAW_INT, 500); +// +// } else { +// /* very low baud rate, limit to 1 Hz / 1000 ms */ +// set_mavlink_interval_limit(MAVLINK_MSG_ID_RAW_IMU, 1000); +// set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 1000); +// set_mavlink_interval_limit(MAVLINK_MSG_ID_HIGHRES_IMU, 1000); +// /* 1 Hz / 1000 ms */ +// set_mavlink_interval_limit(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 1000); +// /* 0.5 Hz */ +// set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 2000); +// /* 0.1 Hz */ +// set_mavlink_interval_limit(MAVLINK_MSG_ID_MANUAL_CONTROL, 10000); +// } int mission_result_sub = orb_subscribe(ORB_ID(mission_result)); struct mission_result_s mission_result; @@ -1709,72 +1590,42 @@ Mavlink::task_main(int argc, char *argv[]) unsigned lowspeed_counter = 0; - /* wakeup source(s) */ - struct pollfd fds[1]; + MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update), sizeof(parameter_update_s)); + MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status), sizeof(vehicle_status_s)); - _params_sub = orb_subscribe(ORB_ID(parameter_update)); + struct vehicle_status_s *status = (struct vehicle_status_s *) status_sub->data; - /* Setup of loop */ - fds[0].fd = _params_sub; - fds[0].events = POLLIN; + add_stream("HEARTBEAT", 1000); + add_stream("SYS_STATUS", 100); while (!_task_should_exit) { - - /* wait for up to 100ms for data */ - int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); - - /* 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; - } + /* main loop */ + usleep(MAIN_LOOP_DELAY); perf_begin(_loop_perf); - /* parameters updated */ - if (fds[0].revents & POLLIN) { - parameters_update(); + hrt_abstime t = hrt_absolute_time(); + + if (param_sub->update(t)) { + /* parameters updated */ + mavlink_update_system(); } - /* 1 Hz */ - if (lowspeed_counter % 10 == 0) { - mavlink_update_system(); - - /* translate the current system state to mavlink state and mode */ - uint8_t mavlink_state = 0; - uint8_t mavlink_base_mode = 0; - uint32_t mavlink_custom_mode = 0; - get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); - - /* send heartbeat */ - mavlink_msg_heartbeat_send(_chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_base_mode, mavlink_custom_mode, mavlink_state); - + if (status_sub->update(t)) { /* switch HIL mode if required */ - if (v_status.hil_state == HIL_STATE_ON) + if (status->hil_state == HIL_STATE_ON) set_hil_enabled(true); - else if (v_status.hil_state == HIL_STATE_OFF) + else if (status->hil_state == HIL_STATE_OFF) set_hil_enabled(false); + } - /* send status (values already copied in the section above) */ - mavlink_msg_sys_status_send(_chan, - v_status.onboard_control_sensors_present, - v_status.onboard_control_sensors_enabled, - v_status.onboard_control_sensors_health, - v_status.load * 1000.0f, - v_status.battery_voltage * 1000.0f, - v_status.battery_current * 1000.0f, - v_status.battery_remaining, - v_status.drop_rate_comm, - v_status.errors_comm, - v_status.errors_count1, - v_status.errors_count2, - v_status.errors_count3, - v_status.errors_count4); + MavlinkStream *stream; + LL_FOREACH(_streams, stream) { + stream->update(t); } /* 0.5 Hz */ - if (lowspeed_counter % 20 == 0) { - + if (lowspeed_counter % (2000000 / MAIN_LOOP_DELAY) == 0) { mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission); } @@ -1800,27 +1651,28 @@ Mavlink::task_main(int argc, char *argv[]) /* check if waypoint has been reached against the last positions */ mavlink_waypoint_eventloop(hrt_absolute_time()); - - /* send parameters at 20 Hz (if queued for sending) */ - mavlink_pm_queued_send(); - mavlink_waypoint_eventloop(hrt_absolute_time()); - - mavlink_waypoint_eventloop(hrt_absolute_time()); - - if (_baudrate > 57600) { + if (lowspeed_counter % (50000 / MAIN_LOOP_DELAY) == 0) { + /* send parameters at 20 Hz (if queued for sending) */ mavlink_pm_queued_send(); + mavlink_waypoint_eventloop(hrt_absolute_time()); + + mavlink_waypoint_eventloop(hrt_absolute_time()); + + if (_baudrate > 57600) { + mavlink_pm_queued_send(); + } + + /* send one string at 10 Hz */ + if (!mavlink_logbuffer_is_empty(&lb)) { + struct mavlink_logmessage msg; + int lb_ret = mavlink_logbuffer_read(&lb, &msg); + + if (lb_ret == OK) { + mavlink_missionlib_send_gcs_string(msg.text); + } + } } - /* send one string at 10 Hz */ - if (!mavlink_logbuffer_is_empty(&lb)) { - struct mavlink_logmessage msg; - int lb_ret = mavlink_logbuffer_read(&lb, &msg); - - if (lb_ret == OK) { - mavlink_missionlib_send_gcs_string(msg.text); - } - } - perf_end(_loop_perf); } @@ -1878,7 +1730,7 @@ int mavlink_main(int argc, char *argv[]) // Instantiate thread char buf[32]; - sprintf(buf, "mavlink if%d", Mavlink::instance_count()); + sprintf(buf, "mavlink_if%d", Mavlink::instance_count()); /*mavlink->_mavlink_task = */task_spawn_cmd(buf, SCHED_DEFAULT, diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 9461a51f8f..56d262000b 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -41,6 +41,12 @@ #pragma once #include +#include +#include +#include +#include +#include +#include #include #include @@ -68,12 +74,12 @@ #include #include #include -#include #include #include -#include -#include +#include "mavlink_bridge_header.h" +#include "mavlink_orb_subscription.h" +#include "mavlink_stream.h" // FIXME XXX - TO BE MOVED TO XML enum MAVLINK_WPM_STATES { @@ -185,7 +191,7 @@ public: */ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg); - void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode); + void get_mavlink_mode_and_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode); /** * Enable / disable Hardware in the Loop simulation mode. @@ -197,57 +203,10 @@ public: */ int set_hil_enabled(bool hil_enabled); - struct mavlink_subscriptions { - int sensor_sub; - int att_sub; - int global_pos_sub; - int act_0_sub; - int act_1_sub; - int act_2_sub; - int act_3_sub; - int gps_sub; - int man_control_sp_sub; - int safety_sub; - int actuators_sub; - int armed_sub; - int actuators_effective_sub; - int local_pos_sub; - int spa_sub; - int spl_sub; - int triplet_sub; - int debug_key_value; - int input_rc_sub; - int optical_flow; - int rates_setpoint_sub; - int get_sub; - int airspeed_sub; - int navigation_capabilities_sub; - int position_setpoint_triplet_sub; - int rc_sub; - int status_sub; - int home_sub; - }; + MavlinkOrbSubscription *add_orb_subscription(const struct orb_metadata *topic, size_t size); - struct mavlink_subscriptions subs; - - struct mavlink_subscriptions* get_subs() { return &subs; } mavlink_channel_t get_chan() { return _chan; } - /** Global position */ - struct vehicle_global_position_s global_pos; - /** Local position */ - struct vehicle_local_position_s local_pos; - /** navigation capabilities */ - struct navigation_capabilities_s nav_cap; - /** Vehicle status */ - struct vehicle_status_s v_status; - /** RC channels */ - struct rc_channels_s rc; - /** Actuator armed state */ - struct actuator_armed_s armed; - /** Position setpoint triplet */ - struct position_setpoint_triplet_s pos_sp_triplet; - bool _task_should_exit; /**< if true, mavlink task should exit */ protected: @@ -270,7 +229,8 @@ private: /* states */ bool _mavlink_hil_enabled; /**< Hardware in the loop mode */ - int _params_sub; + MavlinkOrbSubscription *_subscriptions; + MavlinkStream *_streams; orb_advert_t mission_pub; struct mission_s mission; @@ -306,21 +266,6 @@ private: bool mavlink_link_termination_allowed; - /** - * Update our local parameter cache. - */ - void parameters_update(); - - /** - * Send all parameters at once. - * - * This function blocks until all parameters have been sent. - * it delays each parameter by the passed amount of microseconds. - * - * @param delay The delay in us between sending all parameters. - */ - void mavlink_pm_send_all_params(unsigned int delay); - /** * Send one parameter. * @@ -381,12 +326,7 @@ private: int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); - int set_mavlink_interval_limit(int mavlink_msg_id, int min_interval); - - /** - * Callback for param interface. - */ - static void mavlink_pm_callback(void *arg, param_t param); + int add_stream(const char *stream_name, const unsigned int interval); static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); diff --git a/src/modules/mavlink/mavlink_orb_listener.cpp b/src/modules/mavlink/mavlink_messages.cpp similarity index 77% rename from src/modules/mavlink/mavlink_orb_listener.cpp rename to src/modules/mavlink/mavlink_messages.cpp index ea1e3a8bb9..77ec344dc6 100644 --- a/src/modules/mavlink/mavlink_orb_listener.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1,131 +1,138 @@ -/**************************************************************************** +/* + * mavlink_messages.cpp * - * 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. - * - ****************************************************************************/ - -/** - * @file orb_listener.cpp - * Monitors ORB topics and sends update messages as appropriate. - * - * @author Lorenz Meier - * @author Julian Oes + * Created on: 25.02.2014 + * Author: ton */ -// XXX trim includes -#include -#include -#include -#include -#include -#include -#include "mavlink_bridge_header.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include -#include +#include +#include -#include "mavlink_orb_listener.h" -#include "mavlink_main.h" +#include "mavlink_messages.h" -uint16_t cm_uint16_from_m_float(float m); +struct msgs_list_s msgs_list[] = { + { + .name = "HEARTBEAT", + .callback = msg_heartbeat, + .topics = { ORB_ID(vehicle_status), ORB_ID(position_setpoint_triplet), nullptr }, + .sizes = { sizeof(struct vehicle_status_s), sizeof(struct position_setpoint_triplet_s) } + }, + { + .name = "SYS_STATUS", + .callback = msg_sys_status, + .topics = { ORB_ID(vehicle_status), nullptr }, + .sizes = { sizeof(struct vehicle_status_s) } + }, + { .name = nullptr } +}; -uint16_t -cm_uint16_from_m_float(float m) +void +msg_heartbeat(const MavlinkStream *stream) { - if (m < 0.0f) { - return 0; + struct vehicle_status_s *status = (struct vehicle_status_s *)stream->subscriptions[0]->data; + struct position_setpoint_triplet_s *pos_sp_triplet = (struct position_setpoint_triplet_s *)stream->subscriptions[1]->data; - } else if (m > 655.35f) { - return 65535; + uint8_t mavlink_state = 0; + uint8_t mavlink_base_mode = 0; + uint32_t mavlink_custom_mode = 0; + + /* HIL */ + if (status->hil_state == HIL_STATE_ON) { + mavlink_base_mode |= MAV_MODE_FLAG_HIL_ENABLED; } - return (uint16_t)(m * 100.0f); -} + /* arming state */ + if (status->arming_state == ARMING_STATE_ARMED + || status->arming_state == ARMING_STATE_ARMED_ERROR) { + mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; + } -MavlinkOrbListener::MavlinkOrbListener(Mavlink* parent) : - - _loop_perf(perf_alloc(PC_ELAPSED, "mavlink orb")), - _mavlink(parent), - _subscriptions(nullptr), - _streams(nullptr) -{ - -} - -MavlinkOrbListener::~MavlinkOrbListener() -{ - -} - -MavlinkOrbSubscription *MavlinkOrbListener::add_subscription(const struct orb_metadata *meta, const size_t size, const MavlinkStream *stream, const unsigned int interval) -{ - /* check if already subscribed to this topic */ - MavlinkOrbSubscription *sub; - - LL_FOREACH(_subscriptions, sub) { - if (sub->meta == meta) { - /* already subscribed */ - if (sub->interval > interval) { - /* subscribed with bigger interval, change interval */ - sub->set_interval(interval); - } - return sub; + /* main state */ + mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; + union px4_custom_mode custom_mode; + custom_mode.data = 0; + if (pos_sp_triplet->nav_state == NAV_STATE_NONE) { + /* use main state when navigator is not active */ + if (status->main_state == MAIN_STATE_MANUAL) { + mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; + } else if (status->main_state == MAIN_STATE_SEATBELT) { + mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT; + } else if (status->main_state == MAIN_STATE_EASY) { + mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY; + } else if (status->main_state == MAIN_STATE_AUTO) { + mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; + } + } else { + /* use navigation state when navigator is active */ + mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + if (pos_sp_triplet->nav_state == NAV_STATE_READY) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; + } else if (pos_sp_triplet->nav_state == NAV_STATE_LOITER) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; + } else if (pos_sp_triplet->nav_state == NAV_STATE_MISSION) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; + } else if (pos_sp_triplet->nav_state == NAV_STATE_RTL) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; + } else if (pos_sp_triplet->nav_state == NAV_STATE_LAND) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; } } + mavlink_custom_mode = custom_mode.data; - /* add new subscription */ - MavlinkOrbSubscription *sub_new = new MavlinkOrbSubscription(meta, size); + /* set system state */ + if (status->arming_state == ARMING_STATE_INIT + || status->arming_state == ARMING_STATE_IN_AIR_RESTORE + || status->arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review + mavlink_state = MAV_STATE_UNINIT; + } else if (status->arming_state == ARMING_STATE_ARMED) { + mavlink_state = MAV_STATE_ACTIVE; + } else if (status->arming_state == ARMING_STATE_ARMED_ERROR) { + mavlink_state = MAV_STATE_CRITICAL; + } else if (status->arming_state == ARMING_STATE_STANDBY) { + mavlink_state = MAV_STATE_STANDBY; + } else if (status->arming_state == ARMING_STATE_REBOOT) { + mavlink_state = MAV_STATE_POWEROFF; + } else { + mavlink_state = MAV_STATE_CRITICAL; + } - sub_new->set_interval(interval); - - LL_APPEND(_subscriptions, sub_new); - - return sub_new; + /* send heartbeat */ + mavlink_msg_heartbeat_send(stream->mavlink->get_chan(), + mavlink_system.type, + MAV_AUTOPILOT_PX4, + mavlink_base_mode, + mavlink_custom_mode, + mavlink_state); } -void MavlinkOrbListener::add_stream(void (*callback)(const MavlinkStream *), const unsigned int subs_n, const struct orb_metadata **metas, const size_t *sizes, const uintptr_t arg, const unsigned int interval) +void +msg_sys_status(const MavlinkStream *stream) { - MavlinkStream *stream = new MavlinkStream(this, callback, subs_n, metas, sizes, arg, interval); + struct vehicle_status_s *status = (struct vehicle_status_s *)stream->subscriptions[0]->data; - stream->mavlink = _mavlink; - - LL_APPEND(_streams, stream); + mavlink_msg_sys_status_send(stream->mavlink->get_chan(), + status->onboard_control_sensors_present, + status->onboard_control_sensors_enabled, + status->onboard_control_sensors_health, + status->load * 1000.0f, + status->battery_voltage * 1000.0f, + status->battery_current * 1000.0f, + status->battery_remaining, + status->drop_rate_comm, + status->errors_comm, + status->errors_count1, + status->errors_count2, + status->errors_count3, + status->errors_count4); } //void @@ -259,25 +266,6 @@ void MavlinkOrbListener::add_stream(void (*callback)(const MavlinkStream *), con // l->listener->gps_counter++; //} // - -void -MavlinkOrbListener::msg_heartbeat(const MavlinkStream *stream) -{ - /* translate the current syste state to mavlink state and mode */ - uint8_t mavlink_state = 0; - uint8_t mavlink_base_mode = 0; - uint32_t mavlink_custom_mode = 0; - //l->mavlink->get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); - - /* send heartbeat */ - mavlink_msg_heartbeat_send(stream->mavlink->get_chan(), - mavlink_system.type, - MAV_AUTOPILOT_PX4, - mavlink_base_mode, - mavlink_custom_mode, - mavlink_state); -} - // //void //MavlinkOrbListener::l_rc_channels(const struct listener *l) @@ -623,57 +611,3 @@ MavlinkOrbListener::msg_heartbeat(const MavlinkStream *stream) // l->listener->nav_cap.turn_distance); // //} - -void * -MavlinkOrbListener::uorb_receive_thread(void *arg) -{ - /* set thread name */ - char thread_name[18]; - sprintf(thread_name, "mavlink_uorb_rcv_%d", _mavlink->get_channel()); - prctl(PR_SET_NAME, thread_name, getpid()); - - /* add mavlink streams */ - /* common buffer for topics and data sizes */ - const struct orb_metadata *topics[1]; - size_t sizes[1]; - - /* --- HEARTBEAT --- */ - topics[0] = ORB_ID(vehicle_status); - sizes[0] = sizeof(vehicle_status_s); - add_stream(msg_heartbeat, 1, topics, sizes, 0, 500); - - while (!_mavlink->_task_should_exit) { - /* check all streams each 1ms */ - hrt_abstime t = hrt_absolute_time(); - MavlinkStream *stream; - LL_FOREACH(_streams, stream) { - stream->update(t); - } - usleep(1000); - } - - return NULL; -} - -void * MavlinkOrbListener::uorb_start_helper(void *context) -{ - MavlinkOrbListener* urcv = new MavlinkOrbListener(((Mavlink *)context)); - return urcv->uorb_receive_thread(NULL); -} - -pthread_t -MavlinkOrbListener::uorb_receive_start(Mavlink* mavlink) -{ - /* start the listener loop */ - pthread_attr_t uorb_attr; - pthread_attr_init(&uorb_attr); - - /* Set stack size, needs less than 2k */ - pthread_attr_setstacksize(&uorb_attr, 2048); - - pthread_t thread; - pthread_create(&thread, &uorb_attr, MavlinkOrbListener::uorb_start_helper, (void*)mavlink); - - pthread_attr_destroy(&uorb_attr); - return thread; -} diff --git a/src/modules/mavlink/mavlink_messages.h b/src/modules/mavlink/mavlink_messages.h new file mode 100644 index 0000000000..78279c08f0 --- /dev/null +++ b/src/modules/mavlink/mavlink_messages.h @@ -0,0 +1,28 @@ +/* + * mavlink_messages.h + * + * Created on: 25.02.2014 + * Author: ton + */ + +#ifndef MAVLINK_MESSAGES_H_ +#define MAVLINK_MESSAGES_H_ + +#include "mavlink_stream.h" + +#define MAX_TOPICS_PER_MAVLINK_STREAM 4 + +struct msgs_list_s { + char *name; + void (*callback)(const MavlinkStream *); + const struct orb_metadata *topics[MAX_TOPICS_PER_MAVLINK_STREAM+1]; + size_t sizes[MAX_TOPICS_PER_MAVLINK_STREAM+1]; +}; + +extern struct msgs_list_s msgs_list[]; + +static void msg_heartbeat(const MavlinkStream *stream); +static void msg_sys_status(const MavlinkStream *stream); + + +#endif /* MAVLINK_MESSAGES_H_ */ diff --git a/src/modules/mavlink/mavlink_orb_listener.h b/src/modules/mavlink/mavlink_orb_listener.h deleted file mode 100644 index 1a103e43d3..0000000000 --- a/src/modules/mavlink/mavlink_orb_listener.h +++ /dev/null @@ -1,138 +0,0 @@ -/**************************************************************************** - * - * 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. - * - ****************************************************************************/ - -/** - * @file mavlink_orb_listener.h - * MAVLink 1.0 protocol interface definition. - * - * @author Lorenz Meier - * @author Julian Oes - */ - -#include -#include - -#pragma once - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "mavlink_orb_subscription.h" -#include "mavlink_stream.h" - -class Mavlink; -class MavlinkStream; - -class MavlinkOrbListener -{ -public: - /** - * Constructor - */ - MavlinkOrbListener(Mavlink* parent); - - /** - * Destructor, closes all subscriptions. - */ - ~MavlinkOrbListener(); - - static pthread_t uorb_receive_start(Mavlink *mavlink); - - MavlinkOrbSubscription *add_subscription(const struct orb_metadata *meta, size_t size, const MavlinkStream *stream, const unsigned int interval); - void add_stream(void (*callback)(const MavlinkStream *), const unsigned int subs_n, const struct orb_metadata **metas, const size_t *sizes, const uintptr_t arg, const unsigned int interval); - static void * uorb_start_helper(void *context); - -private: - - perf_counter_t _loop_perf; /**< loop performance counter */ - - Mavlink* _mavlink; - - MavlinkOrbSubscription *_subscriptions; - static const unsigned _max_subscriptions = 32; - MavlinkStream *_streams; - - void *uorb_receive_thread(void *arg); - - static void msg_heartbeat(const MavlinkStream *stream); - -// static void l_sensor_combined(const struct listener *l); -// static void l_vehicle_attitude(const struct listener *l); -// static void l_vehicle_gps_position(const struct listener *l); -// static void l_vehicle_status(const struct listener *l); -// static void l_rc_channels(const struct listener *l); -// static void l_input_rc(const struct listener *l); -// static void l_global_position(const struct listener *l); -// static void l_local_position(const struct listener *l); -// static void l_global_position_setpoint(const struct listener *l); -// static void l_local_position_setpoint(const struct listener *l); -// static void l_attitude_setpoint(const struct listener *l); -// static void l_actuator_outputs(const struct listener *l); -// static void l_actuator_armed(const struct listener *l); -// static void l_manual_control_setpoint(const struct listener *l); -// static void l_vehicle_attitude_controls(const struct listener *l); -// static void l_debug_key_value(const struct listener *l); -// static void l_optical_flow(const struct listener *l); -// static void l_vehicle_rates_setpoint(const struct listener *l); -// static void l_home(const struct listener *l); -// static void l_airspeed(const struct listener *l); -// static void l_nav_cap(const struct listener *l); - -}; diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp index 16044ef72a..84ac11483f 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.cpp +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -13,14 +13,13 @@ #include "mavlink_orb_subscription.h" -MavlinkOrbSubscription::MavlinkOrbSubscription(const struct orb_metadata *meta, size_t size) +MavlinkOrbSubscription::MavlinkOrbSubscription(const struct orb_metadata *topic, size_t size) { - this->meta = meta; + this->topic = topic; this->data = malloc(size); memset(this->data, 0, size); - this->fd = orb_subscribe(meta); + this->fd = orb_subscribe(topic); this->last_update = 0; - this->interval = 0; } MavlinkOrbSubscription::~MavlinkOrbSubscription() @@ -29,19 +28,15 @@ MavlinkOrbSubscription::~MavlinkOrbSubscription() free(data); } -int MavlinkOrbSubscription::set_interval(const unsigned int interval) -{ - this->interval = interval; - return orb_set_interval(fd, interval); -} - -int MavlinkOrbSubscription::update(const hrt_abstime t) +bool MavlinkOrbSubscription::update(const hrt_abstime t) { if (last_update != t) { bool updated; orb_check(fd, &updated); - if (updated) - return orb_copy(meta, fd, data); + if (updated) { + orb_copy(topic, fd, data); + return true; + } } - return OK; + return false; } diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h index 2c72abaeff..9a7340e9bb 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.h +++ b/src/modules/mavlink/mavlink_orb_subscription.h @@ -16,14 +16,12 @@ public: MavlinkOrbSubscription(const struct orb_metadata *meta, size_t size); ~MavlinkOrbSubscription(); - int set_interval(const unsigned int interval); - int update(const hrt_abstime t); + bool update(const hrt_abstime t); - const struct orb_metadata *meta; + const struct orb_metadata *topic; int fd; void *data; hrt_abstime last_update; - unsigned int interval; MavlinkOrbSubscription *next; }; diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index 2a6728fdb2..9df4263ee2 100644 --- a/src/modules/mavlink/mavlink_stream.cpp +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -7,20 +7,20 @@ #include -#include "mavlink_orb_listener.h" +#include "mavlink_stream.h" +#include "mavlink_main.h" -MavlinkStream::MavlinkStream(MavlinkOrbListener *listener, void (*callback)(const MavlinkStream *), const unsigned int subs_n, const struct orb_metadata **metas, const size_t *sizes, const uintptr_t arg, const unsigned int interval) +MavlinkStream::MavlinkStream(Mavlink *mavlink, void (*callback)(const MavlinkStream *), const unsigned int subs_n, const struct orb_metadata **topics, const size_t *sizes, const uintptr_t arg, const unsigned int interval) { this->callback = callback; this->arg = arg; this->interval = interval * 1000; this->mavlink = mavlink; - this->listener = listener; this->subscriptions_n = subs_n; this->subscriptions = (MavlinkOrbSubscription **) malloc(subs_n * sizeof(MavlinkOrbSubscription *)); for (int i = 0; i < subs_n; i++) { - this->subscriptions[i] = listener->add_subscription(metas[i], sizes[i], this, interval); + this->subscriptions[i] = mavlink->add_orb_subscription(topics[i], sizes[i]); } } diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index 0f959d7206..c3e60917ef 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -8,8 +8,13 @@ #ifndef MAVLINK_STREAM_H_ #define MAVLINK_STREAM_H_ +#include + class Mavlink; -class MavlinkOrbListener; +class MavlinkStream; + +#include "mavlink_main.h" + class MavlinkOrbSubscription; class MavlinkStream { @@ -22,9 +27,8 @@ public: unsigned int interval; MavlinkStream *next; Mavlink *mavlink; - MavlinkOrbListener* listener; - MavlinkStream(MavlinkOrbListener *listener, void (*callback)(const MavlinkStream *), const unsigned int subs_n, const struct orb_metadata **metas, const size_t *sizes, const uintptr_t arg, const unsigned int interval); + MavlinkStream(Mavlink *mavlink, void (*callback)(const MavlinkStream *), const unsigned int subs_n, const struct orb_metadata **topics, const size_t *sizes, const uintptr_t arg, const unsigned int interval); ~MavlinkStream(); int update(const hrt_abstime t); }; diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index dc8d5586f4..5ea6c0f46a 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -39,8 +39,8 @@ MODULE_COMMAND = mavlink SRCS += mavlink_main.cpp \ mavlink.c \ mavlink_receiver.cpp \ - mavlink_orb_listener.cpp \ mavlink_orb_subscription.cpp \ + mavlink_messages.cpp \ mavlink_stream.cpp INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink From 769a2af1f8925a2d47fd47a2d25f8d7baac150ec Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 26 Feb 2014 00:38:21 +0400 Subject: [PATCH 093/381] mavlink: HIGHRES_IMU message added, default message streams added --- src/modules/mavlink/mavlink_main.cpp | 72 +++++------------------- src/modules/mavlink/mavlink_messages.cpp | 17 ++++++ src/modules/mavlink/mavlink_messages.h | 2 +- 3 files changed, 33 insertions(+), 58 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 4a75b00ab9..1563a257b9 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1526,62 +1526,6 @@ Mavlink::task_main(int argc, char *argv[]) /* initialize waypoint manager */ mavlink_wpm_init(wpm); - /* all subscriptions are now active, set up initial guess about rate limits */ -// if (_baudrate >= 230400) { -// /* 200 Hz / 5 ms */ -// set_mavlink_interval_limit(MAVLINK_MSG_ID_HIGHRES_IMU, 20); -// set_mavlink_interval_limit(MAVLINK_MSG_ID_RAW_IMU, 20); -// /* 50 Hz / 20 ms */ -// set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 30); -// /* 20 Hz / 50 ms */ -// set_mavlink_interval_limit(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 10); -// set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50); -// /* 10 Hz */ -// set_mavlink_interval_limit(MAVLINK_MSG_ID_GPS_RAW_INT, 100); -// /* 10 Hz */ -// set_mavlink_interval_limit(MAVLINK_MSG_ID_MANUAL_CONTROL, 100); -// -// } else if (_baudrate >= 115200) { -// /* 20 Hz / 50 ms */ -// set_mavlink_interval_limit(MAVLINK_MSG_ID_HIGHRES_IMU, 50); -// set_mavlink_interval_limit(MAVLINK_MSG_ID_RAW_IMU, 50); -// set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 50); -// set_mavlink_interval_limit(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50); -// /* 5 Hz / 200 ms */ -// set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200); -// /* 5 Hz / 200 ms */ -// set_mavlink_interval_limit(MAVLINK_MSG_ID_GPS_RAW_INT, 200); -// /* 2 Hz */ -// set_mavlink_interval_limit(MAVLINK_MSG_ID_MANUAL_CONTROL, 500); -// -// } else if (_baudrate >= 57600) { -// /* 10 Hz / 100 ms */ -// set_mavlink_interval_limit(MAVLINK_MSG_ID_RAW_IMU, 300); -// set_mavlink_interval_limit(MAVLINK_MSG_ID_HIGHRES_IMU, 300); -// /* 10 Hz / 100 ms ATTITUDE */ -// set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 200); -// /* 5 Hz / 200 ms */ -// set_mavlink_interval_limit(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200); -// /* 5 Hz / 200 ms */ -// set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 500); -// /* 2 Hz */ -// set_mavlink_interval_limit(MAVLINK_MSG_ID_MANUAL_CONTROL, 500); -// /* 2 Hz */ -// set_mavlink_interval_limit(MAVLINK_MSG_ID_GPS_RAW_INT, 500); -// -// } else { -// /* very low baud rate, limit to 1 Hz / 1000 ms */ -// set_mavlink_interval_limit(MAVLINK_MSG_ID_RAW_IMU, 1000); -// set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 1000); -// set_mavlink_interval_limit(MAVLINK_MSG_ID_HIGHRES_IMU, 1000); -// /* 1 Hz / 1000 ms */ -// set_mavlink_interval_limit(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 1000); -// /* 0.5 Hz */ -// set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 2000); -// /* 0.1 Hz */ -// set_mavlink_interval_limit(MAVLINK_MSG_ID_MANUAL_CONTROL, 10000); -// } - int mission_result_sub = orb_subscribe(ORB_ID(mission_result)); struct mission_result_s mission_result; memset(&mission_result, 0, sizeof(mission_result)); @@ -1595,8 +1539,22 @@ Mavlink::task_main(int argc, char *argv[]) struct vehicle_status_s *status = (struct vehicle_status_s *) status_sub->data; + + /* add default streams, intervals depend on baud rate */ +// if (_baudrate >= 230400) { +// } else if (_baudrate >= 115200) { +// } else if (_baudrate >= 57600) { +// } + add_stream("HEARTBEAT", 1000); - add_stream("SYS_STATUS", 100); + add_stream("SYS_STATUS", 1000); + add_stream("HIGHRES_IMU", 300); + add_stream("RAW_IMU", 300); + add_stream("ATTITUDE", 200); + add_stream("NAMED_VALUE_FLOAT", 200); + add_stream("SERVO_OUTPUT_RAW", 500); + add_stream("GPS_RAW_INT", 500); + add_stream("MANUAL_CONTROL", 500); while (!_task_should_exit) { /* main loop */ diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 77ec344dc6..7a56c36a56 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -135,6 +135,23 @@ msg_sys_status(const MavlinkStream *stream) status->errors_count4); } +void +msg_highres_imu(const MavlinkStream *stream) +{ + struct sensor_combined_s *sensor = (struct sensor_combined_s *)stream->subscriptions[0]->data; + + uint16_t fields_updated = 0; + + if (stream->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) + mavlink_msg_highres_imu_send(stream->mavlink->get_chan(), sensor->timestamp, + sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2], + sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2], + sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2], + sensor->baro_pres_mbar, sensor->differential_pressure_pa, + sensor->baro_alt_meter, sensor->baro_temp_celcius, + fields_updated); +} + //void //MavlinkOrbListener::l_sensor_combined(const struct listener *l) //{ diff --git a/src/modules/mavlink/mavlink_messages.h b/src/modules/mavlink/mavlink_messages.h index 78279c08f0..a6326bad1a 100644 --- a/src/modules/mavlink/mavlink_messages.h +++ b/src/modules/mavlink/mavlink_messages.h @@ -23,6 +23,6 @@ extern struct msgs_list_s msgs_list[]; static void msg_heartbeat(const MavlinkStream *stream); static void msg_sys_status(const MavlinkStream *stream); - +static void msg_highres_imu(const MavlinkStream *stream); #endif /* MAVLINK_MESSAGES_H_ */ From dac0427b602be5de9058e78851a9182fd236ce0a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 25 Feb 2014 17:17:47 -0800 Subject: [PATCH 094/381] fw_att_pos_estimator: Removed unused code --- .../fw_att_pos_estimator_main.cpp | 97 +------------------ 1 file changed, 1 insertion(+), 96 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 38162959d3..bb3541a396 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -790,6 +790,7 @@ FixedwingEstimator::task_main() RecallStates(statesAtPosTime, (IMUmsec - msecPosDelay)); // run the fusion step FuseVelposNED(); + } else { fuseVelData = false; fusePosData = false; @@ -829,102 +830,6 @@ FixedwingEstimator::task_main() fuseVtasData = false; } - // if (hrt_elapsed_time(&start_time) > 100000 && !_initialized && (GPSstatus == 3)) { - // InitialiseFilter(velNED); - // _initialized = true; - - // warnx("init done."); - // } - - // if (_initialized) { - - // /* predict states and covariances */ - - // /* run the strapdown INS every sensor update */ - // UpdateStrapdownEquationsNED(); - - // /* store the predictions */ - // StoreStates(IMUmsec); - - // /* evaluate if on ground */ - // // OnGroundCheck(); - // onGround = false; - - // /* prepare the delta angles and time used by the covariance prediction */ - // summedDelAng = summedDelAng + correctedDelAng; - // summedDelVel = summedDelVel + correctedDelVel; - - // dt += dtIMU; - - // /* predict the covairance if the total delta angle has exceeded the threshold - // * or the time limit will be exceeded on the next measurement update - // */ - // if ((dt >= (covTimeStepMax - dtIMU)) || (summedDelAng.length() > covDelAngMax)) { - // CovariancePrediction(dt); - // summedDelAng = summedDelAng.zero(); - // summedDelVel = summedDelVel.zero(); - // dt = 0.0f; - // } - - // } - - - // if (gps_updated && _initialized) { - - // /* convert GPS measurements to horizontal NE, altitude and 3D velocity NED */ - // calcposNED(posNED, gpsLat, gpsLon, gpsHgt, latRef, lonRef, hgtRef); - - // posNE[0] = posNED[0]; - // posNE[1] = posNED[1]; - - // // set flags for further processing - // fuseVelData = true; - // fusePosData = true; - - // /* recall states after adjusting for delays */ - // RecallStates(statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); - // RecallStates(statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); - - // /* run the actual fusion */ - // FuseVelposNED(); - // } else { - // fuseVelData = false; - // fusePosData = false; - // } - - // if (baro_updated && _initialized) { - - // fuseHgtData = true; - // // recall states stored at time of measurement after adjusting for delays - // RecallStates(statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms)); - // // run the fusion step - // FuseVelposNED(); - // } else { - // fuseHgtData = false; - // } - - // if (mag_updated && _initialized) { - // fuseMagData = true; - // RecallStates(statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); - - // } else { - // fuseMagData = false; - // } - - // if (_initialized) { - // FuseMagnetometer(); - // } - - // if (airspeed_updated && _initialized - // && _airspeed.true_airspeed_m_s > 6.0f /* XXX magic number */) { - - // fuseVtasData = true; - // RecallStates(statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data - // FuseAirspeed(); - // } else { - // fuseVtasData = false; - // } - // Publish results if (_initialized) { From 7310fd608500be69153c5d033f74b056f1bb986e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 26 Feb 2014 21:28:35 +0400 Subject: [PATCH 095/381] mavlink: use inherited classes instead of callbacks for mavlink messages formatting, fixes and cleanup --- src/modules/mavlink/mavlink_main.cpp | 202 ++++---- src/modules/mavlink/mavlink_main.h | 16 +- src/modules/mavlink/mavlink_messages.cpp | 440 +++++++++++------- src/modules/mavlink/mavlink_messages.h | 15 +- .../mavlink/mavlink_orb_subscription.cpp | 36 +- .../mavlink/mavlink_orb_subscription.h | 17 +- src/modules/mavlink/mavlink_receiver.cpp | 2 +- src/modules/mavlink/mavlink_stream.cpp | 49 +- src/modules/mavlink/mavlink_stream.h | 29 +- 9 files changed, 444 insertions(+), 362 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 1563a257b9..33d81729c4 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -152,12 +152,6 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length static void usage(void); -namespace mavlink -{ - - Mavlink *g_mavlink; -} - Mavlink::Mavlink() : device_name("/dev/ttyS1"), _task_should_exit(false), @@ -166,14 +160,13 @@ Mavlink::Mavlink() : thread_running(false), _mavlink_task(-1), _mavlink_incoming_fd(-1), - -/* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "mavlink")), _mavlink_hil_enabled(false), _subscriptions(nullptr), _streams(nullptr), + mission_pub(-1), - mission_pub(-1) +/* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "mavlink")) { wpm = &wpm_s; fops.ioctl = (int (*)(file*, int, long unsigned int))&mavlink_dev_ioctl; @@ -204,12 +197,14 @@ Mavlink::~Mavlink() } } -void Mavlink::set_mode(enum MAVLINK_MODE mode) +void +Mavlink::set_mode(enum MAVLINK_MODE mode) { _mode = mode; } -int Mavlink::instance_count() +int +Mavlink::instance_count() { /* note: a local buffer count will help if this ever is called often */ Mavlink* inst = ::_head; @@ -222,10 +217,11 @@ int Mavlink::instance_count() return inst_index; } -Mavlink* Mavlink::new_instance() +Mavlink * +Mavlink::new_instance() { - Mavlink* inst = new Mavlink(); - Mavlink* next = ::_head; + Mavlink *inst = new Mavlink(); + Mavlink *next = ::_head; /* create the first instance at _head */ if (::_head == nullptr) { @@ -241,9 +237,10 @@ Mavlink* Mavlink::new_instance() return inst; } -Mavlink* Mavlink::get_instance(unsigned instance) +Mavlink * +Mavlink::get_instance(unsigned instance) { - Mavlink* inst = ::_head; + Mavlink *inst = ::_head; unsigned inst_index = 0; while (inst->_next != nullptr && inst_index < instance) { inst = inst->_next; @@ -257,7 +254,8 @@ Mavlink* Mavlink::get_instance(unsigned instance) return inst; } -int Mavlink::destroy_all_instances() +int +Mavlink::destroy_all_instances() { /* start deleting from the end */ Mavlink *inst_to_del = nullptr; @@ -294,7 +292,8 @@ int Mavlink::destroy_all_instances() return OK; } -bool Mavlink::instance_exists(const char *device_name, Mavlink *self) +bool +Mavlink::instance_exists(const char *device_name, Mavlink *self) { Mavlink* inst = ::_head; while (inst != nullptr) { @@ -307,7 +306,8 @@ bool Mavlink::instance_exists(const char *device_name, Mavlink *self) return false; } -int Mavlink::get_uart_fd(unsigned index) +int +Mavlink::get_uart_fd(unsigned index) { Mavlink* inst = get_instance(index); if (inst) @@ -316,14 +316,16 @@ int Mavlink::get_uart_fd(unsigned index) return -1; } -int Mavlink::get_uart_fd() +int +Mavlink::get_uart_fd() { return _uart; } -int Mavlink::get_channel() +mavlink_channel_t +Mavlink::get_channel() { - return (int)_chan; + return _chan; } /**************************************************************************** @@ -1364,7 +1366,7 @@ MavlinkOrbSubscription *Mavlink::add_orb_subscription(const struct orb_metadata MavlinkOrbSubscription *sub; LL_FOREACH(_subscriptions, sub) { - if (sub->topic == topic) { + if (sub->get_topic() == topic) { /* already subscribed */ return sub; } @@ -1379,26 +1381,19 @@ MavlinkOrbSubscription *Mavlink::add_orb_subscription(const struct orb_metadata } int -Mavlink::add_stream(const char *stream_name, const unsigned int interval) +Mavlink::add_stream(const char *stream_name, const float rate) { - uintptr_t arg = 0; - - unsigned int i = 0; - /* search for message with specified name */ - while (msgs_list[i].name != nullptr) { - if (strcmp(stream_name, msgs_list[i].name) == 0) { - /* count topics, array is nullptr-terminated */ - unsigned int topics_n; - for (topics_n = 0; topics_n < MAX_TOPICS_PER_MAVLINK_STREAM; topics_n++) { - if (msgs_list[i].topics[topics_n] == nullptr) { - break; - } - } - MavlinkStream *stream = new MavlinkStream(this, msgs_list[i].callback, topics_n, msgs_list[i].topics, msgs_list[i].sizes, arg, interval); + /* search for stream with specified name */ + for (unsigned int i = 0; streams_list[i] != nullptr; i++) { + if (strcmp(stream_name, streams_list[i]->get_name()) == 0) { + /* create stream copy for each mavlink instance */ + MavlinkStream *stream = streams_list[i]->new_instance(); + stream->set_channel(get_channel()); + stream->set_interval(1000.0f / rate); + stream->subscribe(this); LL_APPEND(_streams, stream); return OK; } - i++; } return ERROR; } @@ -1407,7 +1402,7 @@ int Mavlink::task_main(int argc, char *argv[]) { /* inform about start */ - warnx("Initializing.."); + warnx("start"); fflush(stdout); /* initialize mavlink text message buffering */ @@ -1463,9 +1458,6 @@ Mavlink::task_main(int argc, char *argv[]) bool usb_uart; - /* print welcome text */ - warnx("MAVLink v1.0 serial interface starting..."); - /* inform about mode */ switch (_mode) { case MODE_TX_HEARTBEAT_ONLY: @@ -1510,13 +1502,13 @@ Mavlink::task_main(int argc, char *argv[]) err(1, "could not open %s", device_name); /* create the device node that's used for sending text log messages, etc. */ - register_driver(MAVLINK_LOG_DEVICE, &fops, 0666, NULL); + if (instance_count() == 1) { + register_driver(MAVLINK_LOG_DEVICE, &fops, 0666, NULL); + } /* initialize logging device */ _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - mavlink_log_info(_mavlink_fd, "[mavlink] started"); - /* Initialize system properties */ mavlink_update_system(); @@ -1537,8 +1529,10 @@ Mavlink::task_main(int argc, char *argv[]) MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update), sizeof(parameter_update_s)); MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status), sizeof(vehicle_status_s)); - struct vehicle_status_s *status = (struct vehicle_status_s *) status_sub->data; + struct vehicle_status_s *status = (struct vehicle_status_s *) status_sub->get_data(); + warnx("started"); + mavlink_log_info(_mavlink_fd, "[mavlink] started"); /* add default streams, intervals depend on baud rate */ // if (_baudrate >= 230400) { @@ -1546,15 +1540,15 @@ Mavlink::task_main(int argc, char *argv[]) // } else if (_baudrate >= 57600) { // } - add_stream("HEARTBEAT", 1000); - add_stream("SYS_STATUS", 1000); - add_stream("HIGHRES_IMU", 300); - add_stream("RAW_IMU", 300); - add_stream("ATTITUDE", 200); - add_stream("NAMED_VALUE_FLOAT", 200); - add_stream("SERVO_OUTPUT_RAW", 500); - add_stream("GPS_RAW_INT", 500); - add_stream("MANUAL_CONTROL", 500); + add_stream("HEARTBEAT", 1.0f); + add_stream("SYS_STATUS", 1.0f); + add_stream("HIGHRES_IMU", 20.0f); +// add_stream("RAW_IMU", 10.0f); + add_stream("ATTITUDE", 20.0f); +// add_stream("NAMED_VALUE_FLOAT", 5.0f); +// add_stream("SERVO_OUTPUT_RAW", 2.0f); +// add_stream("GPS_RAW_INT", 2.0f); +// add_stream("MANUAL_CONTROL", 2.0f); while (!_task_should_exit) { /* main loop */ @@ -1604,31 +1598,24 @@ Mavlink::task_main(int argc, char *argv[]) mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission); } - mavlink_waypoint_eventloop(hrt_absolute_time()); - - /* check if waypoint has been reached against the last positions */ - mavlink_waypoint_eventloop(hrt_absolute_time()); - if (lowspeed_counter % (50000 / MAIN_LOOP_DELAY) == 0) { /* send parameters at 20 Hz (if queued for sending) */ mavlink_pm_queued_send(); mavlink_waypoint_eventloop(hrt_absolute_time()); - mavlink_waypoint_eventloop(hrt_absolute_time()); - if (_baudrate > 57600) { mavlink_pm_queued_send(); } - /* send one string at 10 Hz */ - if (!mavlink_logbuffer_is_empty(&lb)) { + /* send one string at 20 Hz */ + if (!mavlink_logbuffer_is_empty(&lb)) { struct mavlink_logmessage msg; int lb_ret = mavlink_logbuffer_read(&lb, &msg); if (lb_ret == OK) { mavlink_missionlib_send_gcs_string(msg.text); } - } + } } perf_end(_loop_perf); @@ -1636,7 +1623,6 @@ Mavlink::task_main(int argc, char *argv[]) /* wait for threads to complete */ pthread_join(receive_thread, NULL); - pthread_join(uorb_receive_thread, NULL); /* Reset the UART flags to original state */ tcsetattr(_uart, TCSANOW, &uart_config_original); @@ -1654,15 +1640,50 @@ Mavlink::task_main(int argc, char *argv[]) int Mavlink::start_helper(int argc, char *argv[]) { - // Create the instance in task context + /* create the instance in task context */ Mavlink *instance = Mavlink::new_instance(); - // This will actually only return once MAVLink exits + /* this will actually only return once MAVLink exits */ return instance->task_main(argc, argv); } int -Mavlink::start() +Mavlink::start(int argc, char *argv[]) { + // Instantiate thread + char buf[32]; + sprintf(buf, "mavlink_if%d", Mavlink::instance_count()); + + /*mavlink->_mavlink_task = */task_spawn_cmd(buf, + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 4096, + (main_t)&Mavlink::start_helper, + (const char **)argv); + + // while (!this->is_running()) { + // usleep(200); + // } + + // if (mavlink->_mavlink_task < 0) { + // warn("task start failed"); + // return -errno; + // } + + // if (mavlink::g_mavlink != nullptr) { + // errx(1, "already running"); + // } + + // mavlink::g_mavlink = new Mavlink; + + // if (mavlink::g_mavlink == nullptr) { + // errx(1, "alloc failed"); + // } + + // if (OK != mavlink::g_mavlink->start()) { + // delete mavlink::g_mavlink; + // mavlink::g_mavlink = nullptr; + // err(1, "start failed"); + // } return OK; } @@ -1685,44 +1706,7 @@ int mavlink_main(int argc, char *argv[]) } if (!strcmp(argv[1], "start")) { - - // Instantiate thread - char buf[32]; - sprintf(buf, "mavlink_if%d", Mavlink::instance_count()); - - /*mavlink->_mavlink_task = */task_spawn_cmd(buf, - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 2048, - (main_t)&Mavlink::start_helper, - (const char **)argv); - - // while (!this->is_running()) { - // usleep(200); - // } - - // if (mavlink->_mavlink_task < 0) { - // warn("task start failed"); - // return -errno; - // } - - // if (mavlink::g_mavlink != nullptr) { - // errx(1, "already running"); - // } - - // mavlink::g_mavlink = new Mavlink; - - // if (mavlink::g_mavlink == nullptr) { - // errx(1, "alloc failed"); - // } - - // if (OK != mavlink::g_mavlink->start()) { - // delete mavlink::g_mavlink; - // mavlink::g_mavlink = nullptr; - // err(1, "start failed"); - // } - - return 0; + return Mavlink::start(argc, argv); } else if (!strcmp(argv[1], "stop")) { warnx("mavlink stop is deprecated, use stop-all instead"); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 56d262000b..e7f3486da8 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -142,7 +142,7 @@ public: * * @return OK on success. */ - static int start(); + static int start(int argc, char *argv[]); /** * Display the mavlink status. @@ -163,8 +163,6 @@ public: int get_uart_fd(); - int get_channel(); - const char *device_name; enum MAVLINK_MODE { @@ -205,16 +203,11 @@ public: MavlinkOrbSubscription *add_orb_subscription(const struct orb_metadata *topic, size_t size); - mavlink_channel_t get_chan() { return _chan; } + mavlink_channel_t get_channel(); bool _task_should_exit; /**< if true, mavlink task should exit */ protected: - /** - * Pointer to the default cdev file operations table; useful for - * registering clone devices etc. - */ - Mavlink* _next; private: @@ -234,7 +227,7 @@ private: orb_advert_t mission_pub; struct mission_s mission; - uint8_t missionlib_msg_buf[300]; //XXX MAGIC NUMBER + uint8_t missionlib_msg_buf[sizeof(mavlink_message_t)]; MAVLINK_MODE _mode; uint8_t _mavlink_wpm_comp_id; @@ -247,7 +240,6 @@ private: unsigned int total_counter; pthread_t receive_thread; - pthread_t uorb_receive_thread; /* Allocate storage space for waypoints */ mavlink_wpm_storage wpm_s; @@ -326,7 +318,7 @@ private: int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); - int add_stream(const char *stream_name, const unsigned int interval); + int add_stream(const char *stream_name, const float rate); static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 7a56c36a56..df73581f05 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -12,195 +12,289 @@ #include "mavlink_messages.h" - -struct msgs_list_s msgs_list[] = { - { - .name = "HEARTBEAT", - .callback = msg_heartbeat, - .topics = { ORB_ID(vehicle_status), ORB_ID(position_setpoint_triplet), nullptr }, - .sizes = { sizeof(struct vehicle_status_s), sizeof(struct position_setpoint_triplet_s) } - }, - { - .name = "SYS_STATUS", - .callback = msg_sys_status, - .topics = { ORB_ID(vehicle_status), nullptr }, - .sizes = { sizeof(struct vehicle_status_s) } - }, - { .name = nullptr } -}; - -void -msg_heartbeat(const MavlinkStream *stream) -{ - struct vehicle_status_s *status = (struct vehicle_status_s *)stream->subscriptions[0]->data; - struct position_setpoint_triplet_s *pos_sp_triplet = (struct position_setpoint_triplet_s *)stream->subscriptions[1]->data; - - uint8_t mavlink_state = 0; - uint8_t mavlink_base_mode = 0; - uint32_t mavlink_custom_mode = 0; - - /* HIL */ - if (status->hil_state == HIL_STATE_ON) { - mavlink_base_mode |= MAV_MODE_FLAG_HIL_ENABLED; +class MavlinkStreamHeartbeat : public MavlinkStream { +public: + const char *get_name() + { + return "HEARTBEAT"; } - /* arming state */ - if (status->arming_state == ARMING_STATE_ARMED - || status->arming_state == ARMING_STATE_ARMED_ERROR) { - mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; + MavlinkStream *new_instance() + { + return new MavlinkStreamHeartbeat(); } - /* main state */ - mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; - union px4_custom_mode custom_mode; - custom_mode.data = 0; - if (pos_sp_triplet->nav_state == NAV_STATE_NONE) { - /* use main state when navigator is not active */ - if (status->main_state == MAIN_STATE_MANUAL) { - mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; - } else if (status->main_state == MAIN_STATE_SEATBELT) { - mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT; - } else if (status->main_state == MAIN_STATE_EASY) { - mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY; - } else if (status->main_state == MAIN_STATE_AUTO) { +private: + MavlinkOrbSubscription *status_sub; + MavlinkOrbSubscription *pos_sp_triplet_sub; + + struct vehicle_status_s *status; + struct position_setpoint_triplet_s *pos_sp_triplet; + +protected: + void subscribe(Mavlink *mavlink) + { + status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status), sizeof(struct vehicle_status_s)); + status = (struct vehicle_status_s *)status_sub->get_data(); + + pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet), sizeof(position_setpoint_triplet_s)); + pos_sp_triplet = (struct position_setpoint_triplet_s *)status_sub->get_data(); + } + + void send(const hrt_abstime t) { + status_sub->update(t); + pos_sp_triplet_sub->update(t); + + uint8_t mavlink_state = 0; + uint8_t mavlink_base_mode = 0; + uint32_t mavlink_custom_mode = 0; + + /* HIL */ + if (status->hil_state == HIL_STATE_ON) { + mavlink_base_mode |= MAV_MODE_FLAG_HIL_ENABLED; + } + + /* arming state */ + if (status->arming_state == ARMING_STATE_ARMED + || status->arming_state == ARMING_STATE_ARMED_ERROR) { + mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; + } + + /* main state */ + mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; + union px4_custom_mode custom_mode; + custom_mode.data = 0; + if (pos_sp_triplet->nav_state == NAV_STATE_NONE) { + /* use main state when navigator is not active */ + if (status->main_state == MAIN_STATE_MANUAL) { + mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; + } else if (status->main_state == MAIN_STATE_SEATBELT) { + mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT; + } else if (status->main_state == MAIN_STATE_EASY) { + mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY; + } else if (status->main_state == MAIN_STATE_AUTO) { + mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; + } + } else { + /* use navigation state when navigator is active */ mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; + if (pos_sp_triplet->nav_state == NAV_STATE_READY) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; + } else if (pos_sp_triplet->nav_state == NAV_STATE_LOITER) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; + } else if (pos_sp_triplet->nav_state == NAV_STATE_MISSION) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; + } else if (pos_sp_triplet->nav_state == NAV_STATE_RTL) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; + } else if (pos_sp_triplet->nav_state == NAV_STATE_LAND) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; + } } - } else { - /* use navigation state when navigator is active */ - mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - if (pos_sp_triplet->nav_state == NAV_STATE_READY) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; - } else if (pos_sp_triplet->nav_state == NAV_STATE_LOITER) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; - } else if (pos_sp_triplet->nav_state == NAV_STATE_MISSION) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; - } else if (pos_sp_triplet->nav_state == NAV_STATE_RTL) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; - } else if (pos_sp_triplet->nav_state == NAV_STATE_LAND) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; - } - } - mavlink_custom_mode = custom_mode.data; + mavlink_custom_mode = custom_mode.data; - /* set system state */ - if (status->arming_state == ARMING_STATE_INIT - || status->arming_state == ARMING_STATE_IN_AIR_RESTORE - || status->arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review - mavlink_state = MAV_STATE_UNINIT; - } else if (status->arming_state == ARMING_STATE_ARMED) { - mavlink_state = MAV_STATE_ACTIVE; - } else if (status->arming_state == ARMING_STATE_ARMED_ERROR) { - mavlink_state = MAV_STATE_CRITICAL; - } else if (status->arming_state == ARMING_STATE_STANDBY) { - mavlink_state = MAV_STATE_STANDBY; - } else if (status->arming_state == ARMING_STATE_REBOOT) { - mavlink_state = MAV_STATE_POWEROFF; - } else { - mavlink_state = MAV_STATE_CRITICAL; + /* set system state */ + if (status->arming_state == ARMING_STATE_INIT + || status->arming_state == ARMING_STATE_IN_AIR_RESTORE + || status->arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review + mavlink_state = MAV_STATE_UNINIT; + } else if (status->arming_state == ARMING_STATE_ARMED) { + mavlink_state = MAV_STATE_ACTIVE; + } else if (status->arming_state == ARMING_STATE_ARMED_ERROR) { + mavlink_state = MAV_STATE_CRITICAL; + } else if (status->arming_state == ARMING_STATE_STANDBY) { + mavlink_state = MAV_STATE_STANDBY; + } else if (status->arming_state == ARMING_STATE_REBOOT) { + mavlink_state = MAV_STATE_POWEROFF; + } else { + mavlink_state = MAV_STATE_CRITICAL; + } + + mavlink_msg_heartbeat_send(_channel, + mavlink_system.type, + MAV_AUTOPILOT_PX4, + mavlink_base_mode, + mavlink_custom_mode, + mavlink_state); + + } +}; + + +class MavlinkStreamSysStatus : public MavlinkStream { +public: + const char *get_name() + { + return "SYS_STATUS"; } - /* send heartbeat */ - mavlink_msg_heartbeat_send(stream->mavlink->get_chan(), - mavlink_system.type, - MAV_AUTOPILOT_PX4, - mavlink_base_mode, - mavlink_custom_mode, - mavlink_state); -} + MavlinkStream *new_instance() + { + return new MavlinkStreamSysStatus(); + } + +private: + MavlinkOrbSubscription *status_sub; + + struct vehicle_status_s *status; + +protected: + + void subscribe(Mavlink *mavlink) + { + status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status), sizeof(struct vehicle_status_s)); + status = (struct vehicle_status_s *)status_sub->get_data(); + } + + void send(const hrt_abstime t) { + status_sub->update(t); + + mavlink_msg_sys_status_send(_channel, + status->onboard_control_sensors_present, + status->onboard_control_sensors_enabled, + status->onboard_control_sensors_health, + status->load * 1000.0f, + status->battery_voltage * 1000.0f, + status->battery_current * 1000.0f, + status->battery_remaining, + status->drop_rate_comm, + status->errors_comm, + status->errors_count1, + status->errors_count2, + status->errors_count3, + status->errors_count4); + } +}; + + +class MavlinkStreamHighresIMU : public MavlinkStream { +public: + const char *get_name() + { + return "HIGHRES_IMU"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamHighresIMU(); + } + +private: + MavlinkOrbSubscription *sensor_sub; + + struct sensor_combined_s *sensor; + + uint32_t accel_counter = 0; + uint32_t gyro_counter = 0; + uint32_t mag_counter = 0; + uint32_t baro_counter = 0; + +protected: + + void subscribe(Mavlink *mavlink) + { + sensor_sub = mavlink->add_orb_subscription(ORB_ID(sensor_combined), sizeof(struct sensor_combined_s)); + sensor = (struct sensor_combined_s *)sensor_sub->get_data(); + } + + void send(const hrt_abstime t) { + sensor_sub->update(t); + + uint16_t fields_updated = 0; + + if (accel_counter != sensor->accelerometer_counter) { + /* mark first three dimensions as changed */ + fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); + accel_counter = sensor->accelerometer_counter; + } + + if (gyro_counter != sensor->gyro_counter) { + /* mark second group dimensions as changed */ + fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); + gyro_counter = sensor->gyro_counter; + } + + if (mag_counter != sensor->magnetometer_counter) { + /* mark third group dimensions as changed */ + fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); + mag_counter = sensor->magnetometer_counter; + } + + if (baro_counter != sensor->baro_counter) { + /* mark last group dimensions as changed */ + fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); + baro_counter = sensor->baro_counter; + } + + mavlink_msg_highres_imu_send(_channel, + sensor->timestamp, + sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2], + sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2], + sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2], + sensor->baro_pres_mbar, sensor->differential_pressure_pa, + sensor->baro_alt_meter, sensor->baro_temp_celcius, + fields_updated); + } +}; + + +class MavlinkStreamAttitude : public MavlinkStream { +public: + const char *get_name() + { + return "ATTITUDE"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamAttitude(); + } + +private: + MavlinkOrbSubscription *att_sub; + + struct vehicle_attitude_s *att; + +protected: + + void subscribe(Mavlink *mavlink) + { + att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude), sizeof(struct vehicle_attitude_s)); + att = (struct vehicle_attitude_s *)att_sub->get_data(); + } + + void send(const hrt_abstime t) { + att_sub->update(t); + + mavlink_msg_attitude_send(_channel, + att->timestamp / 1000, + att->roll, att->pitch, att->yaw, + att->rollspeed, att->pitchspeed, att->yawspeed); + } +}; + + +MavlinkStream *streams_list[] = { + new MavlinkStreamHeartbeat(), + new MavlinkStreamSysStatus(), + new MavlinkStreamHighresIMU(), + new MavlinkStreamAttitude(), + nullptr +}; + + + -void -msg_sys_status(const MavlinkStream *stream) -{ - struct vehicle_status_s *status = (struct vehicle_status_s *)stream->subscriptions[0]->data; - mavlink_msg_sys_status_send(stream->mavlink->get_chan(), - status->onboard_control_sensors_present, - status->onboard_control_sensors_enabled, - status->onboard_control_sensors_health, - status->load * 1000.0f, - status->battery_voltage * 1000.0f, - status->battery_current * 1000.0f, - status->battery_remaining, - status->drop_rate_comm, - status->errors_comm, - status->errors_count1, - status->errors_count2, - status->errors_count3, - status->errors_count4); -} -void -msg_highres_imu(const MavlinkStream *stream) -{ - struct sensor_combined_s *sensor = (struct sensor_combined_s *)stream->subscriptions[0]->data; - uint16_t fields_updated = 0; - if (stream->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) - mavlink_msg_highres_imu_send(stream->mavlink->get_chan(), sensor->timestamp, - sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2], - sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2], - sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2], - sensor->baro_pres_mbar, sensor->differential_pressure_pa, - sensor->baro_alt_meter, sensor->baro_temp_celcius, - fields_updated); -} -//void -//MavlinkOrbListener::l_sensor_combined(const struct listener *l) -//{ -// struct sensor_combined_s raw; -// -// /* copy sensors raw data into local buffer */ -// orb_copy(ORB_ID(sensor_combined), l->mavlink->get_subs()->sensor_sub, &raw); -// -// /* mark individual fields as _mavlink->get_chan()ged */ -// uint16_t fields_updated = 0; -// -// // if (accel_counter != raw.accelerometer_counter) { -// // /* mark first three dimensions as _mavlink->get_chan()ged */ -// // fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); -// // accel_counter = raw.accelerometer_counter; -// // } -// -// // if (gyro_counter != raw.gyro_counter) { -// // /* mark second group dimensions as _mavlink->get_chan()ged */ -// // fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); -// // gyro_counter = raw.gyro_counter; -// // } -// -// // if (mag_counter != raw.magnetometer_counter) { -// // /* mark third group dimensions as _mavlink->get_chan()ged */ -// // fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); -// // mag_counter = raw.magnetometer_counter; -// // } -// -// // if (baro_counter != raw.baro_counter) { -// // /* mark last group dimensions as _mavlink->get_chan()ged */ -// // fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); -// // baro_counter = raw.baro_counter; -// // } -// -// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) -// mavlink_msg_highres_imu_send(l->mavlink->get_chan(), l->listener->last_sensor_timestamp, -// raw.accelerometer_m_s2[0], raw.accelerometer_m_s2[1], -// raw.accelerometer_m_s2[2], raw.gyro_rad_s[0], -// raw.gyro_rad_s[1], raw.gyro_rad_s[2], -// raw.magnetometer_ga[0], -// raw.magnetometer_ga[1], raw.magnetometer_ga[2], -// raw.baro_pres_mbar, raw.differential_pressure_pa, -// raw.baro_alt_meter, raw.baro_temp_celcius, -// fields_updated); -// -// l->listener->sensors_raw_counter++; -//} -// //void //MavlinkOrbListener::l_vehicle_attitude(const struct listener *l) //{ diff --git a/src/modules/mavlink/mavlink_messages.h b/src/modules/mavlink/mavlink_messages.h index a6326bad1a..3dc6cb6998 100644 --- a/src/modules/mavlink/mavlink_messages.h +++ b/src/modules/mavlink/mavlink_messages.h @@ -10,19 +10,6 @@ #include "mavlink_stream.h" -#define MAX_TOPICS_PER_MAVLINK_STREAM 4 - -struct msgs_list_s { - char *name; - void (*callback)(const MavlinkStream *); - const struct orb_metadata *topics[MAX_TOPICS_PER_MAVLINK_STREAM+1]; - size_t sizes[MAX_TOPICS_PER_MAVLINK_STREAM+1]; -}; - -extern struct msgs_list_s msgs_list[]; - -static void msg_heartbeat(const MavlinkStream *stream); -static void msg_sys_status(const MavlinkStream *stream); -static void msg_highres_imu(const MavlinkStream *stream); +extern MavlinkStream *streams_list[]; #endif /* MAVLINK_MESSAGES_H_ */ diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp index 84ac11483f..b504b6955b 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.cpp +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -13,28 +13,40 @@ #include "mavlink_orb_subscription.h" -MavlinkOrbSubscription::MavlinkOrbSubscription(const struct orb_metadata *topic, size_t size) +MavlinkOrbSubscription::MavlinkOrbSubscription(const struct orb_metadata *topic, size_t size) : _topic(topic), _last_check(0), next(nullptr) { - this->topic = topic; - this->data = malloc(size); - memset(this->data, 0, size); - this->fd = orb_subscribe(topic); - this->last_update = 0; + _data = malloc(size); + memset(_data, 0, size); + _fd = orb_subscribe(_topic); } MavlinkOrbSubscription::~MavlinkOrbSubscription() { - close(fd); - free(data); + close(_fd); + free(_data); } -bool MavlinkOrbSubscription::update(const hrt_abstime t) +const struct orb_metadata * +MavlinkOrbSubscription::get_topic() { - if (last_update != t) { + return _topic; +} + +void * +MavlinkOrbSubscription::get_data() +{ + return _data; +} + +bool +MavlinkOrbSubscription::update(const hrt_abstime t) +{ + if (_last_check != t) { + _last_check = t; bool updated; - orb_check(fd, &updated); + orb_check(_fd, &updated); if (updated) { - orb_copy(topic, fd, data); + orb_copy(_topic, _fd, _data); return true; } } diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h index 9a7340e9bb..c38a9cc431 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.h +++ b/src/modules/mavlink/mavlink_orb_subscription.h @@ -11,18 +11,23 @@ #include #include + class MavlinkOrbSubscription { public: - MavlinkOrbSubscription(const struct orb_metadata *meta, size_t size); + MavlinkOrbSubscription *next; + + MavlinkOrbSubscription(const struct orb_metadata *topic, size_t size); ~MavlinkOrbSubscription(); bool update(const hrt_abstime t); + void *get_data(); + const struct orb_metadata *get_topic(); - const struct orb_metadata *topic; - int fd; - void *data; - hrt_abstime last_update; - MavlinkOrbSubscription *next; +private: + const struct orb_metadata *_topic; + int _fd; + void *_data; + hrt_abstime _last_check; }; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index d07de0f22f..b828420e68 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -818,7 +818,7 @@ MavlinkReceiver::receive_thread(void *arg) /* if read failed, this loop won't execute */ for (ssize_t i = 0; i < nread; i++) { - if (mavlink_parse_char(_mavlink->get_chan(), buf[i], &msg, &status)) { + if (mavlink_parse_char(_mavlink->get_channel(), buf[i], &msg, &status)) { /* handle generic messages and commands */ handle_message(&msg); diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index 9df4263ee2..16407366e4 100644 --- a/src/modules/mavlink/mavlink_stream.cpp +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -10,39 +10,42 @@ #include "mavlink_stream.h" #include "mavlink_main.h" -MavlinkStream::MavlinkStream(Mavlink *mavlink, void (*callback)(const MavlinkStream *), const unsigned int subs_n, const struct orb_metadata **topics, const size_t *sizes, const uintptr_t arg, const unsigned int interval) +MavlinkStream::MavlinkStream() : _interval(1000), _last_sent(0), _channel(MAVLINK_COMM_0), next(nullptr) { - this->callback = callback; - this->arg = arg; - this->interval = interval * 1000; - this->mavlink = mavlink; - this->subscriptions_n = subs_n; - this->subscriptions = (MavlinkOrbSubscription **) malloc(subs_n * sizeof(MavlinkOrbSubscription *)); - - for (int i = 0; i < subs_n; i++) { - this->subscriptions[i] = mavlink->add_orb_subscription(topics[i], sizes[i]); - } } MavlinkStream::~MavlinkStream() { - free(subscriptions); } /** - * Update mavlink stream, i.e. update subscriptions and send message if necessary + * Set messages interval in ms */ -int MavlinkStream::update(const hrt_abstime t) +void +MavlinkStream::set_interval(const unsigned int interval) { - uint64_t dt = t - last_sent; - if (dt > 0 && dt >= interval) { - /* interval expired, update all subscriptions */ - for (unsigned int i = 0; i < subscriptions_n; i++) { - subscriptions[i]->update(t); - } + _interval = interval * 1000; +} - /* format and send mavlink message */ - callback(this); - last_sent = t; +/** + * Set mavlink channel + */ +void +MavlinkStream::set_channel(mavlink_channel_t channel) +{ + _channel = channel; +} + +/** + * Update subscriptions and send message if necessary + */ +int +MavlinkStream::update(const hrt_abstime t) +{ + uint64_t dt = t - _last_sent; + if (dt > 0 && dt >= _interval) { + /* interval expired, send message */ + send(t); + _last_sent = (t / _interval) * _interval; } } diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index c3e60917ef..9f175adbe5 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -15,22 +15,27 @@ class MavlinkStream; #include "mavlink_main.h" -class MavlinkOrbSubscription; - class MavlinkStream { -public: - void (*callback)(const MavlinkStream *); - uintptr_t arg; - unsigned int subscriptions_n; - MavlinkOrbSubscription **subscriptions; - hrt_abstime last_sent; - unsigned int interval; - MavlinkStream *next; - Mavlink *mavlink; +private: + hrt_abstime _last_sent; - MavlinkStream(Mavlink *mavlink, void (*callback)(const MavlinkStream *), const unsigned int subs_n, const struct orb_metadata **topics, const size_t *sizes, const uintptr_t arg, const unsigned int interval); +protected: + mavlink_channel_t _channel; + unsigned int _interval; + + virtual void send(const hrt_abstime t) = 0; + +public: + MavlinkStream *next; + + MavlinkStream(); ~MavlinkStream(); + void set_interval(const unsigned int interval); + void set_channel(mavlink_channel_t channel); int update(const hrt_abstime t); + virtual MavlinkStream *new_instance() = 0; + virtual void subscribe(Mavlink *mavlink) = 0; + virtual const char *get_name() = 0; }; From 2967763b160bb12b8e13e72219fcb689da015ab5 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 26 Feb 2014 22:13:49 +0400 Subject: [PATCH 096/381] mavlink: heartbeat message bug fixed --- 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 df73581f05..0880e8285b 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -38,7 +38,7 @@ protected: status = (struct vehicle_status_s *)status_sub->get_data(); pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet), sizeof(position_setpoint_triplet_s)); - pos_sp_triplet = (struct position_setpoint_triplet_s *)status_sub->get_data(); + pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data(); } void send(const hrt_abstime t) { @@ -62,6 +62,7 @@ protected: /* main state */ mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; + union px4_custom_mode custom_mode; custom_mode.data = 0; if (pos_sp_triplet->nav_state == NAV_STATE_NONE) { @@ -96,7 +97,6 @@ protected: custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; } } - mavlink_custom_mode = custom_mode.data; /* set system state */ if (status->arming_state == ARMING_STATE_INIT @@ -119,7 +119,7 @@ protected: mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_base_mode, - mavlink_custom_mode, + custom_mode.data, mavlink_state); } From f9619e39341dbc45683c6ccd7e06397f8a537216 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 26 Feb 2014 22:46:33 +0400 Subject: [PATCH 097/381] geo: _wrap_XXX minor fix --- src/lib/geo/geo.c | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 9b3e202e6e..59c04f0e39 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -441,14 +441,14 @@ __EXPORT float _wrap_pi(float bearing) } int c = 0; - while (bearing > M_PI_F) { + while (bearing >= M_PI_F) { bearing -= M_TWOPI_F; if (c++ > 3) return NAN; } c = 0; - while (bearing <= -M_PI_F) { + while (bearing < -M_PI_F) { bearing += M_TWOPI_F; if (c++ > 3) return NAN; @@ -465,14 +465,14 @@ __EXPORT float _wrap_2pi(float bearing) } int c = 0; - while (bearing > M_TWOPI_F) { + while (bearing >= M_TWOPI_F) { bearing -= M_TWOPI_F; if (c++ > 3) return NAN; } c = 0; - while (bearing <= 0.0f) { + while (bearing < 0.0f) { bearing += M_TWOPI_F; if (c++ > 3) return NAN; @@ -489,14 +489,14 @@ __EXPORT float _wrap_180(float bearing) } int c = 0; - while (bearing > 180.0f) { + while (bearing >= 180.0f) { bearing -= 360.0f; if (c++ > 3) return NAN; } c = 0; - while (bearing <= -180.0f) { + while (bearing < -180.0f) { bearing += 360.0f; if (c++ > 3) return NAN; @@ -513,14 +513,14 @@ __EXPORT float _wrap_360(float bearing) } int c = 0; - while (bearing > 360.0f) { + while (bearing >= 360.0f) { bearing -= 360.0f; if (c++ > 3) return NAN; } c = 0; - while (bearing <= 0.0f) { + while (bearing < 0.0f) { bearing += 360.0f; if (c++ > 3) return NAN; From 85dc422d9804c894009e37c6eaab67a10c5dca28 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 26 Feb 2014 22:47:19 +0400 Subject: [PATCH 098/381] mavlink: more streams implemented, stack size returned to 2048 --- src/modules/mavlink/mavlink_main.cpp | 26 ++- src/modules/mavlink/mavlink_messages.cpp | 204 +++++++++++++++++------ 2 files changed, 164 insertions(+), 66 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 33d81729c4..5ac76e1cf0 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -61,14 +61,6 @@ #include #include #include -#include -#include -#include -#include -#include -#include -#include -#include #include #include @@ -79,6 +71,10 @@ #include #include +#include +#include +#include + #include "mavlink_bridge_header.h" #include "mavlink_main.h" #include "mavlink_messages.h" @@ -1542,13 +1538,11 @@ Mavlink::task_main(int argc, char *argv[]) add_stream("HEARTBEAT", 1.0f); add_stream("SYS_STATUS", 1.0f); - add_stream("HIGHRES_IMU", 20.0f); -// add_stream("RAW_IMU", 10.0f); - add_stream("ATTITUDE", 20.0f); -// add_stream("NAMED_VALUE_FLOAT", 5.0f); -// add_stream("SERVO_OUTPUT_RAW", 2.0f); -// add_stream("GPS_RAW_INT", 2.0f); -// add_stream("MANUAL_CONTROL", 2.0f); + add_stream("HIGHRES_IMU", 1.0f); + add_stream("ATTITUDE", 10.0f); + add_stream("GPS_RAW_INT", 1.0f); + add_stream("GLOBAL_POSITION_INT", 5.0f); + add_stream("LOCAL_POSITION_NED", 5.0f); while (!_task_should_exit) { /* main loop */ @@ -1656,7 +1650,7 @@ Mavlink::start(int argc, char *argv[]) /*mavlink->_mavlink_task = */task_spawn_cmd(buf, SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 4096, + 2048, (main_t)&Mavlink::start_helper, (const char **)argv); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 0880e8285b..27ae197a65 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -6,12 +6,32 @@ */ #include +#include +#include +#include +#include #include #include +#include +#include #include "mavlink_messages.h" + +uint16_t +cm_uint16_from_m_float(float m) +{ + if (m < 0.0f) { + return 0; + + } else if (m > 655.35f) { + return 65535; + } + + return (uint16_t)(m * 100.0f); +} + class MavlinkStreamHeartbeat : public MavlinkStream { public: const char *get_name() @@ -279,11 +299,145 @@ protected: }; +class MavlinkStreamGPSRawInt : public MavlinkStream { +public: + const char *get_name() + { + return "GPS_RAW_INT"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamGPSRawInt(); + } + +private: + MavlinkOrbSubscription *gps_sub; + + struct vehicle_gps_position_s *gps; + +protected: + + void subscribe(Mavlink *mavlink) + { + gps_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position), sizeof(struct vehicle_gps_position_s)); + gps = (struct vehicle_gps_position_s *)gps_sub->get_data(); + } + + void send(const hrt_abstime t) { + gps_sub->update(t); + + mavlink_msg_gps_raw_int_send(_channel, + gps->timestamp_position, + gps->fix_type, + gps->lat, + gps->lon, + gps->alt, + cm_uint16_from_m_float(gps->eph_m), + cm_uint16_from_m_float(gps->epv_m), + gps->vel_m_s * 100.0f, + _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f, + gps->satellites_visible); + } +}; + + +class MavlinkStreamGlobalPositionInt : public MavlinkStream { +public: + const char *get_name() + { + return "GLOBAL_POSITION_INT"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamGlobalPositionInt(); + } + +private: + MavlinkOrbSubscription *pos_sub; + MavlinkOrbSubscription *home_sub; + + struct vehicle_global_position_s *pos; + struct home_position_s *home; + +protected: + + void subscribe(Mavlink *mavlink) + { + pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position), sizeof(struct vehicle_global_position_s)); + pos = (struct vehicle_global_position_s *)pos_sub->get_data(); + + home_sub = mavlink->add_orb_subscription(ORB_ID(home_position), sizeof(struct home_position_s)); + home = (struct home_position_s *)home_sub->get_data(); + } + + void send(const hrt_abstime t) { + pos_sub->update(t); + home_sub->update(t); + + mavlink_msg_global_position_int_send(_channel, + pos->timestamp / 1000, + pos->lat * 1e7, + pos->lon * 1e7, + pos->alt * 1000.0f, + (pos->alt - home->alt) * 1000.0f, + pos->vel_n * 100.0f, + pos->vel_e * 100.0f, + pos->vel_d * 100.0f, + _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f); + } +}; + + +class MavlinkStreamLocalPositionNED : public MavlinkStream { +public: + const char *get_name() + { + return "LOCAL_POSITION_NED"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamLocalPositionNED(); + } + +private: + MavlinkOrbSubscription *pos_sub; + + struct vehicle_local_position_s *pos; + +protected: + + void subscribe(Mavlink *mavlink) + { + pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position), sizeof(struct vehicle_local_position_s)); + pos = (struct vehicle_local_position_s *)pos_sub->get_data(); + } + + void send(const hrt_abstime t) { + pos_sub->update(t); + + mavlink_msg_local_position_ned_send(_channel, + pos->timestamp / 1000, + pos->x, + pos->y, + pos->z, + pos->vx, + pos->vy, + pos->vz); + } +}; + + MavlinkStream *streams_list[] = { new MavlinkStreamHeartbeat(), new MavlinkStreamSysStatus(), new MavlinkStreamHighresIMU(), new MavlinkStreamAttitude(), + new MavlinkStreamGPSRawInt(), + new MavlinkStreamGlobalPositionInt(), + new MavlinkStreamLocalPositionNED(), nullptr }; @@ -347,22 +501,6 @@ MavlinkStream *streams_list[] = { // /* copy gps data into local buffer */ // orb_copy(ORB_ID(vehicle_gps_position), l->mavlink->get_subs()->gps_sub, &gps); // -// /* GPS COG is 0..2PI in degrees * 1e2 */ -// float cog_deg = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F; -// -// /* GPS position */ -// mavlink_msg_gps_raw_int_send(l->mavlink->get_chan(), -// gps.timestamp_position, -// gps.fix_type, -// gps.lat, -// gps.lon, -// gps.alt, -// cm_uint16_from_m_float(gps.eph_m), -// cm_uint16_from_m_float(gps.epv_m), -// gps.vel_m_s * 1e2f, // from m/s to cm/s -// cog_deg * 1e2f, // from deg to deg * 100 -// gps.satellites_visible); -// // /* update SAT info every 10 seconds */ // if (gps.satellite_info_available && (l->listener->gps_counter % 50 == 0)) { // mavlink_msg_gps_status_send(l->mavlink->get_chan(), @@ -414,40 +552,6 @@ MavlinkStream *streams_list[] = { // } //} // -//void -//MavlinkOrbListener::l_global_position(const struct listener *l) -//{ -// /* copy global position data into local buffer */ -// orb_copy(ORB_ID(vehicle_global_position), l->mavlink->get_subs()->global_pos_sub, &l->listener->global_pos); -// -// mavlink_msg_global_position_int_send(l->mavlink->get_chan(), -// l->listener->global_pos.timestamp / 1000, -// l->listener->global_pos.lat * 1e7, -// l->listener->global_pos.lon * 1e7, -// l->listener->global_pos.alt * 1000.0f, -// (l->listener->global_pos.alt - l->listener->home.alt) * 1000.0f, -// l->listener->global_pos.vel_n * 100.0f, -// l->listener->global_pos.vel_e * 100.0f, -// l->listener->global_pos.vel_d * 100.0f, -// _wrap_2pi(l->listener->global_pos.yaw) * M_RAD_TO_DEG_F * 100.0f); -//} -// -//void -//MavlinkOrbListener::l_local_position(const struct listener *l) -//{ -// /* copy local position data into local buffer */ -// orb_copy(ORB_ID(vehicle_local_position), l->mavlink->get_subs()->local_pos_sub, &l->listener->local_pos); -// -// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) -// mavlink_msg_local_position_ned_send(l->mavlink->get_chan(), -// l->listener->local_pos.timestamp / 1000, -// l->listener->local_pos.x, -// l->listener->local_pos.y, -// l->listener->local_pos.z, -// l->listener->local_pos.vx, -// l->listener->local_pos.vy, -// l->listener->local_pos.vz); -//} // //void //MavlinkOrbListener::l_global_position_setpoint(const struct listener *l) From 18f72f8dd7a3bd4a9af00fe28afd29e0cc65e2e9 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 26 Feb 2014 23:02:53 +0400 Subject: [PATCH 099/381] mavlink: GPS_GLOBAL_ORIGIN message added, set message rate depending on baudrate --- src/modules/mavlink/mavlink_main.cpp | 21 +++++----- src/modules/mavlink/mavlink_messages.cpp | 53 +++++++++++++++++------- 2 files changed, 48 insertions(+), 26 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 5ac76e1cf0..3102c937da 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1531,18 +1531,19 @@ Mavlink::task_main(int argc, char *argv[]) mavlink_log_info(_mavlink_fd, "[mavlink] started"); /* add default streams, intervals depend on baud rate */ -// if (_baudrate >= 230400) { -// } else if (_baudrate >= 115200) { -// } else if (_baudrate >= 57600) { -// } + float rate_mult = _baudrate / 57600.0f; + if (rate_mult > 4.0f) { + rate_mult = 4.0f; + } add_stream("HEARTBEAT", 1.0f); - add_stream("SYS_STATUS", 1.0f); - add_stream("HIGHRES_IMU", 1.0f); - add_stream("ATTITUDE", 10.0f); - add_stream("GPS_RAW_INT", 1.0f); - add_stream("GLOBAL_POSITION_INT", 5.0f); - add_stream("LOCAL_POSITION_NED", 5.0f); + add_stream("SYS_STATUS", 1.0f * rate_mult); + add_stream("GPS_GLOBAL_ORIGIN", 0.5f * rate_mult); + add_stream("HIGHRES_IMU", 1.0f * rate_mult); + add_stream("ATTITUDE", 10.0f * rate_mult); + add_stream("GPS_RAW_INT", 1.0f * rate_mult); + add_stream("GLOBAL_POSITION_INT", 5.0f * rate_mult); + add_stream("LOCAL_POSITION_NED", 5.0f * rate_mult); while (!_task_should_exit) { /* main loop */ diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 27ae197a65..48443fbdc0 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -430,6 +430,42 @@ protected: }; +class MavlinkStreamGPSGlobalOrigin : public MavlinkStream { +public: + const char *get_name() + { + return "GPS_GLOBAL_ORIGIN"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamGPSGlobalOrigin(); + } + +private: + MavlinkOrbSubscription *home_sub; + + struct home_position_s *home; + +protected: + + void subscribe(Mavlink *mavlink) + { + home_sub = mavlink->add_orb_subscription(ORB_ID(home_position), sizeof(struct home_position_s)); + home = (struct home_position_s *)home_sub->get_data(); + } + + void send(const hrt_abstime t) { + home_sub->update(t); + + mavlink_msg_gps_global_origin_send(_channel, + (int32_t)(home->lat * 1e7), + (int32_t)(home->lon * 1e7), + (int32_t)(home->alt) * 1000.0f); + } +}; + + MavlinkStream *streams_list[] = { new MavlinkStreamHeartbeat(), new MavlinkStreamSysStatus(), @@ -438,6 +474,7 @@ MavlinkStream *streams_list[] = { new MavlinkStreamGPSRawInt(), new MavlinkStreamGlobalPositionInt(), new MavlinkStreamLocalPositionNED(), + new MavlinkStreamGPSGlobalOrigin(), nullptr }; @@ -517,14 +554,6 @@ MavlinkStream *streams_list[] = { // // //void -//MavlinkOrbListener::l_rc_channels(const struct listener *l) -//{ -// /* copy rc channels into local buffer */ -// orb_copy(ORB_ID(rc_channels), l->mavlink->get_subs()->rc_sub, &l->listener->rc); -// // XXX Add RC channels scaled message here -//} -// -//void //MavlinkOrbListener::l_input_rc(const struct listener *l) //{ // /* copy rc _mavlink->get_chan()nels into local buffer */ @@ -801,14 +830,6 @@ MavlinkStream *streams_list[] = { //} // //void -//MavlinkOrbListener::l_home(const struct listener *l) -//{ -// orb_copy(ORB_ID(home_position), l->mavlink->get_subs()->home_sub, &l->listener->home); -// -// mavlink_msg_gps_global_origin_send(l->mavlink->get_chan(), (int32_t)(l->listener->home.lat*1e7d), (int32_t)(l->listener->home.lon*1e7d), (int32_t)(l->listener->home.alt)*1e3f); -//} -// -//void //MavlinkOrbListener::l_airspeed(const struct listener *l) //{ // orb_copy(ORB_ID(airspeed), l->mavlink->get_subs()->airspeed_sub, &l->listener->airspeed); From 141982a3ac21e7a0437f1d7692e4024daf873c21 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 27 Feb 2014 13:54:55 +0400 Subject: [PATCH 100/381] mavlink: minor refactoring and cleanup, rate limiter class implemented, new messages added --- src/modules/mavlink/mavlink_main.cpp | 68 +-- src/modules/mavlink/mavlink_main.h | 9 +- src/modules/mavlink/mavlink_messages.cpp | 438 ++++++++++-------- .../mavlink/mavlink_orb_subscription.cpp | 2 +- .../mavlink/mavlink_orb_subscription.h | 4 +- src/modules/mavlink/mavlink_rate_limiter.cpp | 38 ++ src/modules/mavlink/mavlink_rate_limiter.h | 28 ++ src/modules/mavlink/mavlink_stream.cpp | 2 +- src/modules/mavlink/module.mk | 3 +- 9 files changed, 371 insertions(+), 221 deletions(-) create mode 100644 src/modules/mavlink/mavlink_rate_limiter.cpp create mode 100644 src/modules/mavlink/mavlink_rate_limiter.h diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 3102c937da..8a026742c3 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -79,6 +79,7 @@ #include "mavlink_main.h" #include "mavlink_messages.h" #include "mavlink_receiver.h" +#include "mavlink_rate_limiter.h" /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -321,7 +322,7 @@ Mavlink::get_uart_fd() mavlink_channel_t Mavlink::get_channel() { - return _chan; + return _channel; } /**************************************************************************** @@ -1321,7 +1322,7 @@ Mavlink::mavlink_missionlib_send_message(mavlink_message_t *msg) { uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg); - mavlink_send_uart_bytes(_chan, missionlib_msg_buf, len); + mavlink_send_uart_bytes(_channel, missionlib_msg_buf, len); } @@ -1356,7 +1357,7 @@ Mavlink::mavlink_missionlib_send_gcs_string(const char *string) } } -MavlinkOrbSubscription *Mavlink::add_orb_subscription(const struct orb_metadata *topic, const size_t size) +MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic, const size_t size) { /* check if already subscribed to this topic */ MavlinkOrbSubscription *sub; @@ -1406,7 +1407,7 @@ Mavlink::task_main(int argc, char *argv[]) int ch; _baudrate = 57600; - _chan = MAVLINK_COMM_0; + _channel = MAVLINK_COMM_0; _mode = MODE_OFFBOARD; @@ -1520,8 +1521,6 @@ Mavlink::task_main(int argc, char *argv[]) thread_running = true; - unsigned lowspeed_counter = 0; - MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update), sizeof(parameter_update_s)); MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status), sizeof(vehicle_status_s)); @@ -1530,20 +1529,42 @@ Mavlink::task_main(int argc, char *argv[]) warnx("started"); mavlink_log_info(_mavlink_fd, "[mavlink] started"); - /* add default streams, intervals depend on baud rate */ + /* add default streams depending on mode, intervals depend on baud rate */ float rate_mult = _baudrate / 57600.0f; if (rate_mult > 4.0f) { rate_mult = 4.0f; } add_stream("HEARTBEAT", 1.0f); - add_stream("SYS_STATUS", 1.0f * rate_mult); - add_stream("GPS_GLOBAL_ORIGIN", 0.5f * rate_mult); - add_stream("HIGHRES_IMU", 1.0f * rate_mult); - add_stream("ATTITUDE", 10.0f * rate_mult); - add_stream("GPS_RAW_INT", 1.0f * rate_mult); - add_stream("GLOBAL_POSITION_INT", 5.0f * rate_mult); - add_stream("LOCAL_POSITION_NED", 5.0f * rate_mult); + + switch(_mode) { + case MODE_OFFBOARD: + add_stream("SYS_STATUS", 1.0f * rate_mult); + add_stream("GPS_GLOBAL_ORIGIN", 0.5f * rate_mult); + add_stream("HIGHRES_IMU", 1.0f * rate_mult); + add_stream("ATTITUDE", 10.0f * rate_mult); + add_stream("GPS_RAW_INT", 1.0f * rate_mult); + add_stream("GLOBAL_POSITION_INT", 5.0f * rate_mult); + add_stream("LOCAL_POSITION_NED", 5.0f * rate_mult); + add_stream("SERVO_OUTPUT_RAW_0", 1.0f * rate_mult); + break; + + case MODE_HIL: + add_stream("SYS_STATUS", 1.0f * rate_mult); + add_stream("GPS_GLOBAL_ORIGIN", 0.5f * rate_mult); + add_stream("HIGHRES_IMU", 1.0f * rate_mult); + add_stream("ATTITUDE", 10.0f * rate_mult); + add_stream("GPS_RAW_INT", 1.0f * rate_mult); + add_stream("GLOBAL_POSITION_INT", 5.0f * rate_mult); + add_stream("LOCAL_POSITION_NED", 5.0f * rate_mult); + break; + + default: + break; + } + + MavlinkRateLimiter slow_rate_limiter(2000.0f / rate_mult); + MavlinkRateLimiter fast_rate_limiter(100.0f / rate_mult); while (!_task_should_exit) { /* main loop */ @@ -1571,13 +1592,6 @@ Mavlink::task_main(int argc, char *argv[]) stream->update(t); } - /* 0.5 Hz */ - if (lowspeed_counter % (2000000 / MAIN_LOOP_DELAY) == 0) { - mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission); - } - - lowspeed_counter++; - bool updated; orb_check(mission_result_sub, &updated); @@ -1591,18 +1605,16 @@ Mavlink::task_main(int argc, char *argv[]) } mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission); + } else { + if (slow_rate_limiter.check(t)) { + mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission); + } } - if (lowspeed_counter % (50000 / MAIN_LOOP_DELAY) == 0) { - /* send parameters at 20 Hz (if queued for sending) */ + if (fast_rate_limiter.check(t)) { mavlink_pm_queued_send(); mavlink_waypoint_eventloop(hrt_absolute_time()); - if (_baudrate > 57600) { - mavlink_pm_queued_send(); - } - - /* send one string at 20 Hz */ if (!mavlink_logbuffer_is_empty(&lb)) { struct mavlink_logmessage msg; int lb_ret = mavlink_logbuffer_read(&lb, &msg); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index e7f3486da8..506b4317ad 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -201,7 +201,7 @@ public: */ int set_hil_enabled(bool hil_enabled); - MavlinkOrbSubscription *add_orb_subscription(const struct orb_metadata *topic, size_t size); + MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic, size_t size); mavlink_channel_t get_channel(); @@ -231,10 +231,7 @@ private: MAVLINK_MODE _mode; uint8_t _mavlink_wpm_comp_id; - mavlink_channel_t _chan; - -// XXX probably should be in a header... -// extern pthread_t receive_start(int uart); + mavlink_channel_t _channel; struct mavlink_logbuffer lb; unsigned int total_counter; @@ -248,7 +245,7 @@ private: bool _verbose; int _uart; int _baudrate; - bool gcs_link; + /** * If the queue index is not at 0, the queue sending * logic will send parameters from the current index diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 48443fbdc0..9676068419 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -5,6 +5,7 @@ * Author: ton */ +#include #include #include @@ -19,6 +20,10 @@ #include "mavlink_messages.h" +static uint16_t cm_uint16_from_m_float(float m); +static void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, + uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode); + uint16_t cm_uint16_from_m_float(float m) { @@ -32,6 +37,83 @@ cm_uint16_from_m_float(float m) return (uint16_t)(m * 100.0f); } +void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, + uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode) +{ + *mavlink_state = 0; + *mavlink_base_mode = 0; + *mavlink_custom_mode = 0; + + /* HIL */ + if (status->hil_state == HIL_STATE_ON) { + *mavlink_base_mode |= MAV_MODE_FLAG_HIL_ENABLED; + } + + /* arming state */ + if (status->arming_state == ARMING_STATE_ARMED + || status->arming_state == ARMING_STATE_ARMED_ERROR) { + *mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; + } + + /* main state */ + *mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; + + union px4_custom_mode custom_mode; + custom_mode.data = 0; + if (pos_sp_triplet->nav_state == NAV_STATE_NONE) { + /* use main state when navigator is not active */ + if (status->main_state == MAIN_STATE_MANUAL) { + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; + } else if (status->main_state == MAIN_STATE_SEATBELT) { + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT; + } else if (status->main_state == MAIN_STATE_EASY) { + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY; + } else if (status->main_state == MAIN_STATE_AUTO) { + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; + } + } else { + /* use navigation state when navigator is active */ + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + if (pos_sp_triplet->nav_state == NAV_STATE_READY) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; + } else if (pos_sp_triplet->nav_state == NAV_STATE_LOITER) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; + } else if (pos_sp_triplet->nav_state == NAV_STATE_MISSION) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; + } else if (pos_sp_triplet->nav_state == NAV_STATE_RTL) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; + } else if (pos_sp_triplet->nav_state == NAV_STATE_LAND) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; + } + } + + *mavlink_custom_mode = custom_mode.data; + + /* set system state */ + if (status->arming_state == ARMING_STATE_INIT + || status->arming_state == ARMING_STATE_IN_AIR_RESTORE + || status->arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review + *mavlink_state = MAV_STATE_UNINIT; + } else if (status->arming_state == ARMING_STATE_ARMED) { + *mavlink_state = MAV_STATE_ACTIVE; + } else if (status->arming_state == ARMING_STATE_ARMED_ERROR) { + *mavlink_state = MAV_STATE_CRITICAL; + } else if (status->arming_state == ARMING_STATE_STANDBY) { + *mavlink_state = MAV_STATE_STANDBY; + } else if (status->arming_state == ARMING_STATE_REBOOT) { + *mavlink_state = MAV_STATE_POWEROFF; + } else { + *mavlink_state = MAV_STATE_CRITICAL; + } +} + + class MavlinkStreamHeartbeat : public MavlinkStream { public: const char *get_name() @@ -68,78 +150,13 @@ protected: uint8_t mavlink_state = 0; uint8_t mavlink_base_mode = 0; uint32_t mavlink_custom_mode = 0; - - /* HIL */ - if (status->hil_state == HIL_STATE_ON) { - mavlink_base_mode |= MAV_MODE_FLAG_HIL_ENABLED; - } - - /* arming state */ - if (status->arming_state == ARMING_STATE_ARMED - || status->arming_state == ARMING_STATE_ARMED_ERROR) { - mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; - } - - /* main state */ - mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; - - union px4_custom_mode custom_mode; - custom_mode.data = 0; - if (pos_sp_triplet->nav_state == NAV_STATE_NONE) { - /* use main state when navigator is not active */ - if (status->main_state == MAIN_STATE_MANUAL) { - mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; - } else if (status->main_state == MAIN_STATE_SEATBELT) { - mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT; - } else if (status->main_state == MAIN_STATE_EASY) { - mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY; - } else if (status->main_state == MAIN_STATE_AUTO) { - mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; - } - } else { - /* use navigation state when navigator is active */ - mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - if (pos_sp_triplet->nav_state == NAV_STATE_READY) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; - } else if (pos_sp_triplet->nav_state == NAV_STATE_LOITER) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; - } else if (pos_sp_triplet->nav_state == NAV_STATE_MISSION) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; - } else if (pos_sp_triplet->nav_state == NAV_STATE_RTL) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; - } else if (pos_sp_triplet->nav_state == NAV_STATE_LAND) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; - } - } - - /* set system state */ - if (status->arming_state == ARMING_STATE_INIT - || status->arming_state == ARMING_STATE_IN_AIR_RESTORE - || status->arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review - mavlink_state = MAV_STATE_UNINIT; - } else if (status->arming_state == ARMING_STATE_ARMED) { - mavlink_state = MAV_STATE_ACTIVE; - } else if (status->arming_state == ARMING_STATE_ARMED_ERROR) { - mavlink_state = MAV_STATE_CRITICAL; - } else if (status->arming_state == ARMING_STATE_STANDBY) { - mavlink_state = MAV_STATE_STANDBY; - } else if (status->arming_state == ARMING_STATE_REBOOT) { - mavlink_state = MAV_STATE_POWEROFF; - } else { - mavlink_state = MAV_STATE_CRITICAL; - } + get_mavlink_mode_state(status, pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); mavlink_msg_heartbeat_send(_channel, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_base_mode, - custom_mode.data, + mavlink_custom_mode, mavlink_state); } @@ -164,7 +181,6 @@ private: struct vehicle_status_s *status; protected: - void subscribe(Mavlink *mavlink) { status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status), sizeof(struct vehicle_status_s)); @@ -215,7 +231,6 @@ private: uint32_t baro_counter = 0; protected: - void subscribe(Mavlink *mavlink) { sensor_sub = mavlink->add_orb_subscription(ORB_ID(sensor_combined), sizeof(struct sensor_combined_s)); @@ -281,7 +296,6 @@ private: struct vehicle_attitude_s *att; protected: - void subscribe(Mavlink *mavlink) { att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude), sizeof(struct vehicle_attitude_s)); @@ -313,11 +327,9 @@ public: private: MavlinkOrbSubscription *gps_sub; - struct vehicle_gps_position_s *gps; protected: - void subscribe(Mavlink *mavlink) { gps_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position), sizeof(struct vehicle_gps_position_s)); @@ -356,13 +368,12 @@ public: private: MavlinkOrbSubscription *pos_sub; - MavlinkOrbSubscription *home_sub; - struct vehicle_global_position_s *pos; + + MavlinkOrbSubscription *home_sub; struct home_position_s *home; protected: - void subscribe(Mavlink *mavlink) { pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position), sizeof(struct vehicle_global_position_s)); @@ -404,11 +415,9 @@ public: private: MavlinkOrbSubscription *pos_sub; - struct vehicle_local_position_s *pos; protected: - void subscribe(Mavlink *mavlink) { pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position), sizeof(struct vehicle_local_position_s)); @@ -444,11 +453,9 @@ public: private: MavlinkOrbSubscription *home_sub; - struct home_position_s *home; protected: - void subscribe(Mavlink *mavlink) { home_sub = mavlink->add_orb_subscription(ORB_ID(home_position), sizeof(struct home_position_s)); @@ -466,6 +473,171 @@ protected: }; +class MavlinkStreamServoOutputRaw : public MavlinkStream { +public: + MavlinkStreamServoOutputRaw(unsigned int n) : MavlinkStream(), _n(n) + { + sprintf(_name, "SERVO_OUTPUT_RAW_%d", _n); + } + + const char *get_name() + { + return _name; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamServoOutputRaw(_n); + } + +private: + MavlinkOrbSubscription *act_sub; + struct actuator_outputs_s *act; + + char _name[20]; + unsigned int _n; + +protected: + void subscribe(Mavlink *mavlink) + { + orb_id_t act_topics[] = { + ORB_ID(actuator_outputs_0), + ORB_ID(actuator_outputs_1), + ORB_ID(actuator_outputs_2), + ORB_ID(actuator_outputs_3) + }; + + act_sub = mavlink->add_orb_subscription(act_topics[_n], sizeof(struct actuator_outputs_s)); + act = (struct actuator_outputs_s *)act_sub->get_data(); + } + + void send(const hrt_abstime t) { + act_sub->update(t); + + mavlink_msg_servo_output_raw_send(_channel, + act->timestamp / 1000, + _n, + act->output[0], + act->output[1], + act->output[2], + act->output[3], + act->output[4], + act->output[5], + act->output[6], + act->output[7]); + } +}; + + +class MavlinkStreamHILControls : public MavlinkStream { +public: + const char *get_name() + { + return "HIL_CONTROLS"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamHILControls(); + } + +private: + MavlinkOrbSubscription *status_sub; + struct vehicle_status_s *status; + + MavlinkOrbSubscription *pos_sp_triplet_sub; + struct position_setpoint_triplet_s *pos_sp_triplet; + + MavlinkOrbSubscription *act_sub; + struct actuator_outputs_s *act; + +protected: + void subscribe(Mavlink *mavlink) + { + status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status), sizeof(struct vehicle_status_s)); + status = (struct vehicle_status_s *)status_sub->get_data(); + + pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet), sizeof(position_setpoint_triplet_s)); + pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data(); + + act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0), sizeof(struct actuator_outputs_s)); + act = (struct actuator_outputs_s *)act_sub->get_data(); + } + + void send(const hrt_abstime t) { + status_sub->update(t); + pos_sp_triplet_sub->update(t); + act_sub->update(t); + + /* translate the current syste state to mavlink state and mode */ + uint8_t mavlink_state; + uint8_t mavlink_base_mode; + uint32_t mavlink_custom_mode; + get_mavlink_mode_state(status, pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); + + /* HIL message as per MAVLink spec */ + + /* scale / assign outputs depending on system type */ + if (mavlink_system.type == MAV_TYPE_QUADROTOR) { + mavlink_msg_hil_controls_send(_channel, + hrt_absolute_time(), + ((act->output[0] - 900.0f) / 600.0f) / 2.0f, + ((act->output[1] - 900.0f) / 600.0f) / 2.0f, + ((act->output[2] - 900.0f) / 600.0f) / 2.0f, + ((act->output[3] - 900.0f) / 600.0f) / 2.0f, + -1, + -1, + -1, + -1, + mavlink_base_mode, + 0); + + } else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) { + mavlink_msg_hil_controls_send(_channel, + hrt_absolute_time(), + ((act->output[0] - 900.0f) / 600.0f) / 2.0f, + ((act->output[1] - 900.0f) / 600.0f) / 2.0f, + ((act->output[2] - 900.0f) / 600.0f) / 2.0f, + ((act->output[3] - 900.0f) / 600.0f) / 2.0f, + ((act->output[4] - 900.0f) / 600.0f) / 2.0f, + ((act->output[5] - 900.0f) / 600.0f) / 2.0f, + -1, + -1, + mavlink_base_mode, + 0); + + } else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) { + mavlink_msg_hil_controls_send(_channel, + hrt_absolute_time(), + ((act->output[0] - 900.0f) / 600.0f) / 2.0f, + ((act->output[1] - 900.0f) / 600.0f) / 2.0f, + ((act->output[2] - 900.0f) / 600.0f) / 2.0f, + ((act->output[3] - 900.0f) / 600.0f) / 2.0f, + ((act->output[4] - 900.0f) / 600.0f) / 2.0f, + ((act->output[5] - 900.0f) / 600.0f) / 2.0f, + ((act->output[6] - 900.0f) / 600.0f) / 2.0f, + ((act->output[7] - 900.0f) / 600.0f) / 2.0f, + mavlink_base_mode, + 0); + + } else { + mavlink_msg_hil_controls_send(_channel, + hrt_absolute_time(), + (act->output[0] - 1500.0f) / 500.0f, + (act->output[1] - 1500.0f) / 500.0f, + (act->output[2] - 1500.0f) / 500.0f, + (act->output[3] - 1000.0f) / 1000.0f, + (act->output[4] - 1500.0f) / 500.0f, + (act->output[5] - 1500.0f) / 500.0f, + (act->output[6] - 1500.0f) / 500.0f, + (act->output[7] - 1500.0f) / 500.0f, + mavlink_base_mode, + 0); + } + } +}; + + MavlinkStream *streams_list[] = { new MavlinkStreamHeartbeat(), new MavlinkStreamSysStatus(), @@ -475,6 +647,11 @@ MavlinkStream *streams_list[] = { new MavlinkStreamGlobalPositionInt(), new MavlinkStreamLocalPositionNED(), new MavlinkStreamGPSGlobalOrigin(), + new MavlinkStreamServoOutputRaw(0), + new MavlinkStreamServoOutputRaw(1), + new MavlinkStreamServoOutputRaw(2), + new MavlinkStreamServoOutputRaw(3), + new MavlinkStreamHILControls(), nullptr }; @@ -484,8 +661,6 @@ MavlinkStream *streams_list[] = { - - //void //MavlinkOrbListener::l_vehicle_attitude(const struct listener *l) //{ @@ -652,107 +827,6 @@ MavlinkStream *streams_list[] = { //} // //void -//MavlinkOrbListener::l_actuator_outputs(const struct listener *l) -//{ -// struct actuator_outputs_s act_outputs; -// -// orb_id_t ids[] = { -// ORB_ID(actuator_outputs_0), -// ORB_ID(actuator_outputs_1), -// ORB_ID(actuator_outputs_2), -// ORB_ID(actuator_outputs_3) -// }; -// -// /* copy actuator data into local buffer */ -// orb_copy(ids[l->arg], *l->subp, &act_outputs); -// -// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) { -// mavlink_msg_servo_output_raw_send(l->mavlink->get_chan(), l->listener->last_sensor_timestamp / 1000, -// l->arg /* port number - needs GCS support */, -// /* QGC has port number support already */ -// act_outputs.output[0], -// act_outputs.output[1], -// act_outputs.output[2], -// act_outputs.output[3], -// act_outputs.output[4], -// act_outputs.output[5], -// act_outputs.output[6], -// act_outputs.output[7]); -// -// /* only send in HIL mode and only send first group for HIL */ -// if (l->mavlink->get_hil_enabled() && l->listener->armed.armed && ids[l->arg] == ORB_ID(actuator_outputs_0)) { -// -// /* translate the current syste state to mavlink state and mode */ -// uint8_t mavlink_state = 0; -// uint8_t mavlink_base_mode = 0; -// uint32_t mavlink_custom_mode = 0; -// l->mavlink->get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); -// -// /* HIL message as per MAVLink spec */ -// -// /* scale / assign outputs depending on system type */ -// -// if (mavlink_system.type == MAV_TYPE_QUADROTOR) { -// mavlink_msg_hil_controls_send(l->mavlink->get_chan(), -// hrt_absolute_time(), -// ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, -// ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, -// ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f, -// ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f, -// -1, -// -1, -// -1, -// -1, -// mavlink_base_mode, -// 0); -// -// } else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) { -// mavlink_msg_hil_controls_send(l->mavlink->get_chan(), -// hrt_absolute_time(), -// ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, -// ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, -// ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f, -// ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f, -// ((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f, -// ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f, -// -1, -// -1, -// mavlink_base_mode, -// 0); -// -// } else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) { -// mavlink_msg_hil_controls_send(l->mavlink->get_chan(), -// hrt_absolute_time(), -// ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f, -// ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f, -// ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f, -// ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f, -// ((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f, -// ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f, -// ((act_outputs.output[6] - 900.0f) / 600.0f) / 2.0f, -// ((act_outputs.output[7] - 900.0f) / 600.0f) / 2.0f, -// mavlink_base_mode, -// 0); -// -// } else { -// mavlink_msg_hil_controls_send(l->mavlink->get_chan(), -// hrt_absolute_time(), -// (act_outputs.output[0] - 1500.0f) / 500.0f, -// (act_outputs.output[1] - 1500.0f) / 500.0f, -// (act_outputs.output[2] - 1500.0f) / 500.0f, -// (act_outputs.output[3] - 1000.0f) / 1000.0f, -// (act_outputs.output[4] - 1500.0f) / 500.0f, -// (act_outputs.output[5] - 1500.0f) / 500.0f, -// (act_outputs.output[6] - 1500.0f) / 500.0f, -// (act_outputs.output[7] - 1500.0f) / 500.0f, -// mavlink_base_mode, -// 0); -// } -// } -// } -//} -// -//void //MavlinkOrbListener::l_actuator_armed(const struct listener *l) //{ // orb_copy(ORB_ID(actuator_armed), l->mavlink->get_subs()->armed_sub, &l->listener->armed); diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp index b504b6955b..35470a19a1 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.cpp +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -13,7 +13,7 @@ #include "mavlink_orb_subscription.h" -MavlinkOrbSubscription::MavlinkOrbSubscription(const struct orb_metadata *topic, size_t size) : _topic(topic), _last_check(0), next(nullptr) +MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic, size_t size) : _topic(topic), _last_check(0), next(nullptr) { _data = malloc(size); memset(_data, 0, size); diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h index c38a9cc431..79ff3abdbe 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.h +++ b/src/modules/mavlink/mavlink_orb_subscription.h @@ -16,7 +16,7 @@ class MavlinkOrbSubscription { public: MavlinkOrbSubscription *next; - MavlinkOrbSubscription(const struct orb_metadata *topic, size_t size); + MavlinkOrbSubscription(const orb_id_t topic, size_t size); ~MavlinkOrbSubscription(); bool update(const hrt_abstime t); @@ -24,7 +24,7 @@ public: const struct orb_metadata *get_topic(); private: - const struct orb_metadata *_topic; + const orb_id_t _topic; int _fd; void *_data; hrt_abstime _last_check; diff --git a/src/modules/mavlink/mavlink_rate_limiter.cpp b/src/modules/mavlink/mavlink_rate_limiter.cpp new file mode 100644 index 0000000000..f5bb06ccd2 --- /dev/null +++ b/src/modules/mavlink/mavlink_rate_limiter.cpp @@ -0,0 +1,38 @@ +/* + * mavlink_rate_limiter.cpp + * + * Created on: 26.02.2014 + * Author: ton + */ + + +#include "mavlink_rate_limiter.h" + +MavlinkRateLimiter::MavlinkRateLimiter() : _last_sent(0), _interval(1000000) +{ +} + +MavlinkRateLimiter::MavlinkRateLimiter(unsigned int interval) : _last_sent(0), _interval(interval * 1000) +{ +} + +MavlinkRateLimiter::~MavlinkRateLimiter() +{ +} + +void +MavlinkRateLimiter::set_interval(unsigned int interval) +{ + _interval = interval * 1000; +} + +bool +MavlinkRateLimiter::check(hrt_abstime t) +{ + uint64_t dt = t - _last_sent; + if (dt > 0 && dt >= _interval) { + _last_sent = (t / _interval) * _interval; + return true; + } + return false; +} diff --git a/src/modules/mavlink/mavlink_rate_limiter.h b/src/modules/mavlink/mavlink_rate_limiter.h new file mode 100644 index 0000000000..6db65f638f --- /dev/null +++ b/src/modules/mavlink/mavlink_rate_limiter.h @@ -0,0 +1,28 @@ +/* + * mavlink_rate_limiter.h + * + * Created on: 26.02.2014 + * Author: ton + */ + +#ifndef MAVLINK_RATE_LIMITER_H_ +#define MAVLINK_RATE_LIMITER_H_ + +#include + + +class MavlinkRateLimiter { +private: + hrt_abstime _last_sent; + hrt_abstime _interval; + +public: + MavlinkRateLimiter(); + MavlinkRateLimiter(unsigned int interval); + ~MavlinkRateLimiter(); + void set_interval(unsigned int interval); + bool check(hrt_abstime t); +}; + + +#endif /* MAVLINK_RATE_LIMITER_H_ */ diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index 16407366e4..703f74b4c3 100644 --- a/src/modules/mavlink/mavlink_stream.cpp +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -10,7 +10,7 @@ #include "mavlink_stream.h" #include "mavlink_main.h" -MavlinkStream::MavlinkStream() : _interval(1000), _last_sent(0), _channel(MAVLINK_COMM_0), next(nullptr) +MavlinkStream::MavlinkStream() : _interval(1000000), _last_sent(0), _channel(MAVLINK_COMM_0), next(nullptr) { } diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index 5ea6c0f46a..f09efa2e6c 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -41,6 +41,7 @@ SRCS += mavlink_main.cpp \ mavlink_receiver.cpp \ mavlink_orb_subscription.cpp \ mavlink_messages.cpp \ - mavlink_stream.cpp + mavlink_stream.cpp \ + mavlink_rate_limiter.cpp INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink From 2159f948ea81088076b440cd8b673b1ac9f70da3 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 27 Feb 2014 18:31:41 +0400 Subject: [PATCH 101/381] mavlink: -r (datarate) parameter implemented, minor fixes --- ROMFS/px4fmu_common/init.d/rc.usb | 2 +- ROMFS/px4fmu_common/init.d/rcS | 4 +- src/modules/mavlink/mavlink_main.cpp | 83 +++++++++++++++++------- src/modules/mavlink/mavlink_main.h | 3 +- src/modules/mavlink/mavlink_receiver.cpp | 3 +- 5 files changed, 65 insertions(+), 30 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index 3d8be089ee..558be42755 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -5,7 +5,7 @@ echo "Starting MAVLink on this USB console" -mavlink start -b 230400 -d /dev/ttyACM0 +mavlink start -r 10000 -d /dev/ttyACM0 # Exit shell to make it available to MAVLink exit diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 76f021e339..c3065b6fc8 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -390,14 +390,14 @@ then if [ $TTYS1_BUSY == yes ] then # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else - mavlink start -d /dev/ttyS0 + mavlink start -r 1000 -d /dev/ttyS0 usleep 5000 # Exit from nsh to free port for mavlink set EXIT_ON_END yes else # Start MAVLink on default port: ttyS1 - mavlink start + mavlink start -r 1000 usleep 5000 fi fi diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 8a026742c3..974fd27bf9 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -87,6 +87,7 @@ #endif static const int ERROR = -1; +#define MAX_DATA_RATE 10000 // max data rate in bytes/s #define MAIN_LOOP_DELAY 10000 // 100 Hz static Mavlink* _head = nullptr; @@ -449,7 +450,6 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * } /* open uart */ - warnx("UART is %s, baudrate is %d\n", uart_name, baud); _uart = open(uart_name, O_RDWR | O_NOCTTY); /* Try to set baud rate */ @@ -1407,6 +1407,7 @@ Mavlink::task_main(int argc, char *argv[]) int ch; _baudrate = 57600; + _datarate = 0; _channel = MAVLINK_COMM_0; _mode = MODE_OFFBOARD; @@ -1415,7 +1416,7 @@ Mavlink::task_main(int argc, char *argv[]) argc -= 2; argv += 2; - while ((ch = getopt(argc, argv, "b:d:eov")) != EOF) { + while ((ch = getopt(argc, argv, "b:r:d:m:v")) != EOF) { switch (ch) { case 'b': _baudrate = strtoul(optarg, NULL, 10); @@ -1425,6 +1426,14 @@ Mavlink::task_main(int argc, char *argv[]) break; + case 'r': + _datarate = strtoul(optarg, NULL, 10); + + if (_datarate < 10 || _datarate > MAX_DATA_RATE) + errx(1, "invalid data rate '%s'", optarg); + + break; + case 'd': device_name = optarg; break; @@ -1433,8 +1442,19 @@ Mavlink::task_main(int argc, char *argv[]) // mavlink_link_termination_allowed = true; // break; - case 'o': - _mode = MODE_ONBOARD; + case 'm': + if (strcmp(optarg, "offboard") == 0) { + _mode = MODE_OFFBOARD; + + } else if (strcmp(optarg, "onboard") == 0) { + _mode = MODE_ONBOARD; + + } else if (strcmp(optarg, "hil") == 0) { + _mode = MODE_HIL; + + } else if (strcmp(optarg, "custom") == 0) { + _mode = MODE_CUSTOM; + } break; case 'v': @@ -1447,51 +1467,61 @@ Mavlink::task_main(int argc, char *argv[]) } } + if (_datarate == 0) { + /* convert bits to bytes and use 1/2 of bandwidth by default */ + _datarate = _baudrate / 20; + } + + if (_datarate > MAX_DATA_RATE) { + _datarate = MAX_DATA_RATE; + } + if (Mavlink::instance_exists(device_name, this)) { errx(1, "mavlink instance for %s already running", device_name); } - struct termios uart_config_original; - - bool usb_uart; - /* inform about mode */ switch (_mode) { - case MODE_TX_HEARTBEAT_ONLY: - warnx("MODE_TX_HEARTBEAT_ONLY"); + case MODE_CUSTOM: + warnx("mode: CUSTOM"); break; case MODE_OFFBOARD: - warnx("MODE_OFFBOARD"); + warnx("mode: OFFBOARD"); break; case MODE_ONBOARD: - warnx("MODE_ONBOARD"); + warnx("mode: ONBOARD"); break; case MODE_HIL: - warnx("MODE_HIL"); + warnx("mode: HIL"); break; default: - warnx("Error: Unknown mode"); + warnx("ERROR: Unknown mode"); break; } switch(_mode) { case MODE_OFFBOARD: case MODE_HIL: + case MODE_CUSTOM: _mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER; break; case MODE_ONBOARD: _mavlink_wpm_comp_id = MAV_COMP_ID_CAMERA; break; - case MODE_TX_HEARTBEAT_ONLY: default: _mavlink_wpm_comp_id = MAV_COMP_ID_ALL; - warnx("Error: Unknown mode"); break; } - /* Flush stdout in case MAVLink is about to take it over */ + warnx("data rate: %d bytes/s", _datarate); + warnx("port: %s, baudrate: %d", device_name, _baudrate); + + /* flush stdout in case MAVLink is about to take it over */ fflush(stdout); + struct termios uart_config_original; + bool usb_uart; + /* default values for arguments */ _uart = mavlink_open_uart(_baudrate, device_name, &uart_config_original, &usb_uart); @@ -1529,17 +1559,14 @@ Mavlink::task_main(int argc, char *argv[]) warnx("started"); mavlink_log_info(_mavlink_fd, "[mavlink] started"); - /* add default streams depending on mode, intervals depend on baud rate */ - float rate_mult = _baudrate / 57600.0f; - if (rate_mult > 4.0f) { - rate_mult = 4.0f; - } + /* add default streams depending on mode and intervals depending on datarate */ + float rate_mult = _datarate / 1000.0f; add_stream("HEARTBEAT", 1.0f); switch(_mode) { case MODE_OFFBOARD: - add_stream("SYS_STATUS", 1.0f * rate_mult); + add_stream("SYS_STATUS", 1.0f); add_stream("GPS_GLOBAL_ORIGIN", 0.5f * rate_mult); add_stream("HIGHRES_IMU", 1.0f * rate_mult); add_stream("ATTITUDE", 10.0f * rate_mult); @@ -1550,13 +1577,14 @@ Mavlink::task_main(int argc, char *argv[]) break; case MODE_HIL: - add_stream("SYS_STATUS", 1.0f * rate_mult); + add_stream("SYS_STATUS", 1.0f); add_stream("GPS_GLOBAL_ORIGIN", 0.5f * rate_mult); add_stream("HIGHRES_IMU", 1.0f * rate_mult); add_stream("ATTITUDE", 10.0f * rate_mult); add_stream("GPS_RAW_INT", 1.0f * rate_mult); add_stream("GLOBAL_POSITION_INT", 5.0f * rate_mult); add_stream("LOCAL_POSITION_NED", 5.0f * rate_mult); + add_stream("HIL_CONTROLS", 20.0f * rate_mult); break; default: @@ -1703,7 +1731,12 @@ Mavlink::status() static void usage() { - errx(1, "usage: mavlink {start|stop-all} [-d device] [-b baudrate] [-o] [-v]"); + errx(1, "usage: mavlink {start|stop-all} [-d device] [-b baudrate] [-r datarate] [-m mode] [-v]\n\n" + "Supported modes (-m):\n" + "\toffboard\tSend standard telemetry data to ground station (default)\n" + "\tonboard\tOnboard comminication mode, e.g. to connect PX4FLOW\n" + "\thil\tHardware In the Loop mode, send telemetry and HIL_CONTROLS\n" + "\tcustom\tCustom configuration, don't send anything by default, streams can be enabled by 'mavlink stream' command\n"); } int mavlink_main(int argc, char *argv[]) diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 506b4317ad..afbf857877 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -166,7 +166,7 @@ public: const char *device_name; enum MAVLINK_MODE { - MODE_TX_HEARTBEAT_ONLY=0, + MODE_CUSTOM=0, MODE_OFFBOARD, MODE_ONBOARD, MODE_HIL @@ -245,6 +245,7 @@ private: bool _verbose; int _uart; int _baudrate; + int _datarate; /** * If the queue index is not at 0, the queue sending diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index b828420e68..a3546e954b 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -250,7 +250,8 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) if (mavlink_system.sysid < 4) { /* switch to a receiving link mode */ - _mavlink->set_mode(Mavlink::MODE_TX_HEARTBEAT_ONLY); + //TODO why do we need this? + //_mavlink->set_mode(Mavlink::MODE_TX_HEARTBEAT_ONLY); /* * rate control mode - defined by MAVLink From efca2d158aa34c912be91fb3229b211022ae0945 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 28 Feb 2014 00:45:59 +0400 Subject: [PATCH 102/381] mavlink: commanl line streams configuration implemented --- src/modules/mavlink/mavlink_main.cpp | 221 ++++++++++++++++++++------- src/modules/mavlink/mavlink_main.h | 12 +- 2 files changed, 175 insertions(+), 58 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 974fd27bf9..672daf641b 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -87,6 +87,7 @@ #endif static const int ERROR = -1; +#define DEFAULT_DEVICE_NAME "/dev/ttyS1" #define MAX_DATA_RATE 10000 // max data rate in bytes/s #define MAIN_LOOP_DELAY 10000 // 100 Hz @@ -151,9 +152,9 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length static void usage(void); Mavlink::Mavlink() : - device_name("/dev/ttyS1"), + device_name(DEFAULT_DEVICE_NAME), _task_should_exit(false), - _next(nullptr), + next(nullptr), _mavlink_fd(-1), thread_running(false), _mavlink_task(-1), @@ -208,7 +209,7 @@ Mavlink::instance_count() Mavlink* inst = ::_head; unsigned inst_index = 0; while (inst != nullptr) { - inst = inst->_next; + inst = inst->next; inst_index++; } @@ -226,11 +227,11 @@ Mavlink::new_instance() ::_head = inst; /* afterwards follow the next and append the instance */ } else { - while (next->_next != nullptr) { - next = next->_next; + while (next->next != nullptr) { + next = next->next; } /* now parent has a null pointer, fill it */ - next->_next = inst; + next->next = inst; } return inst; } @@ -240,8 +241,8 @@ Mavlink::get_instance(unsigned instance) { Mavlink *inst = ::_head; unsigned inst_index = 0; - while (inst->_next != nullptr && inst_index < instance) { - inst = inst->_next; + while (inst->next != nullptr && inst_index < instance) { + inst = inst->next; inst_index++; } @@ -252,6 +253,20 @@ Mavlink::get_instance(unsigned instance) return inst; } +Mavlink * +Mavlink::get_instance_for_device(const char *device_name) +{ + Mavlink *inst; + + LL_FOREACH(::_head, inst) { + if (strcmp(inst->device_name, device_name) == 0) { + return inst; + } + } + + return nullptr; +} + int Mavlink::destroy_all_instances() { @@ -265,7 +280,7 @@ Mavlink::destroy_all_instances() while (next_inst != nullptr) { inst_to_del = next_inst; - next_inst = inst_to_del->_next; + next_inst = inst_to_del->next; /* set flag to stop thread and wait for all threads to finish */ inst_to_del->_task_should_exit = true; @@ -299,7 +314,7 @@ Mavlink::instance_exists(const char *device_name, Mavlink *self) /* don't compare with itself */ if (inst != self && !strcmp(device_name, inst->device_name)) return true; - inst = inst->_next; + inst = inst->next; } return false; } @@ -348,7 +363,7 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) mavlink_logbuffer_write(&inst->lb, &msg); inst->total_counter++; - inst = inst->_next; + inst = inst->next; } return OK; @@ -1378,20 +1393,47 @@ MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic, cons } int -Mavlink::add_stream(const char *stream_name, const float rate) +Mavlink::configure_stream(const char *stream_name, const float rate) { - /* search for stream with specified name */ - for (unsigned int i = 0; streams_list[i] != nullptr; i++) { - if (strcmp(stream_name, streams_list[i]->get_name()) == 0) { - /* create stream copy for each mavlink instance */ - MavlinkStream *stream = streams_list[i]->new_instance(); - stream->set_channel(get_channel()); - stream->set_interval(1000.0f / rate); - stream->subscribe(this); - LL_APPEND(_streams, stream); + /* calculate interval in ms, 0 means disabled stream */ + unsigned int interval = (rate > 0.0f) ? (1000.0f / rate) : 0; + + /* search if stream exists */ + MavlinkStream *stream; + LL_FOREACH(_streams, stream) { + if (strcmp(stream_name, stream->get_name()) == 0) { + if (interval > 0) { + /* set new interval */ + stream->set_interval(interval); + + } else { + /* delete stream */ + LL_DELETE(_streams, stream); + delete stream; + } return OK; } } + + if (interval > 0) { + /* search for stream with specified name in supported streams list */ + for (unsigned int i = 0; streams_list[i] != nullptr; i++) { + if (strcmp(stream_name, streams_list[i]->get_name()) == 0) { + /* create new instance */ + stream = streams_list[i]->new_instance(); + stream->set_channel(get_channel()); + stream->set_interval(interval); + stream->subscribe(this); + LL_APPEND(_streams, stream); + return OK; + } + } + + } else { + /* stream not found, nothing to disable */ + return OK; + } + return ERROR; } @@ -1416,22 +1458,26 @@ Mavlink::task_main(int argc, char *argv[]) argc -= 2; argv += 2; + /* don't exit from getopt loop to leave getopt global variables in consistent state, + * set error flag instead */ + bool err_flag = false; + while ((ch = getopt(argc, argv, "b:r:d:m:v")) != EOF) { switch (ch) { case 'b': _baudrate = strtoul(optarg, NULL, 10); - - if (_baudrate < 9600 || _baudrate > 921600) - errx(1, "invalid baud rate '%s'", optarg); - + if (_baudrate < 9600 || _baudrate > 921600) { + warnx("invalid baud rate '%s'", optarg); + err_flag = true; + } break; case 'r': _datarate = strtoul(optarg, NULL, 10); - - if (_datarate < 10 || _datarate > MAX_DATA_RATE) - errx(1, "invalid data rate '%s'", optarg); - + if (_datarate < 10 || _datarate > MAX_DATA_RATE) { + warnx("invalid data rate '%s'", optarg); + err_flag = true; + } break; case 'd': @@ -1462,11 +1508,16 @@ Mavlink::task_main(int argc, char *argv[]) break; default: - usage(); + err_flag = true; break; } } + if (err_flag) { + usage(); + exit(1); + } + if (_datarate == 0) { /* convert bits to bytes and use 1/2 of bandwidth by default */ _datarate = _baudrate / 20; @@ -1562,29 +1613,29 @@ Mavlink::task_main(int argc, char *argv[]) /* add default streams depending on mode and intervals depending on datarate */ float rate_mult = _datarate / 1000.0f; - add_stream("HEARTBEAT", 1.0f); + configure_stream("HEARTBEAT", 1.0f); switch(_mode) { case MODE_OFFBOARD: - add_stream("SYS_STATUS", 1.0f); - add_stream("GPS_GLOBAL_ORIGIN", 0.5f * rate_mult); - add_stream("HIGHRES_IMU", 1.0f * rate_mult); - add_stream("ATTITUDE", 10.0f * rate_mult); - add_stream("GPS_RAW_INT", 1.0f * rate_mult); - add_stream("GLOBAL_POSITION_INT", 5.0f * rate_mult); - add_stream("LOCAL_POSITION_NED", 5.0f * rate_mult); - add_stream("SERVO_OUTPUT_RAW_0", 1.0f * rate_mult); + configure_stream("SYS_STATUS", 1.0f); + configure_stream("GPS_GLOBAL_ORIGIN", 0.5f * rate_mult); + configure_stream("HIGHRES_IMU", 1.0f * rate_mult); + configure_stream("ATTITUDE", 10.0f * rate_mult); + configure_stream("GPS_RAW_INT", 1.0f * rate_mult); + configure_stream("GLOBAL_POSITION_INT", 5.0f * rate_mult); + configure_stream("LOCAL_POSITION_NED", 5.0f * rate_mult); + configure_stream("SERVO_OUTPUT_RAW_0", 1.0f * rate_mult); break; case MODE_HIL: - add_stream("SYS_STATUS", 1.0f); - add_stream("GPS_GLOBAL_ORIGIN", 0.5f * rate_mult); - add_stream("HIGHRES_IMU", 1.0f * rate_mult); - add_stream("ATTITUDE", 10.0f * rate_mult); - add_stream("GPS_RAW_INT", 1.0f * rate_mult); - add_stream("GLOBAL_POSITION_INT", 5.0f * rate_mult); - add_stream("LOCAL_POSITION_NED", 5.0f * rate_mult); - add_stream("HIL_CONTROLS", 20.0f * rate_mult); + configure_stream("SYS_STATUS", 1.0f); + configure_stream("GPS_GLOBAL_ORIGIN", 0.5f * rate_mult); + configure_stream("HIGHRES_IMU", 1.0f * rate_mult); + configure_stream("ATTITUDE", 10.0f * rate_mult); + configure_stream("GPS_RAW_INT", 1.0f * rate_mult); + configure_stream("GLOBAL_POSITION_INT", 5.0f * rate_mult); + configure_stream("LOCAL_POSITION_NED", 5.0f * rate_mult); + configure_stream("HIL_CONTROLS", 20.0f * rate_mult); break; default: @@ -1726,17 +1777,76 @@ Mavlink::start(int argc, char *argv[]) void Mavlink::status() { - warnx("Running"); + warnx("running"); +} + +int +Mavlink::stream(int argc, char *argv[]) +{ + const char *device_name = DEFAULT_DEVICE_NAME; + float rate = -1.0f; + const char *stream_name = nullptr; + int ch; + + argc -= 1; + argv += 1; + + /* don't exit from getopt loop to leave getopt global variables in consistent state, + * set error flag instead */ + bool err_flag = false; + + while ((ch = getopt(argc, argv, "r:d:s:")) != EOF) { + switch (ch) { + case 'r': + rate = strtod(optarg, nullptr); + if (rate < 0.0f) { + err_flag = true; + } + break; + + case 'd': + device_name = optarg; + break; + + case 's': + stream_name = optarg; + break; + + default: + err_flag = true; + break; + } + } + + if (!err_flag && rate >= 0.0 && stream_name != nullptr) { + Mavlink *inst = get_instance_for_device(device_name); + if (inst != nullptr) { + if (OK == inst->configure_stream(stream_name, rate)) { + if (rate > 0.0f) { + warnx("stream %s on device %s enabled with rate %.1f Hz", stream_name, device_name, rate); + + } else { + warnx("stream %s on device %s disabled", stream_name, device_name); + } + + } else { + warnx("stream %s not found", stream_name, device_name); + } + + } else { + errx(1, "mavlink for device %s is not running", device_name); + } + + } else { + errx(1, "usage: mavlink stream [-d device] -s stream -r rate"); + } + + return OK; } static void usage() { - errx(1, "usage: mavlink {start|stop-all} [-d device] [-b baudrate] [-r datarate] [-m mode] [-v]\n\n" - "Supported modes (-m):\n" - "\toffboard\tSend standard telemetry data to ground station (default)\n" - "\tonboard\tOnboard comminication mode, e.g. to connect PX4FLOW\n" - "\thil\tHardware In the Loop mode, send telemetry and HIL_CONTROLS\n" - "\tcustom\tCustom configuration, don't send anything by default, streams can be enabled by 'mavlink stream' command\n"); + errx(1, "usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate] [-r rate] [-m mode] [-s stream] [-v]"); } int mavlink_main(int argc, char *argv[]) @@ -1758,6 +1868,9 @@ int mavlink_main(int argc, char *argv[]) // } else if (!strcmp(argv[1], "status")) { // mavlink::g_mavlink->status(); + } else if (!strcmp(argv[1], "stream")) { + return Mavlink::stream(argc, argv); + } else { usage(); } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index afbf857877..049f5fedd5 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -149,11 +149,15 @@ public: */ void status(); + static int stream(int argc, char *argv[]); + static int instance_count(); - static Mavlink* new_instance(); + static Mavlink *new_instance(); - static Mavlink* get_instance(unsigned instance); + static Mavlink *get_instance(unsigned instance); + + static Mavlink *get_instance_for_device(const char *device_name); static int destroy_all_instances(); @@ -208,7 +212,7 @@ public: bool _task_should_exit; /**< if true, mavlink task should exit */ protected: - Mavlink* _next; + Mavlink* next; private: int _mavlink_fd; @@ -316,7 +320,7 @@ private: int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); - int add_stream(const char *stream_name, const float rate); + int configure_stream(const char *stream_name, const float rate); static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); From 8783bed114e8ece85d62f15d0d2a06c28aa0bcfe Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 28 Feb 2014 00:49:12 +0400 Subject: [PATCH 103/381] sdlog2: getopt invalid usage fixed --- src/modules/sdlog2/sdlog2.c | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 68e6a7469f..f952c3f323 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -652,6 +652,10 @@ int sdlog2_thread_main(int argc, char *argv[]) argv += 2; int ch; + /* don't exit from getopt loop to leave getopt global variables in consistent state, + * set error flag instead */ + bool err_flag = false; + while ((ch = getopt(argc, argv, "r:b:eat")) != EOF) { switch (ch) { case 'r': { @@ -699,13 +703,20 @@ int sdlog2_thread_main(int argc, char *argv[]) } else { warnx("unknown option character `\\x%x'", optopt); } + err_flag = true; + break; default: - sdlog2_usage("unrecognized flag"); - errx(1, "exiting"); + warnx("unrecognized flag"); + err_flag = true; + break; } } + if (err_flag) { + sdlog2_usage(NULL); + } + gps_time = 0; /* create log root dir */ From d71396e556a4a729d7fdf14244c1592477cb399e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 27 Feb 2014 16:16:57 -0800 Subject: [PATCH 104/381] Move params to MAVLink params, enable mavlink text feedback. --- .../fw_att_pos_estimator_main.cpp | 52 ++++++++++--------- 1 file changed, 27 insertions(+), 25 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index bb3541a396..7e179d400f 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -77,6 +77,7 @@ #include #include #include +#include #include "estimator.h" @@ -174,18 +175,22 @@ private: bool _initialized; bool _gps_initialized; + int _mavlink_fd; + struct { - float throttle_cruise; - uint32_t vel_delay_ms; - uint32_t pos_delay_ms; - uint32_t height_delay_ms; - uint32_t mag_delay_ms; - uint32_t tas_delay_ms; + int32_t vel_delay_ms; + int32_t pos_delay_ms; + int32_t height_delay_ms; + int32_t mag_delay_ms; + int32_t tas_delay_ms; } _parameters; /**< local copies of interesting parameters */ struct { - param_t throttle_cruise; - + param_t vel_delay_ms; + param_t pos_delay_ms; + param_t height_delay_ms; + param_t mag_delay_ms; + param_t tas_delay_ms; } _parameter_handles; /**< handles for interesting parameters */ @@ -264,10 +269,17 @@ FixedwingEstimator::FixedwingEstimator() : /* states */ _initialized(false), - _gps_initialized(false) + _gps_initialized(false), + _mavlink_fd(-1) { - _parameter_handles.throttle_cruise = param_find("NAV_DUMMY"); + _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + + _parameter_handles.vel_delay_ms = param_find("PE_VEL_DELAY_MS"); + _parameter_handles.pos_delay_ms = param_find("PE_POS_DELAY_MS"); + _parameter_handles.height_delay_ms = param_find("PE_HGT_DELAY_MS"); + _parameter_handles.mag_delay_ms = param_find("PE_MAG_DELAY_MS"); + _parameter_handles.tas_delay_ms = param_find("PE_TAS_DELAY_MS"); /* fetch initial parameter values */ parameters_update(); @@ -327,14 +339,11 @@ int FixedwingEstimator::parameters_update() { - // XXX NEED TO GET HANDLES FIRST! NEEDS PARAM INIT - //param_get(_parameter_handles.throttle_cruise, &(_parameters.throttle_cruise)); - - _parameters.vel_delay_ms = 230; - _parameters.pos_delay_ms = 210; - _parameters.height_delay_ms = 350; - _parameters.mag_delay_ms = 30; - _parameters.tas_delay_ms = 210; + param_get(_parameter_handles.vel_delay_ms, &(_parameters.vel_delay_ms)); + param_get(_parameter_handles.pos_delay_ms, &(_parameters.pos_delay_ms)); + param_get(_parameter_handles.height_delay_ms, &(_parameters.height_delay_ms)); + param_get(_parameter_handles.mag_delay_ms, &(_parameters.mag_delay_ms)); + param_get(_parameter_handles.tas_delay_ms, &(_parameters.tas_delay_ms)); return OK; } @@ -361,13 +370,6 @@ FixedwingEstimator::task_main_trampoline(int argc, char *argv[]) float dt = 0.0f; // time lapsed since last covariance prediction -// Estimated time delays (msec) -uint32_t msecVelDelay = 230; -uint32_t msecPosDelay = 210; -uint32_t msecHgtDelay = 350; -uint32_t msecMagDelay = 30; -uint32_t msecTasDelay = 210; - void FixedwingEstimator::task_main() { From 2dd5636ee0763d261ad40330a42dd9a271accb9b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 27 Feb 2014 16:17:31 -0800 Subject: [PATCH 105/381] Add and enable all filter params. --- .../fw_att_pos_estimator_params.c | 67 ++++++++++++++++++- 1 file changed, 66 insertions(+), 1 deletion(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_params.c b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_params.c index 64e593a15f..6138ef39cd 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_params.c +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_params.c @@ -48,5 +48,70 @@ * */ -PARAM_DEFINE_FLOAT(NAV_DUMMY, 0.0f); +/** + * Velocity estimate delay + * + * The delay in milliseconds of the velocity estimate from GPS. + * + * @min 0 + * @max 1000 + * @group Position Estimator + */ +PARAM_DEFINE_INT32(PE_VEL_DELAY_MS, 230); + +/** + * Position estimate delay + * + * The delay in milliseconds of the position estimate from GPS. + * + * @min 0 + * @max 1000 + * @group Position Estimator + */ +PARAM_DEFINE_INT32(PE_POS_DELAY_MS, 210); + +/** + * Height estimate delay + * + * The delay in milliseconds of the height estimate from the barometer. + * + * @min 0 + * @max 1000 + * @group Position Estimator + */ +PARAM_DEFINE_INT32(PE_HGT_DELAY_MS, 350); + +/** + * Mag estimate delay + * + * The delay in milliseconds of the magnetic field estimate from + * the magnetometer. + * + * @min 0 + * @max 1000 + * @group Position Estimator + */ +PARAM_DEFINE_INT32(PE_MAG_DELAY_MS, 30); + +/** + * True airspeeed estimate delay + * + * The delay in milliseconds of the airspeed estimate. + * + * @min 0 + * @max 1000 + * @group Position Estimator + */ +PARAM_DEFINE_INT32(PE_TAS_DELAY_MS, 210); + +/** + * GPS vs. barometric altitude update weight + * + * RE-CHECK this. + * + * @min 0.0 + * @max 1.0 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_GPS_ALT_WGT, 0.9f); From 44c5726703160619b2a49f4eb4c3d0b0d07925fa Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 27 Feb 2014 16:20:36 -0800 Subject: [PATCH 106/381] Make baro altitude relative. --- .../fw_att_pos_estimator_main.cpp | 97 +++++++++++++++---- 1 file changed, 77 insertions(+), 20 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 7e179d400f..c8ef008d93 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -164,6 +164,8 @@ private: struct sensor_combined_s _sensor_combined; #endif + float _baro_ref; /**< barometer reference altitude */ + perf_counter_t _loop_perf; /**< loop performance counter */ perf_counter_t _perf_gyro; /// 8.0f) { fuseVtasData = true; - RecallStates(statesAtVtasMeasTime, (IMUmsec - msecTasDelay)); // assume 100 msec avg delay for airspeed data + RecallStates(statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data FuseAirspeed(); } else { @@ -900,10 +920,13 @@ FixedwingEstimator::task_main() _local_pos.vy = states[5]; _local_pos.vz = states[6]; - _local_pos.xy_valid = true; + _local_pos.xy_valid = _gps_initialized; _local_pos.z_valid = true; - _local_pos.v_xy_valid = true; + _local_pos.v_xy_valid = _gps_initialized; _local_pos.v_z_valid = true; + _local_pos.xy_global = true; + + _local_pos.z_global = false; /* lazily publish the local position only once available */ if (_local_pos_pub > 0) { @@ -915,17 +938,51 @@ FixedwingEstimator::task_main() _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &_local_pos); } - _global_pos.timestamp = _gyro.timestamp; + _global_pos.timestamp = _local_pos.timestamp; - // /* lazily publish the global position only once available */ - // if (_global_pos_pub > 0) { - // /* publish the attitude setpoint */ - // orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos); + _global_pos.baro_valid = true; + _global_pos.global_valid = true; - // } else { - // /* advertise and publish */ - // _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos); - // } + if (_local_pos.xy_global) { + double est_lat, est_lon; + map_projection_reproject(_local_pos.x, _local_pos.y, &est_lat, &est_lon); + _global_pos.lat = est_lat; + _global_pos.lon = est_lon; + _global_pos.time_gps_usec = _gps.time_gps_usec; + } + + /* set valid values even if position is not valid */ + if (_local_pos.v_xy_valid) { + _global_pos.vel_n = _local_pos.vx; + _global_pos.vel_e = _local_pos.vy; + } else { + _global_pos.vel_n = 0.0f; + _global_pos.vel_e = 0.0f; + } + + _global_pos.alt = _local_pos.ref_alt - _local_pos.z; + + if (_local_pos.z_valid) { + _global_pos.baro_alt = _baro_ref - _local_pos.z; + } + + if (_local_pos.v_z_valid) { + _global_pos.vel_d = _local_pos.vz; + } + + _global_pos.yaw = _local_pos.yaw; + + _global_pos.timestamp = _local_pos.timestamp; + + /* lazily publish the global position only once available */ + if (_global_pos_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos); + + } else { + /* advertise and publish */ + _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos); + } } } From d64fa6e255450b3b642acf6b8bf29e86ed87753d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 27 Feb 2014 16:29:43 -0800 Subject: [PATCH 107/381] Handling GPS and baro offset in altitude init / estimate --- .../fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index c8ef008d93..d598052222 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -164,7 +164,8 @@ private: struct sensor_combined_s _sensor_combined; #endif - float _baro_ref; /**< barometer reference altitude */ + float _baro_ref; /**< barometer reference altitude */ + float _baro_gps_offset; /**< offset between GPS and baro */ perf_counter_t _loop_perf; /**< loop performance counter */ perf_counter_t _perf_gyro; /// Date: Fri, 28 Feb 2014 23:43:52 +0400 Subject: [PATCH 108/381] mavlink_onboard removed --- makefiles/config_px4fmu-v2_default.mk | 1 - src/modules/mavlink_onboard/mavlink.c | 537 ------------------ .../mavlink_onboard/mavlink_bridge_header.h | 83 --- .../mavlink_onboard/mavlink_receiver.c | 344 ----------- src/modules/mavlink_onboard/module.mk | 42 -- src/modules/mavlink_onboard/orb_topics.h | 102 ---- src/modules/mavlink_onboard/util.h | 55 -- 7 files changed, 1164 deletions(-) delete mode 100644 src/modules/mavlink_onboard/mavlink.c delete mode 100644 src/modules/mavlink_onboard/mavlink_bridge_header.h delete mode 100644 src/modules/mavlink_onboard/mavlink_receiver.c delete mode 100644 src/modules/mavlink_onboard/module.mk delete mode 100644 src/modules/mavlink_onboard/orb_topics.h delete mode 100644 src/modules/mavlink_onboard/util.h diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 2ad2f63a95..412d0c9e18 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -70,7 +70,6 @@ MODULES += systemcmds/hw_ver MODULES += modules/commander MODULES += modules/navigator MODULES += modules/mavlink -MODULES += modules/mavlink_onboard # # Estimation modules (EKF/ SO3 / other filters) diff --git a/src/modules/mavlink_onboard/mavlink.c b/src/modules/mavlink_onboard/mavlink.c deleted file mode 100644 index ab9ce45f3e..0000000000 --- a/src/modules/mavlink_onboard/mavlink.c +++ /dev/null @@ -1,537 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file mavlink.c - * MAVLink 1.0 protocol implementation. - * - * @author Lorenz Meier - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "mavlink_bridge_header.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include "orb_topics.h" -#include "util.h" - -__EXPORT int mavlink_onboard_main(int argc, char *argv[]); - -static int mavlink_thread_main(int argc, char *argv[]); - -/* thread state */ -volatile bool thread_should_exit = false; -static volatile bool thread_running = false; -static int mavlink_task; - -/* pthreads */ -static pthread_t receive_thread; - -/* terminate MAVLink on user request - disabled by default */ -static bool mavlink_link_termination_allowed = false; - -mavlink_system_t mavlink_system = { - 100, - 50, - MAV_TYPE_QUADROTOR, - 0, - 0, - 0 -}; // System ID, 1-255, Component/Subsystem ID, 1-255 - -/* XXX not widely used */ -uint8_t chan = MAVLINK_COMM_0; - -/* XXX probably should be in a header... */ -extern pthread_t receive_start(int uart); - -bool mavlink_hil_enabled = false; - -/* protocol interface */ -static int uart; -static int baudrate; -bool gcs_link = true; - -/* interface mode */ -static enum { - MAVLINK_INTERFACE_MODE_OFFBOARD, - MAVLINK_INTERFACE_MODE_ONBOARD -} mavlink_link_mode = MAVLINK_INTERFACE_MODE_OFFBOARD; - -static void mavlink_update_system(void); -static int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); -static void usage(void); - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb) -{ - /* process baud rate */ - int speed; - - switch (baud) { - case 0: speed = B0; break; - - case 50: speed = B50; break; - - case 75: speed = B75; break; - - case 110: speed = B110; break; - - case 134: speed = B134; break; - - case 150: speed = B150; break; - - case 200: speed = B200; break; - - case 300: speed = B300; break; - - case 600: speed = B600; break; - - case 1200: speed = B1200; break; - - case 1800: speed = B1800; break; - - case 2400: speed = B2400; break; - - case 4800: speed = B4800; break; - - case 9600: speed = B9600; break; - - case 19200: speed = B19200; break; - - case 38400: speed = B38400; break; - - case 57600: speed = B57600; break; - - case 115200: speed = B115200; break; - - case 230400: speed = B230400; break; - - case 460800: speed = B460800; break; - - case 921600: speed = B921600; break; - - default: - warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600", baud); - return -EINVAL; - } - - /* open uart */ - warnx("UART is %s, baudrate is %d", uart_name, baud); - uart = open(uart_name, O_RDWR | O_NOCTTY); - - /* Try to set baud rate */ - struct termios uart_config; - int termios_state; - *is_usb = false; - - if (strcmp(uart_name, "/dev/ttyACM0") != OK) { - /* Back up the original uart configuration to restore it after exit */ - if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { - warnx("ERROR getting baudrate / termios config for %s: %d", uart_name, termios_state); - close(uart); - return -1; - } - - /* Fill the struct for the new configuration */ - tcgetattr(uart, &uart_config); - - /* Clear ONLCR flag (which appends a CR for every LF) */ - uart_config.c_oflag &= ~ONLCR; - - /* Set baud rate */ - if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { - warnx("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)", 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)", uart_name); - close(uart); - return -1; - } - - } else { - *is_usb = true; - } - - return uart; -} - -void -mavlink_send_uart_bytes(mavlink_channel_t channel, uint8_t *ch, int length) -{ - write(uart, ch, (size_t)(sizeof(uint8_t) * length)); -} - -/* - * Internal function to give access to the channel status for each channel - */ -mavlink_status_t* mavlink_get_channel_status(uint8_t channel) -{ - static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS]; - return &m_mavlink_status[channel]; -} - -/* - * Internal function to give access to the channel buffer for each channel - */ -mavlink_message_t* mavlink_get_channel_buffer(uint8_t channel) -{ - static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS]; - return &m_mavlink_buffer[channel]; -} - -void mavlink_update_system(void) -{ - static bool initialized = false; - param_t param_system_id; - param_t param_component_id; - param_t param_system_type; - - if (!initialized) { - param_system_id = param_find("MAV_SYS_ID"); - param_component_id = param_find("MAV_COMP_ID"); - param_system_type = param_find("MAV_TYPE"); - } - - /* update system and component id */ - int32_t system_id; - param_get(param_system_id, &system_id); - if (system_id > 0 && system_id < 255) { - mavlink_system.sysid = system_id; - } - - int32_t component_id; - param_get(param_component_id, &component_id); - if (component_id > 0 && component_id < 255) { - mavlink_system.compid = component_id; - } - - int32_t system_type; - param_get(param_system_type, &system_type); - if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) { - mavlink_system.type = system_type; - } -} - -void -get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_armed_s *armed, - uint8_t *mavlink_state, uint8_t *mavlink_mode) -{ - /* reset MAVLink mode bitfield */ - *mavlink_mode = 0; - - /* set mode flags independent of system state */ - if (control_mode->flag_control_manual_enabled) { - *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; - } - - if (control_mode->flag_system_hil_enabled) { - *mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED; - } - - /* set arming state */ - if (armed->armed) { - *mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED; - } else { - *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; - } - - if (control_mode->flag_control_velocity_enabled) { - *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; - } else { - *mavlink_mode &= ~MAV_MODE_FLAG_GUIDED_ENABLED; - } - -// switch (v_status->state_machine) { -// case SYSTEM_STATE_PREFLIGHT: -// if (v_status->flag_preflight_gyro_calibration || -// v_status->flag_preflight_mag_calibration || -// v_status->flag_preflight_accel_calibration) { -// *mavlink_state = MAV_STATE_CALIBRATING; -// } else { -// *mavlink_state = MAV_STATE_UNINIT; -// } -// break; -// -// case SYSTEM_STATE_STANDBY: -// *mavlink_state = MAV_STATE_STANDBY; -// break; -// -// case SYSTEM_STATE_GROUND_READY: -// *mavlink_state = MAV_STATE_ACTIVE; -// break; -// -// case SYSTEM_STATE_MANUAL: -// *mavlink_state = MAV_STATE_ACTIVE; -// *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; -// break; -// -// case SYSTEM_STATE_STABILIZED: -// *mavlink_state = MAV_STATE_ACTIVE; -// *mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED; -// break; -// -// case SYSTEM_STATE_AUTO: -// *mavlink_state = MAV_STATE_ACTIVE; -// *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; -// break; -// -// case SYSTEM_STATE_MISSION_ABORT: -// *mavlink_state = MAV_STATE_EMERGENCY; -// break; -// -// case SYSTEM_STATE_EMCY_LANDING: -// *mavlink_state = MAV_STATE_EMERGENCY; -// break; -// -// case SYSTEM_STATE_EMCY_CUTOFF: -// *mavlink_state = MAV_STATE_EMERGENCY; -// break; -// -// case SYSTEM_STATE_GROUND_ERROR: -// *mavlink_state = MAV_STATE_EMERGENCY; -// break; -// -// case SYSTEM_STATE_REBOOT: -// *mavlink_state = MAV_STATE_POWEROFF; -// break; -// } - -} - -/** - * MAVLink Protocol main function. - */ -int mavlink_thread_main(int argc, char *argv[]) -{ - int ch; - char *device_name = "/dev/ttyS1"; - baudrate = 57600; - - /* XXX this is never written? */ - struct vehicle_status_s v_status; - struct vehicle_control_mode_s control_mode; - struct actuator_armed_s armed; - - /* work around some stupidity in task_create's argv handling */ - argc -= 2; - argv += 2; - - while ((ch = getopt(argc, argv, "b:d:eo")) != EOF) { - switch (ch) { - case 'b': - baudrate = strtoul(optarg, NULL, 10); - if (baudrate == 0) - errx(1, "invalid baud rate '%s'", optarg); - break; - - case 'd': - device_name = optarg; - break; - - case 'e': - mavlink_link_termination_allowed = true; - break; - - case 'o': - mavlink_link_mode = MAVLINK_INTERFACE_MODE_ONBOARD; - break; - - default: - usage(); - } - } - - struct termios uart_config_original; - bool usb_uart; - - /* print welcome text */ - warnx("MAVLink v1.0 serial interface starting..."); - - /* inform about mode */ - warnx((mavlink_link_mode == MAVLINK_INTERFACE_MODE_ONBOARD) ? "ONBOARD MODE" : "DOWNLINK MODE"); - - /* Flush stdout in case MAVLink is about to take it over */ - fflush(stdout); - - /* default values for arguments */ - uart = mavlink_open_uart(baudrate, device_name, &uart_config_original, &usb_uart); - if (uart < 0) - err(1, "could not open %s", device_name); - - /* Initialize system properties */ - mavlink_update_system(); - - /* start the MAVLink receiver */ - receive_thread = receive_start(uart); - - thread_running = true; - - /* arm counter to go off immediately */ - unsigned lowspeed_counter = 10; - - while (!thread_should_exit) { - - /* 1 Hz */ - if (lowspeed_counter == 10) { - mavlink_update_system(); - - /* translate the current system state to mavlink state and mode */ - uint8_t mavlink_state = 0; - uint8_t mavlink_mode = 0; - get_mavlink_mode_and_state(&control_mode, &armed, &mavlink_state, &mavlink_mode); - - /* send heartbeat */ - // TODO fix navigation state, use control_mode topic - mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, 0, mavlink_state); - - /* send status (values already copied in the section above) */ - mavlink_msg_sys_status_send(chan, - v_status.onboard_control_sensors_present, - v_status.onboard_control_sensors_enabled, - v_status.onboard_control_sensors_health, - v_status.load * 1000.0f, - v_status.battery_voltage * 1000.0f, - v_status.battery_current * 1000.0f, - v_status.battery_remaining, - v_status.drop_rate_comm, - v_status.errors_comm, - v_status.errors_count1, - v_status.errors_count2, - v_status.errors_count3, - v_status.errors_count4); - lowspeed_counter = 0; - } - lowspeed_counter++; - - /* sleep 1000 ms */ - usleep(1000000); - } - - /* wait for threads to complete */ - pthread_join(receive_thread, NULL); - - /* Reset the UART flags to original state */ - if (!usb_uart) - tcsetattr(uart, TCSANOW, &uart_config_original); - - thread_running = false; - - exit(0); -} - -static void -usage() -{ - fprintf(stderr, "usage: mavlink_onboard start [-d ] [-b ]\n" - " mavlink_onboard stop\n" - " mavlink_onboard status\n"); - exit(1); -} - -int mavlink_onboard_main(int argc, char *argv[]) -{ - - if (argc < 2) { - warnx("missing command"); - usage(); - } - - if (!strcmp(argv[1], "start")) { - - /* this is not an error */ - if (thread_running) - errx(0, "already running"); - - thread_should_exit = false; - mavlink_task = task_spawn_cmd("mavlink_onboard", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 2048, - mavlink_thread_main, - (const char**)argv); - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; - while (thread_running) { - usleep(200000); - } - warnx("terminated"); - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - errx(0, "running"); - } else { - errx(1, "not running"); - } - } - - warnx("unrecognized command"); - usage(); - /* not getting here */ - return 0; -} - diff --git a/src/modules/mavlink_onboard/mavlink_bridge_header.h b/src/modules/mavlink_onboard/mavlink_bridge_header.h deleted file mode 100644 index b72bbb2b19..0000000000 --- a/src/modules/mavlink_onboard/mavlink_bridge_header.h +++ /dev/null @@ -1,83 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file mavlink_bridge_header - * MAVLink bridge header for UART access. - * - * @author Lorenz Meier - */ - -/* MAVLink adapter header */ -#ifndef MAVLINK_BRIDGE_HEADER_H -#define MAVLINK_BRIDGE_HEADER_H - -#define MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/* use efficient approach, see mavlink_helpers.h */ -#define MAVLINK_SEND_UART_BYTES mavlink_send_uart_bytes - -#define MAVLINK_GET_CHANNEL_BUFFER mavlink_get_channel_buffer -#define MAVLINK_GET_CHANNEL_STATUS mavlink_get_channel_status - -#include -#include - - -/* Struct that stores the communication settings of this system. - you can also define / alter these settings elsewhere, as long - as they're included BEFORE mavlink.h. - So you can set the - - mavlink_system.sysid = 100; // System ID, 1-255 - mavlink_system.compid = 50; // Component/Subsystem ID, 1-255 - - Lines also in your main.c, e.g. by reading these parameter from EEPROM. - */ -extern mavlink_system_t mavlink_system; - -/** - * @brief Send multiple chars (uint8_t) over a comm channel - * - * @param chan MAVLink channel to use, usually MAVLINK_COMM_0 = UART0 - * @param ch Character to send - */ -extern void mavlink_send_uart_bytes(mavlink_channel_t chan, uint8_t *ch, int length); - -mavlink_status_t* mavlink_get_channel_status(uint8_t chan); -mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan); - -#include - -#endif /* MAVLINK_BRIDGE_HEADER_H */ diff --git a/src/modules/mavlink_onboard/mavlink_receiver.c b/src/modules/mavlink_onboard/mavlink_receiver.c deleted file mode 100644 index 4658bcc1d4..0000000000 --- a/src/modules/mavlink_onboard/mavlink_receiver.c +++ /dev/null @@ -1,344 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file mavlink_receiver.c - * MAVLink protocol message receive and dispatch - * - * @author Lorenz Meier - */ - -/* XXX trim includes */ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "mavlink_bridge_header.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include "util.h" -#include "orb_topics.h" - -/* XXX should be in a header somewhere */ -pthread_t receive_start(int uart); - -static void handle_message(mavlink_message_t *msg); -static void *receive_thread(void *arg); - -static mavlink_status_t status; -static struct vehicle_vicon_position_s vicon_position; -static struct vehicle_command_s vcmd; -static struct offboard_control_setpoint_s offboard_control_sp; - -struct vehicle_global_position_s hil_global_pos; -struct vehicle_attitude_s hil_attitude; -orb_advert_t pub_hil_global_pos = -1; -orb_advert_t pub_hil_attitude = -1; - -static orb_advert_t cmd_pub = -1; -static orb_advert_t flow_pub = -1; - -static orb_advert_t offboard_control_sp_pub = -1; -static orb_advert_t vicon_position_pub = -1; - -extern bool gcs_link; - -static void -handle_message(mavlink_message_t *msg) -{ - if (msg->msgid == MAVLINK_MSG_ID_COMMAND_LONG) { - - mavlink_command_long_t cmd_mavlink; - mavlink_msg_command_long_decode(msg, &cmd_mavlink); - - if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid) - || (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) { - //check for MAVLINK terminate command - if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) { - /* This is the link shutdown command, terminate mavlink */ - warnx("terminating..."); - fflush(stdout); - usleep(50000); - - /* terminate other threads and this thread */ - thread_should_exit = true; - - } else { - - /* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ - vcmd.param1 = cmd_mavlink.param1; - vcmd.param2 = cmd_mavlink.param2; - vcmd.param3 = cmd_mavlink.param3; - vcmd.param4 = cmd_mavlink.param4; - vcmd.param5 = cmd_mavlink.param5; - vcmd.param6 = cmd_mavlink.param6; - vcmd.param7 = cmd_mavlink.param7; - vcmd.command = cmd_mavlink.command; - vcmd.target_system = cmd_mavlink.target_system; - vcmd.target_component = cmd_mavlink.target_component; - vcmd.source_system = msg->sysid; - vcmd.source_component = msg->compid; - vcmd.confirmation = cmd_mavlink.confirmation; - - /* check if topic is advertised */ - if (cmd_pub <= 0) { - cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); - } - - /* publish */ - orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd); - } - } - } - - if (msg->msgid == MAVLINK_MSG_ID_OPTICAL_FLOW) { - mavlink_optical_flow_t flow; - mavlink_msg_optical_flow_decode(msg, &flow); - - struct optical_flow_s f; - - f.timestamp = hrt_absolute_time(); - 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; - - /* check if topic is advertised */ - if (flow_pub <= 0) { - flow_pub = orb_advertise(ORB_ID(optical_flow), &f); - - } else { - /* publish */ - orb_publish(ORB_ID(optical_flow), flow_pub, &f); - } - - } - - if (msg->msgid == MAVLINK_MSG_ID_SET_MODE) { - /* Set mode on request */ - mavlink_set_mode_t new_mode; - mavlink_msg_set_mode_decode(msg, &new_mode); - - /* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ - vcmd.param1 = new_mode.base_mode; - vcmd.param2 = new_mode.custom_mode; - vcmd.param3 = 0; - vcmd.param4 = 0; - vcmd.param5 = 0; - vcmd.param6 = 0; - vcmd.param7 = 0; - vcmd.command = MAV_CMD_DO_SET_MODE; - vcmd.target_system = new_mode.target_system; - vcmd.target_component = MAV_COMP_ID_ALL; - vcmd.source_system = msg->sysid; - vcmd.source_component = msg->compid; - vcmd.confirmation = 1; - - /* check if topic is advertised */ - if (cmd_pub <= 0) { - cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); - - } else { - /* create command */ - orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd); - } - } - - /* Handle Vicon position estimates */ - if (msg->msgid == MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE) { - mavlink_vicon_position_estimate_t pos; - mavlink_msg_vicon_position_estimate_decode(msg, &pos); - - vicon_position.x = pos.x; - vicon_position.y = pos.y; - vicon_position.z = pos.z; - - if (vicon_position_pub <= 0) { - vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position); - - } else { - orb_publish(ORB_ID(vehicle_vicon_position), vicon_position_pub, &vicon_position); - } - } - - /* Handle quadrotor motor setpoints */ - - if (msg->msgid == MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST) { - mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t quad_motors_setpoint; - mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &quad_motors_setpoint); - - if (mavlink_system.sysid < 4) { - - /* switch to a receiving link mode */ - gcs_link = false; - - /* - * rate control mode - defined by MAVLink - */ - - uint8_t ml_mode = 0; - bool ml_armed = false; - - switch (quad_motors_setpoint.mode) { - case 0: - ml_armed = false; - break; - - case 1: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES; - ml_armed = true; - - break; - - case 2: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; - ml_armed = true; - - break; - - case 3: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY; - break; - - case 4: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION; - break; - } - - offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1] / (float)INT16_MAX; - offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1] / (float)INT16_MAX; - offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1] / (float)INT16_MAX; - offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / (float)UINT16_MAX; - - if (quad_motors_setpoint.thrust[mavlink_system.sysid - 1] == 0) { - ml_armed = false; - } - - offboard_control_sp.armed = ml_armed; - offboard_control_sp.mode = ml_mode; - - offboard_control_sp.timestamp = hrt_absolute_time(); - - /* check if topic has to be advertised */ - if (offboard_control_sp_pub <= 0) { - offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); - - } else { - /* Publish */ - orb_publish(ORB_ID(offboard_control_setpoint), offboard_control_sp_pub, &offboard_control_sp); - } - } - } - -} - - -/** - * Receive data from UART. - */ -static void * -receive_thread(void *arg) -{ - int uart_fd = *((int *)arg); - - const int timeout = 1000; - uint8_t buf[32]; - - mavlink_message_t msg; - - prctl(PR_SET_NAME, "mavlink_onboard_rcv", getpid()); - - struct pollfd fds[] = { { .fd = uart_fd, .events = POLLIN } }; - - ssize_t nread = 0; - - while (!thread_should_exit) { - if (poll(fds, 1, timeout) > 0) { - if (nread < sizeof(buf)) { - /* to avoid reading very small chunks wait for data before reading */ - usleep(1000); - } - - /* non-blocking read. read may return negative values */ - nread = read(uart_fd, buf, sizeof(buf)); - - /* if read failed, this loop won't execute */ - for (ssize_t i = 0; i < nread; i++) { - if (mavlink_parse_char(chan, buf[i], &msg, &status)) { - /* handle generic messages and commands */ - handle_message(&msg); - } - } - } - } - - return NULL; -} - -pthread_t -receive_start(int uart) -{ - pthread_attr_t receiveloop_attr; - pthread_attr_init(&receiveloop_attr); - - struct sched_param param; - param.sched_priority = SCHED_PRIORITY_MAX - 40; - (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); - - pthread_attr_setstacksize(&receiveloop_attr, 2048); - - pthread_t thread; - pthread_create(&thread, &receiveloop_attr, receive_thread, &uart); - return thread; -} diff --git a/src/modules/mavlink_onboard/module.mk b/src/modules/mavlink_onboard/module.mk deleted file mode 100644 index a7a4980faf..0000000000 --- a/src/modules/mavlink_onboard/module.mk +++ /dev/null @@ -1,42 +0,0 @@ -############################################################################ -# -# Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# MAVLink protocol to uORB interface process (XXX hack for onboard use) -# - -MODULE_COMMAND = mavlink_onboard -SRCS = mavlink.c \ - mavlink_receiver.c - -INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink diff --git a/src/modules/mavlink_onboard/orb_topics.h b/src/modules/mavlink_onboard/orb_topics.h deleted file mode 100644 index bbc9f6e66a..0000000000 --- a/src/modules/mavlink_onboard/orb_topics.h +++ /dev/null @@ -1,102 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file orb_topics.h - * Common sets of topics subscribed to or published by the MAVLink driver, - * and structures maintained by those subscriptions. - */ -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -struct mavlink_subscriptions { - int sensor_sub; - int att_sub; - int global_pos_sub; - int act_0_sub; - int act_1_sub; - int act_2_sub; - int act_3_sub; - int gps_sub; - int man_control_sp_sub; - int safety_sub; - int actuators_sub; - int local_pos_sub; - int spa_sub; - int spl_sub; - int spg_sub; - int debug_key_value; - int input_rc_sub; -}; - -extern struct mavlink_subscriptions mavlink_subs; - -/** Global position */ -extern struct vehicle_global_position_s global_pos; - -/** Local position */ -extern struct vehicle_local_position_s local_pos; - -/** Vehicle status */ -// extern struct vehicle_status_s v_status; - -/** RC channels */ -extern struct rc_channels_s rc; - -/** Actuator armed state */ -// extern struct actuator_armed_s armed; - -/** Worker thread starter */ -extern pthread_t uorb_receive_start(void); diff --git a/src/modules/mavlink_onboard/util.h b/src/modules/mavlink_onboard/util.h deleted file mode 100644 index c84b6fd26e..0000000000 --- a/src/modules/mavlink_onboard/util.h +++ /dev/null @@ -1,55 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file util.h - * Utility and helper functions and data. - */ - -#pragma once - -#include "orb_topics.h" - -/** MAVLink communications channel */ -extern uint8_t chan; - -/** Shutdown marker */ -extern volatile bool thread_should_exit; - -/** - * Translate the custom state into standard mavlink modes and state. - */ -extern void -get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_armed_s *armed, - uint8_t *mavlink_state, uint8_t *mavlink_mode); From 323b90bfd9f3a1524f36c089d532ed3836cc2ea3 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 28 Feb 2014 23:44:51 +0400 Subject: [PATCH 109/381] mavlink: uORB topics includes moved to mavlink_messages.cpp, more messages implemented --- src/modules/mavlink/mavlink_main.h | 28 +- src/modules/mavlink/mavlink_messages.cpp | 433 ++++++++++++++++------- 2 files changed, 300 insertions(+), 161 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 049f5fedd5..41e781ee8f 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -42,44 +42,18 @@ #include #include -#include #include #include #include #include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include #include #include "mavlink_bridge_header.h" #include "mavlink_orb_subscription.h" #include "mavlink_stream.h" +#include "mavlink_messages.h" // FIXME XXX - TO BE MOVED TO XML enum MAVLINK_WPM_STATES { diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 9676068419..3634acefa4 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -10,12 +10,33 @@ #include #include +#include +#include +#include +#include #include +#include #include #include +#include +#include +#include +#include +#include +#include #include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include "mavlink_messages.h" @@ -128,9 +149,9 @@ public: private: MavlinkOrbSubscription *status_sub; - MavlinkOrbSubscription *pos_sp_triplet_sub; - struct vehicle_status_s *status; + + MavlinkOrbSubscription *pos_sp_triplet_sub; struct position_setpoint_triplet_s *pos_sp_triplet; protected: @@ -177,7 +198,6 @@ public: private: MavlinkOrbSubscription *status_sub; - struct vehicle_status_s *status; protected: @@ -210,6 +230,10 @@ protected: class MavlinkStreamHighresIMU : public MavlinkStream { public: + MavlinkStreamHighresIMU() : MavlinkStream(), accel_counter(0), gyro_counter(0), mag_counter(0), baro_counter(0) + { + } + const char *get_name() { return "HIGHRES_IMU"; @@ -222,13 +246,12 @@ public: private: MavlinkOrbSubscription *sensor_sub; - struct sensor_combined_s *sensor; - uint32_t accel_counter = 0; - uint32_t gyro_counter = 0; - uint32_t mag_counter = 0; - uint32_t baro_counter = 0; + uint32_t accel_counter; + uint32_t gyro_counter; + uint32_t mag_counter; + uint32_t baro_counter; protected: void subscribe(Mavlink *mavlink) @@ -292,7 +315,6 @@ public: private: MavlinkOrbSubscription *att_sub; - struct vehicle_attitude_s *att; protected: @@ -313,6 +335,75 @@ protected: }; +class MavlinkStreamVFRHUD : public MavlinkStream { +public: + const char *get_name() + { + return "VFR_HUD"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamVFRHUD(); + } + +private: + MavlinkOrbSubscription *att_sub; + struct vehicle_attitude_s *att; + + MavlinkOrbSubscription *pos_sub; + struct vehicle_global_position_s *pos; + + MavlinkOrbSubscription *armed_sub; + struct actuator_armed_s *armed; + + MavlinkOrbSubscription *act_sub; + struct actuator_controls_s *act; + + MavlinkOrbSubscription *airspeed_sub; + struct airspeed_s *airspeed; + +protected: + void subscribe(Mavlink *mavlink) + { + att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude), sizeof(struct vehicle_attitude_s)); + att = (struct vehicle_attitude_s *)att_sub->get_data(); + + pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position), sizeof(struct vehicle_global_position_s)); + pos = (struct vehicle_global_position_s *)pos_sub->get_data(); + + armed_sub = mavlink->add_orb_subscription(ORB_ID(actuator_armed), sizeof(struct actuator_armed_s)); + armed = (struct actuator_armed_s *)armed_sub->get_data(); + + act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_controls_0), sizeof(struct actuator_controls_s)); + act = (struct actuator_controls_s *)act_sub->get_data(); + + airspeed_sub = mavlink->add_orb_subscription(ORB_ID(airspeed), sizeof(struct airspeed_s)); + airspeed = (struct airspeed_s *)airspeed_sub->get_data(); + } + + void send(const hrt_abstime t) { + att_sub->update(t); + pos_sub->update(t); + armed_sub->update(t); + act_sub->update(t); + airspeed_sub->update(t); + + float groundspeed = sqrtf(pos->vel_n * pos->vel_n + pos->vel_e * pos->vel_e); + uint16_t heading = _wrap_2pi(att->yaw) * M_RAD_TO_DEG_F; + float throttle = armed->armed ? act->control[3] * 100.0f : 0.0f; + + mavlink_msg_vfr_hud_send(_channel, + airspeed->true_airspeed_m_s, + groundspeed, + heading, + throttle, + pos->alt, + -pos->vel_d); + } +}; + + class MavlinkStreamGPSRawInt : public MavlinkStream { public: const char *get_name() @@ -638,11 +729,203 @@ protected: }; +class MavlinkStreamGlobalPositionSetpointInt : public MavlinkStream { +public: + const char *get_name() + { + return "GLOBAL_POSITION_SETPOINT_INT"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamGlobalPositionSetpointInt(); + } + +private: + MavlinkOrbSubscription *pos_sp_triplet_sub; + struct position_setpoint_triplet_s *pos_sp_triplet; + +protected: + void subscribe(Mavlink *mavlink) + { + pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet), sizeof(position_setpoint_triplet_s)); + pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data(); + } + + void send(const hrt_abstime t) { + pos_sp_triplet_sub->update(t); + + mavlink_msg_global_position_setpoint_int_send(_channel, + MAV_FRAME_GLOBAL, + (int32_t)(pos_sp_triplet->current.lat * 1e7), + (int32_t)(pos_sp_triplet->current.lon * 1e7), + (int32_t)(pos_sp_triplet->current.alt * 1000), + (int16_t)(pos_sp_triplet->current.yaw * M_RAD_TO_DEG_F * 100.0f)); + } +}; + + +class MavlinkStreamLocalPositionSetpoint : public MavlinkStream { +public: + const char *get_name() + { + return "LOCAL_POSITION_SETPOINT"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamLocalPositionSetpoint(); + } + +private: + MavlinkOrbSubscription *pos_sp_sub; + struct vehicle_local_position_setpoint_s *pos_sp; + +protected: + void subscribe(Mavlink *mavlink) + { + pos_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint), sizeof(vehicle_local_position_setpoint_s)); + pos_sp = (struct vehicle_local_position_setpoint_s *)pos_sp_sub->get_data(); + } + + void send(const hrt_abstime t) { + pos_sp_sub->update(t); + + mavlink_msg_local_position_setpoint_send(_channel, + MAV_FRAME_LOCAL_NED, + pos_sp->x, + pos_sp->y, + pos_sp->z, + pos_sp->yaw); + } +}; + + +class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream { +public: + const char *get_name() + { + return "ROLL_PITCH_YAW_THRUST_SETPOINT"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamRollPitchYawThrustSetpoint(); + } + +private: + MavlinkOrbSubscription *att_sp_sub; + struct vehicle_attitude_setpoint_s *att_sp; + +protected: + void subscribe(Mavlink *mavlink) + { + att_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint), sizeof(vehicle_attitude_setpoint_s)); + att_sp = (struct vehicle_attitude_setpoint_s *)att_sp_sub->get_data(); + } + + void send(const hrt_abstime t) { + att_sp_sub->update(t); + + mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel, + att_sp->timestamp / 1000, + att_sp->roll_body, + att_sp->pitch_body, + att_sp->yaw_body, + att_sp->thrust); + } +}; + + +class MavlinkStreamRollPitchYawRatesThrustSetpoint : public MavlinkStream { +public: + const char *get_name() + { + return "ROLL_PITCH_YAW_RATES_THRUST_SETPOINT"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamRollPitchYawRatesThrustSetpoint(); + } + +private: + MavlinkOrbSubscription *att_rates_sp_sub; + struct vehicle_rates_setpoint_s *att_rates_sp; + +protected: + void subscribe(Mavlink *mavlink) + { + att_rates_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint), sizeof(vehicle_rates_setpoint_s)); + att_rates_sp = (struct vehicle_rates_setpoint_s *)att_rates_sp_sub->get_data(); + } + + void send(const hrt_abstime t) { + att_rates_sp_sub->update(t); + + mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel, + att_rates_sp->timestamp / 1000, + att_rates_sp->roll, + att_rates_sp->pitch, + att_rates_sp->yaw, + att_rates_sp->thrust); + } +}; + + +class MavlinkStreamRCChannelsRaw : public MavlinkStream { +public: + const char *get_name() + { + return "RC_CHANNELS_RAW"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamRCChannelsRaw(); + } + +private: + MavlinkOrbSubscription *rc_sub; + struct rc_input_values *rc; + +protected: + void subscribe(Mavlink *mavlink) + { + rc_sub = mavlink->add_orb_subscription(ORB_ID(input_rc), sizeof(struct rc_input_values)); + rc = (struct rc_input_values *)rc_sub->get_data(); + } + + void send(const hrt_abstime t) { + rc_sub->update(t); + + const unsigned port_width = 8; + + for (unsigned i = 0; (i * port_width) < rc->channel_count; i++) { + /* Channels are sent in MAVLink main loop at a fixed interval */ + mavlink_msg_rc_channels_raw_send(_channel, + rc->timestamp_publication / 1000, + i, + (rc->channel_count > (i * port_width) + 0) ? rc->values[(i * port_width) + 0] : UINT16_MAX, + (rc->channel_count > (i * port_width) + 1) ? rc->values[(i * port_width) + 1] : UINT16_MAX, + (rc->channel_count > (i * port_width) + 2) ? rc->values[(i * port_width) + 2] : UINT16_MAX, + (rc->channel_count > (i * port_width) + 3) ? rc->values[(i * port_width) + 3] : UINT16_MAX, + (rc->channel_count > (i * port_width) + 4) ? rc->values[(i * port_width) + 4] : UINT16_MAX, + (rc->channel_count > (i * port_width) + 5) ? rc->values[(i * port_width) + 5] : UINT16_MAX, + (rc->channel_count > (i * port_width) + 6) ? rc->values[(i * port_width) + 6] : UINT16_MAX, + (rc->channel_count > (i * port_width) + 7) ? rc->values[(i * port_width) + 7] : UINT16_MAX, + rc->rssi); + } + } +}; + + MavlinkStream *streams_list[] = { new MavlinkStreamHeartbeat(), new MavlinkStreamSysStatus(), new MavlinkStreamHighresIMU(), new MavlinkStreamAttitude(), + new MavlinkStreamVFRHUD(), new MavlinkStreamGPSRawInt(), new MavlinkStreamGlobalPositionInt(), new MavlinkStreamLocalPositionNED(), @@ -652,6 +935,11 @@ MavlinkStream *streams_list[] = { new MavlinkStreamServoOutputRaw(2), new MavlinkStreamServoOutputRaw(3), new MavlinkStreamHILControls(), + new MavlinkStreamGlobalPositionSetpointInt(), + new MavlinkStreamLocalPositionSetpoint(), + new MavlinkStreamRollPitchYawThrustSetpoint(), + new MavlinkStreamRollPitchYawRatesThrustSetpoint(), + new MavlinkStreamRCChannelsRaw(), nullptr }; @@ -705,132 +993,9 @@ MavlinkStream *streams_list[] = { // l->listener->attitude_counter++; //} // -//void -//MavlinkOrbListener::l_vehicle_gps_position(const struct listener *l) -//{ -// struct vehicle_gps_position_s gps; -// -// /* copy gps data into local buffer */ -// orb_copy(ORB_ID(vehicle_gps_position), l->mavlink->get_subs()->gps_sub, &gps); -// -// /* update SAT info every 10 seconds */ -// if (gps.satellite_info_available && (l->listener->gps_counter % 50 == 0)) { -// mavlink_msg_gps_status_send(l->mavlink->get_chan(), -// gps.satellites_visible, -// gps.satellite_prn, -// gps.satellite_used, -// gps.satellite_elevation, -// gps.satellite_azimuth, -// gps.satellite_snr); -// } -// -// l->listener->gps_counter++; -//} // // -//void -//MavlinkOrbListener::l_input_rc(const struct listener *l) -//{ -// /* copy rc _mavlink->get_chan()nels into local buffer */ -// orb_copy(ORB_ID(input_rc), l->mavlink->get_subs()->input_rc_sub, &l->listener->rc_raw); // -// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) { -// -// const unsigned port_width = 8; -// -// for (unsigned i = 0; (i * port_width) < l->listener->rc_raw.channel_count; i++) { -// /* Channels are sent in MAVLink main loop at a fixed interval */ -// mavlink_msg_rc_channels_raw_send(l->mavlink->get_chan(), -// l->listener->rc_raw.timestamp_publication / 1000, -// i, -// (l->listener->rc_raw.channel_count > (i * port_width) + 0) ? l->listener->rc_raw.values[(i * port_width) + 0] : UINT16_MAX, -// (l->listener->rc_raw.channel_count > (i * port_width) + 1) ? l->listener->rc_raw.values[(i * port_width) + 1] : UINT16_MAX, -// (l->listener->rc_raw.channel_count > (i * port_width) + 2) ? l->listener->rc_raw.values[(i * port_width) + 2] : UINT16_MAX, -// (l->listener->rc_raw.channel_count > (i * port_width) + 3) ? l->listener->rc_raw.values[(i * port_width) + 3] : UINT16_MAX, -// (l->listener->rc_raw.channel_count > (i * port_width) + 4) ? l->listener->rc_raw.values[(i * port_width) + 4] : UINT16_MAX, -// (l->listener->rc_raw.channel_count > (i * port_width) + 5) ? l->listener->rc_raw.values[(i * port_width) + 5] : UINT16_MAX, -// (l->listener->rc_raw.channel_count > (i * port_width) + 6) ? l->listener->rc_raw.values[(i * port_width) + 6] : UINT16_MAX, -// (l->listener->rc_raw.channel_count > (i * port_width) + 7) ? l->listener->rc_raw.values[(i * port_width) + 7] : UINT16_MAX, -// l->listener->rc_raw.rssi); -// } -// } -//} -// -// -//void -//MavlinkOrbListener::l_global_position_setpoint(const struct listener *l) -//{ -// struct position_setpoint_triplet_s triplet; -// orb_copy(ORB_ID(position_setpoint_triplet), l->mavlink->get_subs()->triplet_sub, &triplet); -// -// if (!triplet.current.valid) -// return; -// -// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) -// mavlink_msg_global_position_setpoint_int_send(l->mavlink->get_chan(), -// MAV_FRAME_GLOBAL, -// (int32_t)(triplet.current.lat * 1e7d), -// (int32_t)(triplet.current.lon * 1e7d), -// (int32_t)(triplet.current.alt * 1e3f), -// (int16_t)(triplet.current.yaw * M_RAD_TO_DEG_F * 1e2f)); -//} -// -//void -//MavlinkOrbListener::l_local_position_setpoint(const struct listener *l) -//{ -// struct vehicle_local_position_setpoint_s local_sp; -// -// /* copy local position data into local buffer */ -// orb_copy(ORB_ID(vehicle_local_position_setpoint), l->mavlink->get_subs()->spl_sub, &local_sp); -// -// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) -// mavlink_msg_local_position_setpoint_send(l->mavlink->get_chan(), -// MAV_FRAME_LOCAL_NED, -// local_sp.x, -// local_sp.y, -// local_sp.z, -// local_sp.yaw); -//} -// -//void -//MavlinkOrbListener::l_attitude_setpoint(const struct listener *l) -//{ -// struct vehicle_attitude_setpoint_s att_sp; -// -// /* copy local position data into local buffer */ -// orb_copy(ORB_ID(vehicle_attitude_setpoint), l->mavlink->get_subs()->spa_sub, &att_sp); -// -// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) -// mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(l->mavlink->get_chan(), -// att_sp.timestamp / 1000, -// att_sp.roll_body, -// att_sp.pitch_body, -// att_sp.yaw_body, -// att_sp.thrust); -//} -// -//void -//MavlinkOrbListener::l_vehicle_rates_setpoint(const struct listener *l) -//{ -// struct vehicle_rates_setpoint_s rates_sp; -// -// /* copy local position data into local buffer */ -// orb_copy(ORB_ID(vehicle_rates_setpoint), l->mavlink->get_subs()->rates_setpoint_sub, &rates_sp); -// -// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) -// mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(l->mavlink->get_chan(), -// rates_sp.timestamp / 1000, -// rates_sp.roll, -// rates_sp.pitch, -// rates_sp.yaw, -// rates_sp.thrust); -//} -// -//void -//MavlinkOrbListener::l_actuator_armed(const struct listener *l) -//{ -// orb_copy(ORB_ID(actuator_armed), l->mavlink->get_subs()->armed_sub, &l->listener->armed); -//} // //void //MavlinkOrbListener::l_manual_control_setpoint(const struct listener *l) From 77d1989abae9d267bb74f86fb0104dbefcbcd52a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Mar 2014 00:06:30 +0400 Subject: [PATCH 110/381] mavlink: more message streams implemented --- src/modules/mavlink/mavlink_messages.cpp | 144 ++++++++++++----------- 1 file changed, 78 insertions(+), 66 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 3634acefa4..8097ecdb36 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -335,6 +335,45 @@ protected: }; +class MavlinkStreamAttitudeQuaternion : public MavlinkStream { +public: + const char *get_name() + { + return "ATTITUDE_QUATERNION"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamAttitudeQuaternion(); + } + +private: + MavlinkOrbSubscription *att_sub; + struct vehicle_attitude_s *att; + +protected: + void subscribe(Mavlink *mavlink) + { + att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude), sizeof(struct vehicle_attitude_s)); + att = (struct vehicle_attitude_s *)att_sub->get_data(); + } + + void send(const hrt_abstime t) { + att_sub->update(t); + + mavlink_msg_attitude_quaternion_send(_channel, + att->timestamp / 1000, + att->q[0], + att->q[1], + att->q[2], + att->q[3], + att->rollspeed, + att->pitchspeed, + att->yawspeed); + } +}; + + class MavlinkStreamVFRHUD : public MavlinkStream { public: const char *get_name() @@ -920,11 +959,49 @@ protected: }; +class MavlinkStreamManualControl : public MavlinkStream { +public: + const char *get_name() + { + return "MANUAL_CONTROL"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamManualControl(); + } + +private: + MavlinkOrbSubscription *manual_sub; + struct manual_control_setpoint_s *manual; + +protected: + void subscribe(Mavlink *mavlink) + { + manual_sub = mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint), sizeof(struct manual_control_setpoint_s)); + manual = (struct manual_control_setpoint_s *)manual_sub->get_data(); + } + + void send(const hrt_abstime t) { + manual_sub->update(t); + + mavlink_msg_manual_control_send(_channel, + mavlink_system.sysid, + manual->roll * 1000, + manual->pitch * 1000, + manual->yaw * 1000, + manual->throttle * 1000, + 0); + } +}; + + MavlinkStream *streams_list[] = { new MavlinkStreamHeartbeat(), new MavlinkStreamSysStatus(), new MavlinkStreamHighresIMU(), new MavlinkStreamAttitude(), + new MavlinkStreamAttitudeQuaternion(), new MavlinkStreamVFRHUD(), new MavlinkStreamGPSRawInt(), new MavlinkStreamGlobalPositionInt(), @@ -940,6 +1017,7 @@ MavlinkStream *streams_list[] = { new MavlinkStreamRollPitchYawThrustSetpoint(), new MavlinkStreamRollPitchYawRatesThrustSetpoint(), new MavlinkStreamRCChannelsRaw(), + new MavlinkStreamManualControl(), nullptr }; @@ -949,71 +1027,11 @@ MavlinkStream *streams_list[] = { -//void -//MavlinkOrbListener::l_vehicle_attitude(const struct listener *l) -//{ -// /* copy attitude data into local buffer */ -// orb_copy(ORB_ID(vehicle_attitude), l->mavlink->get_subs()->att_sub, &l->listener->att); -// -// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) { -// /* send sensor values */ -// mavlink_msg_attitude_send(l->mavlink->get_chan(), -// l->listener->last_sensor_timestamp / 1000, -// l->listener->att.roll, -// l->listener->att.pitch, -// l->listener->att.yaw, -// l->listener->att.rollspeed, -// l->listener->att.pitchspeed, -// l->listener->att.yawspeed); -// -// /* limit VFR message rate to 10Hz */ -// hrt_abstime t = hrt_absolute_time(); -// if (t >= l->listener->last_sent_vfr + 100000) { -// l->listener->last_sent_vfr = t; -// float groundspeed = sqrtf(l->listener->global_pos.vel_n * l->listener->global_pos.vel_n + l->listener->global_pos.vel_e * l->listener->global_pos.vel_e); -// uint16_t heading = _wrap_2pi(l->listener->att.yaw) * M_RAD_TO_DEG_F; -// float throttle = l->listener->armed.armed ? l->listener->actuators_0.control[3] * 100.0f : 0.0f; -// mavlink_msg_vfr_hud_send(l->mavlink->get_chan(), l->listener->airspeed.true_airspeed_m_s, groundspeed, heading, throttle, l->listener->global_pos.alt, -l->listener->global_pos.vel_d); -// } -// -// /* send quaternion values if it exists */ -// if(l->listener->att.q_valid) { -// mavlink_msg_attitude_quaternion_send(l->mavlink->get_chan(), -// l->listener->last_sensor_timestamp / 1000, -// l->listener->att.q[0], -// l->listener->att.q[1], -// l->listener->att.q[2], -// l->listener->att.q[3], -// l->listener->att.rollspeed, -// l->listener->att.pitchspeed, -// l->listener->att.yawspeed); -// } -// } -// -// l->listener->attitude_counter++; -//} // // // // // -//void -//MavlinkOrbListener::l_manual_control_setpoint(const struct listener *l) -//{ -// struct manual_control_setpoint_s man_control; -// -// /* copy manual control data into local buffer */ -// orb_copy(ORB_ID(manual_control_setpoint), l->mavlink->get_subs()->man_control_sp_sub, &man_control); -// -// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) -// mavlink_msg_manual_control_send(l->mavlink->get_chan(), -// mavlink_system.sysid, -// man_control.roll * 1000, -// man_control.pitch * 1000, -// man_control.yaw * 1000, -// man_control.throttle * 1000, -// 0); -//} // //void //MavlinkOrbListener::l_vehicle_attitude_controls(const struct listener *l) @@ -1069,12 +1087,6 @@ MavlinkStream *streams_list[] = { //} // //void -//MavlinkOrbListener::l_airspeed(const struct listener *l) -//{ -// orb_copy(ORB_ID(airspeed), l->mavlink->get_subs()->airspeed_sub, &l->listener->airspeed); -//} -// -//void //MavlinkOrbListener::l_nav_cap(const struct listener *l) //{ // From 836f7c435fe31572e45333877142dce8b4d2fc78 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Mar 2014 00:16:51 +0400 Subject: [PATCH 111/381] mavlink: code style and copyright fixes --- src/modules/mavlink/mavlink_main.cpp | 448 +++++++++------- src/modules/mavlink/mavlink_main.h | 35 +- src/modules/mavlink/mavlink_messages.cpp | 495 ++++++++++-------- src/modules/mavlink/mavlink_messages.h | 41 +- .../mavlink/mavlink_orb_subscription.cpp | 44 +- .../mavlink/mavlink_orb_subscription.h | 44 +- src/modules/mavlink/mavlink_rate_limiter.cpp | 44 +- src/modules/mavlink/mavlink_rate_limiter.h | 44 +- src/modules/mavlink/mavlink_receiver.cpp | 33 +- src/modules/mavlink/mavlink_receiver.h | 4 +- src/modules/mavlink/mavlink_stream.cpp | 42 +- src/modules/mavlink/mavlink_stream.h | 44 +- 12 files changed, 867 insertions(+), 451 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 672daf641b..c977865538 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -37,6 +37,7 @@ * * @author Lorenz Meier * @author Julian Oes + * @author Anton Babushkin */ #include @@ -91,7 +92,7 @@ static const int ERROR = -1; #define MAX_DATA_RATE 10000 // max data rate in bytes/s #define MAIN_LOOP_DELAY 10000 // 100 Hz -static Mavlink* _head = nullptr; +static Mavlink *_head = nullptr; /* TODO: if this is a class member it crashes */ static struct file_operations fops; @@ -112,40 +113,47 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length int uart = -1; switch (channel) { - case MAVLINK_COMM_0: - uart = Mavlink::get_uart_fd(0); + case MAVLINK_COMM_0: + uart = Mavlink::get_uart_fd(0); break; - case MAVLINK_COMM_1: - uart = Mavlink::get_uart_fd(1); + + case MAVLINK_COMM_1: + uart = Mavlink::get_uart_fd(1); break; - case MAVLINK_COMM_2: - uart = Mavlink::get_uart_fd(2); + + case MAVLINK_COMM_2: + uart = Mavlink::get_uart_fd(2); break; - case MAVLINK_COMM_3: - uart = Mavlink::get_uart_fd(3); + + case MAVLINK_COMM_3: + uart = Mavlink::get_uart_fd(3); break; - #ifdef MAVLINK_COMM_4 - case MAVLINK_COMM_4: - uart = Mavlink::get_uart_fd(4); +#ifdef MAVLINK_COMM_4 + + case MAVLINK_COMM_4: + uart = Mavlink::get_uart_fd(4); break; - #endif - #ifdef MAVLINK_COMM_5 - case MAVLINK_COMM_5: - uart = Mavlink::get_uart_fd(5); +#endif +#ifdef MAVLINK_COMM_5 + + case MAVLINK_COMM_5: + uart = Mavlink::get_uart_fd(5); break; - #endif - #ifdef MAVLINK_COMM_6 - case MAVLINK_COMM_6: - uart = Mavlink::get_uart_fd(6); +#endif +#ifdef MAVLINK_COMM_6 + + case MAVLINK_COMM_6: + uart = Mavlink::get_uart_fd(6); break; - #endif +#endif } ssize_t desired = (sizeof(uint8_t) * length); ssize_t ret = write(uart, ch, desired); - if (ret != desired) + if (ret != desired) { warn("write err"); + } } @@ -168,7 +176,7 @@ Mavlink::Mavlink() : _loop_perf(perf_alloc(PC_ELAPSED, "mavlink")) { wpm = &wpm_s; - fops.ioctl = (int (*)(file*, int, long unsigned int))&mavlink_dev_ioctl; + fops.ioctl = (int (*)(file *, int, long unsigned int))&mavlink_dev_ioctl; // _parameter_handles.min_altitude = param_find("NAV_MIN_ALT"); } @@ -206,8 +214,9 @@ int Mavlink::instance_count() { /* note: a local buffer count will help if this ever is called often */ - Mavlink* inst = ::_head; + Mavlink *inst = ::_head; unsigned inst_index = 0; + while (inst != nullptr) { inst = inst->next; inst_index++; @@ -225,14 +234,17 @@ Mavlink::new_instance() /* create the first instance at _head */ if (::_head == nullptr) { ::_head = inst; - /* afterwards follow the next and append the instance */ + /* afterwards follow the next and append the instance */ + } else { while (next->next != nullptr) { next = next->next; } + /* now parent has a null pointer, fill it */ next->next = inst; } + return inst; } @@ -241,6 +253,7 @@ Mavlink::get_instance(unsigned instance) { Mavlink *inst = ::_head; unsigned inst_index = 0; + while (inst->next != nullptr && inst_index < instance) { inst = inst->next; inst_index++; @@ -277,6 +290,7 @@ Mavlink::destroy_all_instances() unsigned iterations = 0; warnx("waiting for instances to stop"); + while (next_inst != nullptr) { inst_to_del = next_inst; @@ -284,6 +298,7 @@ Mavlink::destroy_all_instances() /* set flag to stop thread and wait for all threads to finish */ inst_to_del->_task_should_exit = true; + while (inst_to_del->thread_running) { printf("."); usleep(10000); @@ -294,6 +309,7 @@ Mavlink::destroy_all_instances() return ERROR; } } + delete inst_to_del; } @@ -308,23 +324,29 @@ Mavlink::destroy_all_instances() bool Mavlink::instance_exists(const char *device_name, Mavlink *self) { - Mavlink* inst = ::_head; + Mavlink *inst = ::_head; + while (inst != nullptr) { /* don't compare with itself */ - if (inst != self && !strcmp(device_name, inst->device_name)) + if (inst != self && !strcmp(device_name, inst->device_name)) { return true; + } + inst = inst->next; } + return false; } int Mavlink::get_uart_fd(unsigned index) { - Mavlink* inst = get_instance(index); - if (inst) + Mavlink *inst = get_instance(index); + + if (inst) { return inst->get_uart_fd(); + } return -1; } @@ -353,21 +375,23 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) case (int)MAVLINK_IOC_SEND_TEXT_CRITICAL: case (int)MAVLINK_IOC_SEND_TEXT_EMERGENCY: { - const char *txt = (const char *)arg; + const char *txt = (const char *)arg; // printf("logmsg: %s\n", txt); - struct mavlink_logmessage msg; - strncpy(msg.text, txt, sizeof(msg.text)); + struct mavlink_logmessage msg; + strncpy(msg.text, txt, sizeof(msg.text)); - Mavlink* inst = ::_head; - while (inst != nullptr) { + Mavlink *inst = ::_head; - mavlink_logbuffer_write(&inst->lb, &msg); - inst->total_counter++; - inst = inst->next; + while (inst != nullptr) { + mavlink_logbuffer_write(&inst->lb, &msg); + inst->total_counter++; + inst = inst->next; + + } + + return OK; } - return OK; - } default: return ENOTTY; @@ -582,7 +606,7 @@ int Mavlink::mavlink_pm_send_param_for_name(const char *name) int Mavlink::mavlink_pm_send_param(param_t param) { - if (param == PARAM_INVALID) return 1; + if (param == PARAM_INVALID) { return 1; } /* buffers for param transmission */ static char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN]; @@ -714,38 +738,40 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item { /* only support global waypoints for now */ switch (mavlink_mission_item->frame) { - case MAV_FRAME_GLOBAL: - mission_item->lat = (double)mavlink_mission_item->x; - mission_item->lon = (double)mavlink_mission_item->y; - mission_item->altitude = mavlink_mission_item->z; - mission_item->altitude_is_relative = false; - break; + case MAV_FRAME_GLOBAL: + mission_item->lat = (double)mavlink_mission_item->x; + mission_item->lon = (double)mavlink_mission_item->y; + mission_item->altitude = mavlink_mission_item->z; + mission_item->altitude_is_relative = false; + break; - case MAV_FRAME_GLOBAL_RELATIVE_ALT: - mission_item->lat = (double)mavlink_mission_item->x; - mission_item->lon = (double)mavlink_mission_item->y; - mission_item->altitude = mavlink_mission_item->z; - mission_item->altitude_is_relative = true; - break; + case MAV_FRAME_GLOBAL_RELATIVE_ALT: + mission_item->lat = (double)mavlink_mission_item->x; + mission_item->lon = (double)mavlink_mission_item->y; + mission_item->altitude = mavlink_mission_item->z; + mission_item->altitude_is_relative = true; + break; - case MAV_FRAME_LOCAL_NED: - case MAV_FRAME_LOCAL_ENU: - return MAV_MISSION_UNSUPPORTED_FRAME; - case MAV_FRAME_MISSION: - default: - return MAV_MISSION_ERROR; + case MAV_FRAME_LOCAL_NED: + case MAV_FRAME_LOCAL_ENU: + return MAV_MISSION_UNSUPPORTED_FRAME; + + case MAV_FRAME_MISSION: + default: + return MAV_MISSION_ERROR; } switch (mavlink_mission_item->command) { - case MAV_CMD_NAV_TAKEOFF: - mission_item->pitch_min = mavlink_mission_item->param2; - break; - default: - mission_item->acceptance_radius = mavlink_mission_item->param2; - break; + case MAV_CMD_NAV_TAKEOFF: + mission_item->pitch_min = mavlink_mission_item->param2; + break; + + default: + mission_item->acceptance_radius = mavlink_mission_item->param2; + break; } - mission_item->yaw = _wrap_pi(mavlink_mission_item->param4*M_DEG_TO_RAD_F); + mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F); mission_item->loiter_radius = fabsf(mavlink_mission_item->param3); mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */ mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command; @@ -762,25 +788,27 @@ int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_ { if (mission_item->altitude_is_relative) { mavlink_mission_item->frame = MAV_FRAME_GLOBAL; + } else { mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; } - + switch (mission_item->nav_cmd) { - case NAV_CMD_TAKEOFF: - mavlink_mission_item->param2 = mission_item->pitch_min; - break; - default: - mavlink_mission_item->param2 = mission_item->acceptance_radius; - break; + case NAV_CMD_TAKEOFF: + mavlink_mission_item->param2 = mission_item->pitch_min; + break; + + default: + mavlink_mission_item->param2 = mission_item->acceptance_radius; + break; } mavlink_mission_item->x = (float)mission_item->lat; mavlink_mission_item->y = (float)mission_item->lon; mavlink_mission_item->z = mission_item->altitude; - mavlink_mission_item->param4 = mission_item->yaw*M_RAD_TO_DEG_F; - mavlink_mission_item->param3 = mission_item->loiter_radius*(float)mission_item->loiter_direction; + mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F; + mavlink_mission_item->param3 = mission_item->loiter_radius * (float)mission_item->loiter_direction; mavlink_mission_item->command = mission_item->nav_cmd; mavlink_mission_item->param1 = mission_item->time_inside; mavlink_mission_item->autocontinue = mission_item->autocontinue; @@ -817,7 +845,7 @@ void Mavlink::mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8 mavlink_msg_mission_ack_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpa); mavlink_missionlib_send_message(&msg); - if (_verbose) warnx("Sent waypoint ack (%u) to ID %u", wpa.type, wpa.target_system); + if (_verbose) { warnx("Sent waypoint ack (%u) to ID %u", wpa.type, wpa.target_system); } } /* @@ -846,7 +874,8 @@ void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq) } else { mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds"); - if (_verbose) warnx("ERROR: index out of bounds"); + + if (_verbose) { warnx("ERROR: index out of bounds"); } } } @@ -862,7 +891,7 @@ void Mavlink::mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uin mavlink_msg_mission_count_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpc); mavlink_missionlib_send_message(&msg); - if (_verbose) warnx("Sent waypoint count (%u) to ID %u", wpc.count, wpc.target_system); + if (_verbose) { warnx("Sent waypoint count (%u) to ID %u", wpc.count, wpc.target_system); } } void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq) @@ -870,11 +899,12 @@ void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t struct mission_item_s mission_item; ssize_t len = sizeof(struct mission_item_s); - + dm_item_t dm_current; if (wpm->current_dataman_id == 0) { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + } else { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; } @@ -892,10 +922,12 @@ void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t mavlink_msg_mission_item_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wp); mavlink_missionlib_send_message(&msg); - if (_verbose) warnx("Sent waypoint %u to ID %u", wp.seq, wp.target_system); + if (_verbose) { warnx("Sent waypoint %u to ID %u", wp.seq, wp.target_system); } + } else { mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR); - if (_verbose) warnx("ERROR: could not read WP%u", seq); + + if (_verbose) { warnx("ERROR: could not read WP%u", seq); } } } @@ -910,11 +942,12 @@ void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, u mavlink_msg_mission_request_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpr); mavlink_missionlib_send_message(&msg); - if (_verbose) warnx("Sent waypoint request %u to ID %u", wpr.seq, wpr.target_system); + if (_verbose) { warnx("Sent waypoint request %u to ID %u", wpr.seq, wpr.target_system); } } else { mavlink_missionlib_send_gcs_string("ERROR: Waypoint index exceeds list capacity"); - if (_verbose) warnx("ERROR: Waypoint index exceeds list capacity"); + + if (_verbose) { warnx("ERROR: Waypoint index exceeds list capacity"); } } } @@ -935,7 +968,7 @@ void Mavlink::mavlink_wpm_send_waypoint_reached(uint16_t seq) mavlink_msg_mission_item_reached_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wp_reached); mavlink_missionlib_send_message(&msg); - if (_verbose) warnx("Sent waypoint %u reached message", wp_reached.seq); + if (_verbose) { warnx("Sent waypoint %u reached message", wp_reached.seq); } } void Mavlink::mavlink_waypoint_eventloop(uint64_t now) @@ -945,7 +978,7 @@ void Mavlink::mavlink_waypoint_eventloop(uint64_t now) mavlink_missionlib_send_gcs_string("Operation timeout"); - if (_verbose) warnx("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", wpm->current_state); + if (_verbose) { warnx("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", wpm->current_state); } wpm->current_state = MAVLINK_WPM_STATE_IDLE; wpm->current_partner_sysid = 0; @@ -960,7 +993,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) switch (msg->msgid) { - case MAVLINK_MSG_ID_MISSION_ACK: { + case MAVLINK_MSG_ID_MISSION_ACK: { mavlink_mission_ack_t wpa; mavlink_msg_mission_ack_decode(msg, &wpa); @@ -977,13 +1010,14 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch"); - if (_verbose) warnx("REJ. WP CMD: curr partner id mismatch"); + + if (_verbose) { warnx("REJ. WP CMD: curr partner id mismatch"); } } break; } - case MAVLINK_MSG_ID_MISSION_SET_CURRENT: { + case MAVLINK_MSG_ID_MISSION_SET_CURRENT: { mavlink_mission_set_current_t wpc; mavlink_msg_mission_set_current_decode(msg, &wpc); @@ -995,30 +1029,33 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) mission.current_index = wpc.seq; publish_mission(); - + /* don't answer yet, wait for the navigator to respond, then publish the mission_result */ // mavlink_wpm_send_waypoint_current(wpc.seq); } else { mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list"); - if (_verbose) warnx("IGN WP CURR CMD: Not in list"); + + if (_verbose) { warnx("IGN WP CURR CMD: Not in list"); } } } else { mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy"); - if (_verbose) warnx("IGN WP CURR CMD: Busy"); + + if (_verbose) { warnx("IGN WP CURR CMD: Busy"); } } } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); - if (_verbose) warnx("REJ. WP CMD: target id mismatch"); + + if (_verbose) { warnx("REJ. WP CMD: target id mismatch"); } } break; } - case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: { + case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: { mavlink_mission_request_list_t wprl; mavlink_msg_mission_request_list_decode(msg, &wprl); @@ -1027,14 +1064,14 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (wpm->current_state == MAVLINK_WPM_STATE_IDLE || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { if (wpm->size > 0) { - + wpm->current_state = MAVLINK_WPM_STATE_SENDLIST; wpm->current_wp_id = 0; wpm->current_partner_sysid = msg->sysid; wpm->current_partner_compid = msg->compid; } else { - if (_verbose) warnx("No waypoints send"); + if (_verbose) { warnx("No waypoints send"); } } wpm->current_count = wpm->size; @@ -1042,17 +1079,20 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } else { mavlink_missionlib_send_gcs_string("IGN REQUEST LIST: Busy"); - if (_verbose) warnx("IGN REQUEST LIST: Busy"); + + if (_verbose) { warnx("IGN REQUEST LIST: Busy"); } } + } else { mavlink_missionlib_send_gcs_string("REJ. REQUEST LIST: target id mismatch"); - if (_verbose) warnx("REJ. REQUEST LIST: target id mismatch"); + + if (_verbose) { warnx("REJ. REQUEST LIST: target id mismatch"); } } break; } - case MAVLINK_MSG_ID_MISSION_REQUEST: { + case MAVLINK_MSG_ID_MISSION_REQUEST: { mavlink_mission_request_t wpr; mavlink_msg_mission_request_decode(msg, &wpr); @@ -1062,22 +1102,28 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (wpr.seq >= wpm->size) { mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list"); - if (_verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.", wpr.seq); + + if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.", wpr.seq); } + break; } - /* - * Ensure that we are in the correct state and that the first request has id 0 + /* + * Ensure that we are in the correct state and that the first request has id 0 * and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint) */ if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { if (wpr.seq == 0) { - if (_verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); + if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } + wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; + } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0"); - if (_verbose) warnx("REJ. WP CMD: First id != 0"); + + if (_verbose) { warnx("REJ. WP CMD: First id != 0"); } + break; } @@ -1085,22 +1131,26 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (wpr.seq == wpm->current_wp_id) { - if (_verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); + if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } } else if (wpr.seq == wpm->current_wp_id + 1) { - if (_verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); - + if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } + } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected"); - if (_verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).", wpr.seq, wpm->current_wp_id, wpm->current_wp_id + 1); + + if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).", wpr.seq, wpm->current_wp_id, wpm->current_wp_id + 1); } + break; } } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); - if (_verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).", wpm->current_state); + + if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).", wpm->current_state); } + break; } @@ -1109,11 +1159,12 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (wpr.seq < wpm->size) { - mavlink_wpm_send_waypoint(wpm->current_partner_sysid, wpm->current_partner_compid,wpm->current_wp_id); + mavlink_wpm_send_waypoint(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id); } else { mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR); - if (_verbose) warnx("ERROR: Waypoint %u out of bounds", wpr.seq); + + if (_verbose) { warnx("ERROR: Waypoint %u out of bounds", wpr.seq); } } @@ -1122,18 +1173,21 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if ((wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) && !(msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid)) { mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); - if (_verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.", msg->sysid, wpm->current_partner_sysid); + + if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.", msg->sysid, wpm->current_partner_sysid); } } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); - if (_verbose) warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); + + if (_verbose) { warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } } } + break; } - case MAVLINK_MSG_ID_MISSION_COUNT: { + case MAVLINK_MSG_ID_MISSION_COUNT: { mavlink_mission_count_t wpc; mavlink_msg_mission_count_decode(msg, &wpc); @@ -1143,18 +1197,21 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) { if (wpc.count > NUM_MISSIONS_SUPPORTED) { - if (_verbose) warnx("Too many waypoints: %d, supported: %d", wpc.count, NUM_MISSIONS_SUPPORTED); + if (_verbose) { warnx("Too many waypoints: %d, supported: %d", wpc.count, NUM_MISSIONS_SUPPORTED); } + mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_NO_SPACE); break; } if (wpc.count == 0) { mavlink_missionlib_send_gcs_string("COUNT 0"); - if (_verbose) warnx("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE"); + + if (_verbose) { warnx("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE"); } + break; } - - if (_verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid); + + if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid); } wpm->current_state = MAVLINK_WPM_STATE_GETLIST; wpm->current_wp_id = 0; @@ -1168,24 +1225,31 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (wpm->current_wp_id == 0) { mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN"); - if (_verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) again from %u", wpc.count, msg->sysid); + + if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) again from %u", wpc.count, msg->sysid); } + } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); - if (_verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.", wpm->current_wp_id); + + if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.", wpm->current_wp_id); } } + } else { - mavlink_missionlib_send_gcs_string("IGN MISSION_COUNT CMD: Busy"); - if (_verbose) warnx("IGN MISSION_COUNT CMD: Busy"); + mavlink_missionlib_send_gcs_string("IGN MISSION_COUNT CMD: Busy"); + + if (_verbose) { warnx("IGN MISSION_COUNT CMD: Busy"); } } + } else { mavlink_missionlib_send_gcs_string("REJ. WP COUNT CMD: target id mismatch"); - if (_verbose) warnx("IGNORED WAYPOINT COUNT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); + + if (_verbose) { warnx("IGNORED WAYPOINT COUNT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } } } break; - case MAVLINK_MSG_ID_MISSION_ITEM: { + case MAVLINK_MSG_ID_MISSION_ITEM: { mavlink_mission_item_t wp; mavlink_msg_mission_item_decode(msg, &wp); @@ -1200,11 +1264,12 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST) { - if (wp.seq != 0) { - mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP not 0"); - warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.", wp.seq); - break; - } + if (wp.seq != 0) { + mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP not 0"); + warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.", wp.seq); + break; + } + } else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { if (wp.seq >= wpm->current_count) { @@ -1239,6 +1304,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (wpm->current_dataman_id == 0) { dm_next = DM_KEY_WAYPOINTS_OFFBOARD_1; mission.dataman_id = 1; + } else { dm_next = DM_KEY_WAYPOINTS_OFFBOARD_0; mission.dataman_id = 0; @@ -1260,13 +1326,13 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) wpm->current_wp_id = wp.seq + 1; if (wpm->current_wp_id == wpm->current_count && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { - - if (_verbose) warnx("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE", wpm->current_count); + + if (_verbose) { warnx("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE", wpm->current_count); } mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED); mission.count = wpm->current_count; - + publish_mission(); wpm->current_dataman_id = mission.dataman_id; @@ -1280,13 +1346,14 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); - if (_verbose) warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); + + if (_verbose) { warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } } break; } - case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: { + case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: { mavlink_mission_clear_all_t wpca; mavlink_msg_mission_clear_all_decode(msg, &wpca); @@ -1305,21 +1372,24 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_0) == OK && dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_1) == OK) { mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED); + } else { mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR); } - + } else { mavlink_missionlib_send_gcs_string("IGN WP CLEAR CMD: Busy"); - if (_verbose) warnx("IGN WP CLEAR CMD: Busy"); + + if (_verbose) { warnx("IGN WP CLEAR CMD: Busy"); } } } else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm->current_state != MAVLINK_WPM_STATE_IDLE) { mavlink_missionlib_send_gcs_string("REJ. WP CLERR CMD: target id mismatch"); - if (_verbose) warnx("IGNORED WAYPOINT CLEAR COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); + + if (_verbose) { warnx("IGNORED WAYPOINT CLEAR COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH"); } } break; @@ -1352,8 +1422,9 @@ Mavlink::mavlink_missionlib_send_gcs_string(const char *string) while (i < len - 1) { statustext.text[i] = string[i]; - if (string[i] == '\0') + if (string[i] == '\0') { break; + } i++; } @@ -1411,6 +1482,7 @@ Mavlink::configure_stream(const char *stream_name, const float rate) LL_DELETE(_streams, stream); delete stream; } + return OK; } } @@ -1466,18 +1538,22 @@ Mavlink::task_main(int argc, char *argv[]) switch (ch) { case 'b': _baudrate = strtoul(optarg, NULL, 10); + if (_baudrate < 9600 || _baudrate > 921600) { warnx("invalid baud rate '%s'", optarg); err_flag = true; } + break; case 'r': _datarate = strtoul(optarg, NULL, 10); + if (_datarate < 10 || _datarate > MAX_DATA_RATE) { warnx("invalid data rate '%s'", optarg); err_flag = true; } + break; case 'd': @@ -1501,6 +1577,7 @@ Mavlink::task_main(int argc, char *argv[]) } else if (strcmp(optarg, "custom") == 0) { _mode = MODE_CUSTOM; } + break; case 'v': @@ -1533,35 +1610,41 @@ Mavlink::task_main(int argc, char *argv[]) /* inform about mode */ switch (_mode) { - case MODE_CUSTOM: - warnx("mode: CUSTOM"); - break; - case MODE_OFFBOARD: - warnx("mode: OFFBOARD"); - break; - case MODE_ONBOARD: - warnx("mode: ONBOARD"); - break; - case MODE_HIL: - warnx("mode: HIL"); - break; - default: - warnx("ERROR: Unknown mode"); - break; + case MODE_CUSTOM: + warnx("mode: CUSTOM"); + break; + + case MODE_OFFBOARD: + warnx("mode: OFFBOARD"); + break; + + case MODE_ONBOARD: + warnx("mode: ONBOARD"); + break; + + case MODE_HIL: + warnx("mode: HIL"); + break; + + default: + warnx("ERROR: Unknown mode"); + break; } - switch(_mode) { - case MODE_OFFBOARD: - case MODE_HIL: - case MODE_CUSTOM: - _mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER; - break; - case MODE_ONBOARD: - _mavlink_wpm_comp_id = MAV_COMP_ID_CAMERA; - break; - default: - _mavlink_wpm_comp_id = MAV_COMP_ID_ALL; - break; + switch (_mode) { + case MODE_OFFBOARD: + case MODE_HIL: + case MODE_CUSTOM: + _mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER; + break; + + case MODE_ONBOARD: + _mavlink_wpm_comp_id = MAV_COMP_ID_CAMERA; + break; + + default: + _mavlink_wpm_comp_id = MAV_COMP_ID_ALL; + break; } warnx("data rate: %d bytes/s", _datarate); @@ -1576,8 +1659,9 @@ Mavlink::task_main(int argc, char *argv[]) /* default values for arguments */ _uart = mavlink_open_uart(_baudrate, device_name, &uart_config_original, &usb_uart); - if (_uart < 0) + if (_uart < 0) { err(1, "could not open %s", device_name); + } /* create the device node that's used for sending text log messages, etc. */ if (instance_count() == 1) { @@ -1615,7 +1699,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("HEARTBEAT", 1.0f); - switch(_mode) { + switch (_mode) { case MODE_OFFBOARD: configure_stream("SYS_STATUS", 1.0f); configure_stream("GPS_GLOBAL_ORIGIN", 0.5f * rate_mult); @@ -1660,10 +1744,12 @@ Mavlink::task_main(int argc, char *argv[]) if (status_sub->update(t)) { /* switch HIL mode if required */ - if (status->hil_state == HIL_STATE_ON) + if (status->hil_state == HIL_STATE_ON) { set_hil_enabled(true); - else if (status->hil_state == HIL_STATE_OFF) + + } else if (status->hil_state == HIL_STATE_OFF) { set_hil_enabled(false); + } } MavlinkStream *stream; @@ -1677,13 +1763,14 @@ Mavlink::task_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result); - if (_verbose) warnx("Got mission result: new current: %d", mission_result.index_current_mission); + if (_verbose) { warnx("Got mission result: new current: %d", mission_result.index_current_mission); } if (mission_result.mission_reached) { mavlink_wpm_send_waypoint_reached((uint16_t)mission_result.mission_index_reached); } mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission); + } else { if (slow_rate_limiter.check(t)) { mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission); @@ -1740,11 +1827,11 @@ Mavlink::start(int argc, char *argv[]) sprintf(buf, "mavlink_if%d", Mavlink::instance_count()); /*mavlink->_mavlink_task = */task_spawn_cmd(buf, - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 2048, - (main_t)&Mavlink::start_helper, - (const char **)argv); + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2048, + (main_t)&Mavlink::start_helper, + (const char **)argv); // while (!this->is_running()) { // usleep(200); @@ -1775,7 +1862,7 @@ Mavlink::start(int argc, char *argv[]) } void -Mavlink::status() +Mavlink::status() { warnx("running"); } @@ -1799,9 +1886,11 @@ Mavlink::stream(int argc, char *argv[]) switch (ch) { case 'r': rate = strtod(optarg, nullptr); + if (rate < 0.0f) { err_flag = true; } + break; case 'd': @@ -1820,6 +1909,7 @@ Mavlink::stream(int argc, char *argv[]) if (!err_flag && rate >= 0.0 && stream_name != nullptr) { Mavlink *inst = get_instance_for_device(device_name); + if (inst != nullptr) { if (OK == inst->configure_stream(stream_name, rate)) { if (rate > 0.0f) { @@ -1865,15 +1955,15 @@ int mavlink_main(int argc, char *argv[]) } else if (!strcmp(argv[1], "stop-all")) { return Mavlink::destroy_all_instances(); - // } else if (!strcmp(argv[1], "status")) { - // mavlink::g_mavlink->status(); + // } else if (!strcmp(argv[1], "status")) { + // mavlink::g_mavlink->status(); } else if (!strcmp(argv[1], "stream")) { return Mavlink::stream(argc, argv); - } else { - usage(); - } + } else { + usage(); + } return 0; } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 41e781ee8f..532c9bcee3 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -36,6 +36,7 @@ * MAVLink 1.0 protocol interface definition. * * @author Lorenz Meier + * @author Anton Babushkin */ #pragma once @@ -55,24 +56,24 @@ #include "mavlink_stream.h" #include "mavlink_messages.h" - // FIXME XXX - TO BE MOVED TO XML +// FIXME XXX - TO BE MOVED TO XML enum MAVLINK_WPM_STATES { - MAVLINK_WPM_STATE_IDLE = 0, - MAVLINK_WPM_STATE_SENDLIST, - MAVLINK_WPM_STATE_SENDLIST_SENDWPS, - MAVLINK_WPM_STATE_GETLIST, - MAVLINK_WPM_STATE_GETLIST_GETWPS, - MAVLINK_WPM_STATE_GETLIST_GOTALL, - MAVLINK_WPM_STATE_ENUM_END + MAVLINK_WPM_STATE_IDLE = 0, + MAVLINK_WPM_STATE_SENDLIST, + MAVLINK_WPM_STATE_SENDLIST_SENDWPS, + MAVLINK_WPM_STATE_GETLIST, + MAVLINK_WPM_STATE_GETLIST_GETWPS, + MAVLINK_WPM_STATE_GETLIST_GOTALL, + MAVLINK_WPM_STATE_ENUM_END }; enum MAVLINK_WPM_CODES { - MAVLINK_WPM_CODE_OK = 0, - MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED, - MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED, - MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS, - MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED, - MAVLINK_WPM_CODE_ENUM_END + MAVLINK_WPM_CODE_OK = 0, + MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED, + MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED, + MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS, + MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED, + MAVLINK_WPM_CODE_ENUM_END }; @@ -144,7 +145,7 @@ public: const char *device_name; enum MAVLINK_MODE { - MODE_CUSTOM=0, + MODE_CUSTOM = 0, MODE_OFFBOARD, MODE_ONBOARD, MODE_HIL @@ -186,7 +187,7 @@ public: bool _task_should_exit; /**< if true, mavlink task should exit */ protected: - Mavlink* next; + Mavlink *next; private: int _mavlink_fd; @@ -233,7 +234,7 @@ private: unsigned int mavlink_param_queue_index; bool mavlink_link_termination_allowed; - + /** * Send one parameter. * diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 8097ecdb36..820faae1cc 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1,8 +1,42 @@ -/* - * mavlink_messages.cpp +/**************************************************************************** * - * Created on: 25.02.2014 - * Author: ton + * 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. + * + ****************************************************************************/ + +/** + * @file mavlink_messages.cpp + * MAVLink 1.0 message formatters implementation. + * + * @author Lorenz Meier + * @author Anton Babushkin */ #include @@ -43,7 +77,7 @@ static uint16_t cm_uint16_from_m_float(float m); static void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, - uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode); + uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode); uint16_t cm_uint16_from_m_float(float m) @@ -59,7 +93,7 @@ cm_uint16_from_m_float(float m) } void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, - uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode) + uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode) { *mavlink_state = 0; *mavlink_base_mode = 0; @@ -72,7 +106,7 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set /* arming state */ if (status->arming_state == ARMING_STATE_ARMED - || status->arming_state == ARMING_STATE_ARMED_ERROR) { + || status->arming_state == ARMING_STATE_ARMED_ERROR) { *mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } @@ -81,34 +115,44 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set union px4_custom_mode custom_mode; custom_mode.data = 0; + if (pos_sp_triplet->nav_state == NAV_STATE_NONE) { - /* use main state when navigator is not active */ + /* use main state when navigator is not active */ if (status->main_state == MAIN_STATE_MANUAL) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; + } else if (status->main_state == MAIN_STATE_SEATBELT) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT; + } else if (status->main_state == MAIN_STATE_EASY) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY; + } else if (status->main_state == MAIN_STATE_AUTO) { *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; } + } else { /* use navigation state when navigator is active */ *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + if (pos_sp_triplet->nav_state == NAV_STATE_READY) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; + } else if (pos_sp_triplet->nav_state == NAV_STATE_LOITER) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; + } else if (pos_sp_triplet->nav_state == NAV_STATE_MISSION) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; + } else if (pos_sp_triplet->nav_state == NAV_STATE_RTL) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; + } else if (pos_sp_triplet->nav_state == NAV_STATE_LAND) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; } @@ -118,24 +162,30 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set /* set system state */ if (status->arming_state == ARMING_STATE_INIT - || status->arming_state == ARMING_STATE_IN_AIR_RESTORE - || status->arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review + || status->arming_state == ARMING_STATE_IN_AIR_RESTORE + || status->arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review *mavlink_state = MAV_STATE_UNINIT; + } else if (status->arming_state == ARMING_STATE_ARMED) { *mavlink_state = MAV_STATE_ACTIVE; + } else if (status->arming_state == ARMING_STATE_ARMED_ERROR) { *mavlink_state = MAV_STATE_CRITICAL; + } else if (status->arming_state == ARMING_STATE_STANDBY) { *mavlink_state = MAV_STATE_STANDBY; + } else if (status->arming_state == ARMING_STATE_REBOOT) { *mavlink_state = MAV_STATE_POWEROFF; + } else { *mavlink_state = MAV_STATE_CRITICAL; } } -class MavlinkStreamHeartbeat : public MavlinkStream { +class MavlinkStreamHeartbeat : public MavlinkStream +{ public: const char *get_name() { @@ -164,7 +214,8 @@ protected: pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data(); } - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { status_sub->update(t); pos_sp_triplet_sub->update(t); @@ -184,7 +235,8 @@ protected: }; -class MavlinkStreamSysStatus : public MavlinkStream { +class MavlinkStreamSysStatus : public MavlinkStream +{ public: const char *get_name() { @@ -207,28 +259,30 @@ protected: status = (struct vehicle_status_s *)status_sub->get_data(); } - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { status_sub->update(t); mavlink_msg_sys_status_send(_channel, - status->onboard_control_sensors_present, - status->onboard_control_sensors_enabled, - status->onboard_control_sensors_health, - status->load * 1000.0f, - status->battery_voltage * 1000.0f, - status->battery_current * 1000.0f, - status->battery_remaining, - status->drop_rate_comm, - status->errors_comm, - status->errors_count1, - status->errors_count2, - status->errors_count3, - status->errors_count4); + status->onboard_control_sensors_present, + status->onboard_control_sensors_enabled, + status->onboard_control_sensors_health, + status->load * 1000.0f, + status->battery_voltage * 1000.0f, + status->battery_current * 1000.0f, + status->battery_remaining, + status->drop_rate_comm, + status->errors_comm, + status->errors_count1, + status->errors_count2, + status->errors_count3, + status->errors_count4); } }; -class MavlinkStreamHighresIMU : public MavlinkStream { +class MavlinkStreamHighresIMU : public MavlinkStream +{ public: MavlinkStreamHighresIMU() : MavlinkStream(), accel_counter(0), gyro_counter(0), mag_counter(0), baro_counter(0) { @@ -260,7 +314,8 @@ protected: sensor = (struct sensor_combined_s *)sensor_sub->get_data(); } - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { sensor_sub->update(t); uint16_t fields_updated = 0; @@ -290,18 +345,19 @@ protected: } mavlink_msg_highres_imu_send(_channel, - sensor->timestamp, - sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2], - sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2], - sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2], - sensor->baro_pres_mbar, sensor->differential_pressure_pa, - sensor->baro_alt_meter, sensor->baro_temp_celcius, - fields_updated); + sensor->timestamp, + sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2], + sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2], + sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2], + sensor->baro_pres_mbar, sensor->differential_pressure_pa, + sensor->baro_alt_meter, sensor->baro_temp_celcius, + fields_updated); } }; -class MavlinkStreamAttitude : public MavlinkStream { +class MavlinkStreamAttitude : public MavlinkStream +{ public: const char *get_name() { @@ -324,18 +380,20 @@ protected: att = (struct vehicle_attitude_s *)att_sub->get_data(); } - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { att_sub->update(t); mavlink_msg_attitude_send(_channel, - att->timestamp / 1000, - att->roll, att->pitch, att->yaw, - att->rollspeed, att->pitchspeed, att->yawspeed); + att->timestamp / 1000, + att->roll, att->pitch, att->yaw, + att->rollspeed, att->pitchspeed, att->yawspeed); } }; -class MavlinkStreamAttitudeQuaternion : public MavlinkStream { +class MavlinkStreamAttitudeQuaternion : public MavlinkStream +{ public: const char *get_name() { @@ -358,23 +416,25 @@ protected: att = (struct vehicle_attitude_s *)att_sub->get_data(); } - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { att_sub->update(t); mavlink_msg_attitude_quaternion_send(_channel, - att->timestamp / 1000, - att->q[0], - att->q[1], - att->q[2], - att->q[3], - att->rollspeed, - att->pitchspeed, - att->yawspeed); + att->timestamp / 1000, + att->q[0], + att->q[1], + att->q[2], + att->q[3], + att->rollspeed, + att->pitchspeed, + att->yawspeed); } }; -class MavlinkStreamVFRHUD : public MavlinkStream { +class MavlinkStreamVFRHUD : public MavlinkStream +{ public: const char *get_name() { @@ -421,7 +481,8 @@ protected: airspeed = (struct airspeed_s *)airspeed_sub->get_data(); } - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { att_sub->update(t); pos_sub->update(t); armed_sub->update(t); @@ -433,17 +494,18 @@ protected: float throttle = armed->armed ? act->control[3] * 100.0f : 0.0f; mavlink_msg_vfr_hud_send(_channel, - airspeed->true_airspeed_m_s, - groundspeed, - heading, - throttle, - pos->alt, - -pos->vel_d); + airspeed->true_airspeed_m_s, + groundspeed, + heading, + throttle, + pos->alt, + -pos->vel_d); } }; -class MavlinkStreamGPSRawInt : public MavlinkStream { +class MavlinkStreamGPSRawInt : public MavlinkStream +{ public: const char *get_name() { @@ -466,25 +528,27 @@ protected: gps = (struct vehicle_gps_position_s *)gps_sub->get_data(); } - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { gps_sub->update(t); mavlink_msg_gps_raw_int_send(_channel, - gps->timestamp_position, - gps->fix_type, - gps->lat, - gps->lon, - gps->alt, - cm_uint16_from_m_float(gps->eph_m), - cm_uint16_from_m_float(gps->epv_m), - gps->vel_m_s * 100.0f, - _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f, - gps->satellites_visible); + gps->timestamp_position, + gps->fix_type, + gps->lat, + gps->lon, + gps->alt, + cm_uint16_from_m_float(gps->eph_m), + cm_uint16_from_m_float(gps->epv_m), + gps->vel_m_s * 100.0f, + _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f, + gps->satellites_visible); } }; -class MavlinkStreamGlobalPositionInt : public MavlinkStream { +class MavlinkStreamGlobalPositionInt : public MavlinkStream +{ public: const char *get_name() { @@ -513,25 +577,27 @@ protected: home = (struct home_position_s *)home_sub->get_data(); } - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { pos_sub->update(t); home_sub->update(t); mavlink_msg_global_position_int_send(_channel, - pos->timestamp / 1000, - pos->lat * 1e7, - pos->lon * 1e7, - pos->alt * 1000.0f, - (pos->alt - home->alt) * 1000.0f, - pos->vel_n * 100.0f, - pos->vel_e * 100.0f, - pos->vel_d * 100.0f, - _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f); + pos->timestamp / 1000, + pos->lat * 1e7, + pos->lon * 1e7, + pos->alt * 1000.0f, + (pos->alt - home->alt) * 1000.0f, + pos->vel_n * 100.0f, + pos->vel_e * 100.0f, + pos->vel_d * 100.0f, + _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f); } }; -class MavlinkStreamLocalPositionNED : public MavlinkStream { +class MavlinkStreamLocalPositionNED : public MavlinkStream +{ public: const char *get_name() { @@ -554,22 +620,24 @@ protected: pos = (struct vehicle_local_position_s *)pos_sub->get_data(); } - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { pos_sub->update(t); mavlink_msg_local_position_ned_send(_channel, - pos->timestamp / 1000, - pos->x, - pos->y, - pos->z, - pos->vx, - pos->vy, - pos->vz); + pos->timestamp / 1000, + pos->x, + pos->y, + pos->z, + pos->vx, + pos->vy, + pos->vz); } }; -class MavlinkStreamGPSGlobalOrigin : public MavlinkStream { +class MavlinkStreamGPSGlobalOrigin : public MavlinkStream +{ public: const char *get_name() { @@ -592,18 +660,20 @@ protected: home = (struct home_position_s *)home_sub->get_data(); } - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { home_sub->update(t); mavlink_msg_gps_global_origin_send(_channel, - (int32_t)(home->lat * 1e7), - (int32_t)(home->lon * 1e7), - (int32_t)(home->alt) * 1000.0f); + (int32_t)(home->lat * 1e7), + (int32_t)(home->lon * 1e7), + (int32_t)(home->alt) * 1000.0f); } }; -class MavlinkStreamServoOutputRaw : public MavlinkStream { +class MavlinkStreamServoOutputRaw : public MavlinkStream +{ public: MavlinkStreamServoOutputRaw(unsigned int n) : MavlinkStream(), _n(n) { @@ -641,25 +711,27 @@ protected: act = (struct actuator_outputs_s *)act_sub->get_data(); } - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { act_sub->update(t); mavlink_msg_servo_output_raw_send(_channel, - act->timestamp / 1000, - _n, - act->output[0], - act->output[1], - act->output[2], - act->output[3], - act->output[4], - act->output[5], - act->output[6], - act->output[7]); + act->timestamp / 1000, + _n, + act->output[0], + act->output[1], + act->output[2], + act->output[3], + act->output[4], + act->output[5], + act->output[6], + act->output[7]); } }; -class MavlinkStreamHILControls : public MavlinkStream { +class MavlinkStreamHILControls : public MavlinkStream +{ public: const char *get_name() { @@ -694,7 +766,8 @@ protected: act = (struct actuator_outputs_s *)act_sub->get_data(); } - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { status_sub->update(t); pos_sp_triplet_sub->update(t); act_sub->update(t); @@ -710,65 +783,66 @@ protected: /* scale / assign outputs depending on system type */ if (mavlink_system.type == MAV_TYPE_QUADROTOR) { mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - ((act->output[0] - 900.0f) / 600.0f) / 2.0f, - ((act->output[1] - 900.0f) / 600.0f) / 2.0f, - ((act->output[2] - 900.0f) / 600.0f) / 2.0f, - ((act->output[3] - 900.0f) / 600.0f) / 2.0f, - -1, - -1, - -1, - -1, - mavlink_base_mode, - 0); + hrt_absolute_time(), + ((act->output[0] - 900.0f) / 600.0f) / 2.0f, + ((act->output[1] - 900.0f) / 600.0f) / 2.0f, + ((act->output[2] - 900.0f) / 600.0f) / 2.0f, + ((act->output[3] - 900.0f) / 600.0f) / 2.0f, + -1, + -1, + -1, + -1, + mavlink_base_mode, + 0); } else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) { mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - ((act->output[0] - 900.0f) / 600.0f) / 2.0f, - ((act->output[1] - 900.0f) / 600.0f) / 2.0f, - ((act->output[2] - 900.0f) / 600.0f) / 2.0f, - ((act->output[3] - 900.0f) / 600.0f) / 2.0f, - ((act->output[4] - 900.0f) / 600.0f) / 2.0f, - ((act->output[5] - 900.0f) / 600.0f) / 2.0f, - -1, - -1, - mavlink_base_mode, - 0); + hrt_absolute_time(), + ((act->output[0] - 900.0f) / 600.0f) / 2.0f, + ((act->output[1] - 900.0f) / 600.0f) / 2.0f, + ((act->output[2] - 900.0f) / 600.0f) / 2.0f, + ((act->output[3] - 900.0f) / 600.0f) / 2.0f, + ((act->output[4] - 900.0f) / 600.0f) / 2.0f, + ((act->output[5] - 900.0f) / 600.0f) / 2.0f, + -1, + -1, + mavlink_base_mode, + 0); } else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) { mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - ((act->output[0] - 900.0f) / 600.0f) / 2.0f, - ((act->output[1] - 900.0f) / 600.0f) / 2.0f, - ((act->output[2] - 900.0f) / 600.0f) / 2.0f, - ((act->output[3] - 900.0f) / 600.0f) / 2.0f, - ((act->output[4] - 900.0f) / 600.0f) / 2.0f, - ((act->output[5] - 900.0f) / 600.0f) / 2.0f, - ((act->output[6] - 900.0f) / 600.0f) / 2.0f, - ((act->output[7] - 900.0f) / 600.0f) / 2.0f, - mavlink_base_mode, - 0); + hrt_absolute_time(), + ((act->output[0] - 900.0f) / 600.0f) / 2.0f, + ((act->output[1] - 900.0f) / 600.0f) / 2.0f, + ((act->output[2] - 900.0f) / 600.0f) / 2.0f, + ((act->output[3] - 900.0f) / 600.0f) / 2.0f, + ((act->output[4] - 900.0f) / 600.0f) / 2.0f, + ((act->output[5] - 900.0f) / 600.0f) / 2.0f, + ((act->output[6] - 900.0f) / 600.0f) / 2.0f, + ((act->output[7] - 900.0f) / 600.0f) / 2.0f, + mavlink_base_mode, + 0); } else { mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - (act->output[0] - 1500.0f) / 500.0f, - (act->output[1] - 1500.0f) / 500.0f, - (act->output[2] - 1500.0f) / 500.0f, - (act->output[3] - 1000.0f) / 1000.0f, - (act->output[4] - 1500.0f) / 500.0f, - (act->output[5] - 1500.0f) / 500.0f, - (act->output[6] - 1500.0f) / 500.0f, - (act->output[7] - 1500.0f) / 500.0f, - mavlink_base_mode, - 0); + hrt_absolute_time(), + (act->output[0] - 1500.0f) / 500.0f, + (act->output[1] - 1500.0f) / 500.0f, + (act->output[2] - 1500.0f) / 500.0f, + (act->output[3] - 1000.0f) / 1000.0f, + (act->output[4] - 1500.0f) / 500.0f, + (act->output[5] - 1500.0f) / 500.0f, + (act->output[6] - 1500.0f) / 500.0f, + (act->output[7] - 1500.0f) / 500.0f, + mavlink_base_mode, + 0); } } }; -class MavlinkStreamGlobalPositionSetpointInt : public MavlinkStream { +class MavlinkStreamGlobalPositionSetpointInt : public MavlinkStream +{ public: const char *get_name() { @@ -791,7 +865,8 @@ protected: pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data(); } - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { pos_sp_triplet_sub->update(t); mavlink_msg_global_position_setpoint_int_send(_channel, @@ -804,7 +879,8 @@ protected: }; -class MavlinkStreamLocalPositionSetpoint : public MavlinkStream { +class MavlinkStreamLocalPositionSetpoint : public MavlinkStream +{ public: const char *get_name() { @@ -827,7 +903,8 @@ protected: pos_sp = (struct vehicle_local_position_setpoint_s *)pos_sp_sub->get_data(); } - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { pos_sp_sub->update(t); mavlink_msg_local_position_setpoint_send(_channel, @@ -840,7 +917,8 @@ protected: }; -class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream { +class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream +{ public: const char *get_name() { @@ -863,7 +941,8 @@ protected: att_sp = (struct vehicle_attitude_setpoint_s *)att_sp_sub->get_data(); } - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { att_sp_sub->update(t); mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel, @@ -876,7 +955,8 @@ protected: }; -class MavlinkStreamRollPitchYawRatesThrustSetpoint : public MavlinkStream { +class MavlinkStreamRollPitchYawRatesThrustSetpoint : public MavlinkStream +{ public: const char *get_name() { @@ -899,7 +979,8 @@ protected: att_rates_sp = (struct vehicle_rates_setpoint_s *)att_rates_sp_sub->get_data(); } - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { att_rates_sp_sub->update(t); mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel, @@ -912,7 +993,8 @@ protected: }; -class MavlinkStreamRCChannelsRaw : public MavlinkStream { +class MavlinkStreamRCChannelsRaw : public MavlinkStream +{ public: const char *get_name() { @@ -935,7 +1017,8 @@ protected: rc = (struct rc_input_values *)rc_sub->get_data(); } - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { rc_sub->update(t); const unsigned port_width = 8; @@ -943,23 +1026,24 @@ protected: for (unsigned i = 0; (i * port_width) < rc->channel_count; i++) { /* Channels are sent in MAVLink main loop at a fixed interval */ mavlink_msg_rc_channels_raw_send(_channel, - rc->timestamp_publication / 1000, - i, - (rc->channel_count > (i * port_width) + 0) ? rc->values[(i * port_width) + 0] : UINT16_MAX, - (rc->channel_count > (i * port_width) + 1) ? rc->values[(i * port_width) + 1] : UINT16_MAX, - (rc->channel_count > (i * port_width) + 2) ? rc->values[(i * port_width) + 2] : UINT16_MAX, - (rc->channel_count > (i * port_width) + 3) ? rc->values[(i * port_width) + 3] : UINT16_MAX, - (rc->channel_count > (i * port_width) + 4) ? rc->values[(i * port_width) + 4] : UINT16_MAX, - (rc->channel_count > (i * port_width) + 5) ? rc->values[(i * port_width) + 5] : UINT16_MAX, - (rc->channel_count > (i * port_width) + 6) ? rc->values[(i * port_width) + 6] : UINT16_MAX, - (rc->channel_count > (i * port_width) + 7) ? rc->values[(i * port_width) + 7] : UINT16_MAX, - rc->rssi); + rc->timestamp_publication / 1000, + i, + (rc->channel_count > (i * port_width) + 0) ? rc->values[(i * port_width) + 0] : UINT16_MAX, + (rc->channel_count > (i * port_width) + 1) ? rc->values[(i * port_width) + 1] : UINT16_MAX, + (rc->channel_count > (i * port_width) + 2) ? rc->values[(i * port_width) + 2] : UINT16_MAX, + (rc->channel_count > (i * port_width) + 3) ? rc->values[(i * port_width) + 3] : UINT16_MAX, + (rc->channel_count > (i * port_width) + 4) ? rc->values[(i * port_width) + 4] : UINT16_MAX, + (rc->channel_count > (i * port_width) + 5) ? rc->values[(i * port_width) + 5] : UINT16_MAX, + (rc->channel_count > (i * port_width) + 6) ? rc->values[(i * port_width) + 6] : UINT16_MAX, + (rc->channel_count > (i * port_width) + 7) ? rc->values[(i * port_width) + 7] : UINT16_MAX, + rc->rssi); } } }; -class MavlinkStreamManualControl : public MavlinkStream { +class MavlinkStreamManualControl : public MavlinkStream +{ public: const char *get_name() { @@ -982,43 +1066,44 @@ protected: manual = (struct manual_control_setpoint_s *)manual_sub->get_data(); } - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { manual_sub->update(t); mavlink_msg_manual_control_send(_channel, - mavlink_system.sysid, - manual->roll * 1000, - manual->pitch * 1000, - manual->yaw * 1000, - manual->throttle * 1000, - 0); + mavlink_system.sysid, + manual->roll * 1000, + manual->pitch * 1000, + manual->yaw * 1000, + manual->throttle * 1000, + 0); } }; MavlinkStream *streams_list[] = { - new MavlinkStreamHeartbeat(), - new MavlinkStreamSysStatus(), - new MavlinkStreamHighresIMU(), - new MavlinkStreamAttitude(), - new MavlinkStreamAttitudeQuaternion(), - new MavlinkStreamVFRHUD(), - new MavlinkStreamGPSRawInt(), - new MavlinkStreamGlobalPositionInt(), - new MavlinkStreamLocalPositionNED(), - new MavlinkStreamGPSGlobalOrigin(), - new MavlinkStreamServoOutputRaw(0), - new MavlinkStreamServoOutputRaw(1), - new MavlinkStreamServoOutputRaw(2), - new MavlinkStreamServoOutputRaw(3), - new MavlinkStreamHILControls(), - new MavlinkStreamGlobalPositionSetpointInt(), - new MavlinkStreamLocalPositionSetpoint(), - new MavlinkStreamRollPitchYawThrustSetpoint(), - new MavlinkStreamRollPitchYawRatesThrustSetpoint(), - new MavlinkStreamRCChannelsRaw(), - new MavlinkStreamManualControl(), - nullptr + new MavlinkStreamHeartbeat(), + new MavlinkStreamSysStatus(), + new MavlinkStreamHighresIMU(), + new MavlinkStreamAttitude(), + new MavlinkStreamAttitudeQuaternion(), + new MavlinkStreamVFRHUD(), + new MavlinkStreamGPSRawInt(), + new MavlinkStreamGlobalPositionInt(), + new MavlinkStreamLocalPositionNED(), + new MavlinkStreamGPSGlobalOrigin(), + new MavlinkStreamServoOutputRaw(0), + new MavlinkStreamServoOutputRaw(1), + new MavlinkStreamServoOutputRaw(2), + new MavlinkStreamServoOutputRaw(3), + new MavlinkStreamHILControls(), + new MavlinkStreamGlobalPositionSetpointInt(), + new MavlinkStreamLocalPositionSetpoint(), + new MavlinkStreamRollPitchYawThrustSetpoint(), + new MavlinkStreamRollPitchYawRatesThrustSetpoint(), + new MavlinkStreamRCChannelsRaw(), + new MavlinkStreamManualControl(), + nullptr }; diff --git a/src/modules/mavlink/mavlink_messages.h b/src/modules/mavlink/mavlink_messages.h index 3dc6cb6998..b8823263a8 100644 --- a/src/modules/mavlink/mavlink_messages.h +++ b/src/modules/mavlink/mavlink_messages.h @@ -1,8 +1,41 @@ -/* - * mavlink_messages.h +/**************************************************************************** * - * Created on: 25.02.2014 - * Author: ton + * 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. + * + ****************************************************************************/ + +/** + * @file mavlink_messages.h + * MAVLink 1.0 message formatters definition. + * + * @author Anton Babushkin */ #ifndef MAVLINK_MESSAGES_H_ diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp index 35470a19a1..e1208bca9b 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.cpp +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -1,10 +1,42 @@ -/* - * mavlink_orb_subscription.cpp +/**************************************************************************** * - * Created on: 23.02.2014 - * Author: ton - */ + * 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_orb_subscription.cpp + * uORB subscription implementation. + * + * @author Anton Babushkin + */ #include #include @@ -45,10 +77,12 @@ MavlinkOrbSubscription::update(const hrt_abstime t) _last_check = t; bool updated; orb_check(_fd, &updated); + if (updated) { orb_copy(_topic, _fd, _data); return true; } } + return false; } diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h index 79ff3abdbe..3cf33ccef2 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.h +++ b/src/modules/mavlink/mavlink_orb_subscription.h @@ -1,8 +1,41 @@ -/* - * mavlink_orb_subscription.h +/**************************************************************************** * - * Created on: 23.02.2014 - * Author: ton + * 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_orb_subscription.h + * uORB subscription definition. + * + * @author Anton Babushkin */ #ifndef MAVLINK_ORB_SUBSCRIPTION_H_ @@ -12,7 +45,8 @@ #include -class MavlinkOrbSubscription { +class MavlinkOrbSubscription +{ public: MavlinkOrbSubscription *next; diff --git a/src/modules/mavlink/mavlink_rate_limiter.cpp b/src/modules/mavlink/mavlink_rate_limiter.cpp index f5bb06ccd2..f6ed6e6627 100644 --- a/src/modules/mavlink/mavlink_rate_limiter.cpp +++ b/src/modules/mavlink/mavlink_rate_limiter.cpp @@ -1,10 +1,42 @@ -/* - * mavlink_rate_limiter.cpp +/**************************************************************************** * - * Created on: 26.02.2014 - * Author: ton - */ + * 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_rate_limiter.cpp + * Message rate limiter implementation. + * + * @author Anton Babushkin + */ #include "mavlink_rate_limiter.h" @@ -30,9 +62,11 @@ bool MavlinkRateLimiter::check(hrt_abstime t) { uint64_t dt = t - _last_sent; + if (dt > 0 && dt >= _interval) { _last_sent = (t / _interval) * _interval; return true; } + return false; } diff --git a/src/modules/mavlink/mavlink_rate_limiter.h b/src/modules/mavlink/mavlink_rate_limiter.h index 6db65f638f..0b37538e62 100644 --- a/src/modules/mavlink/mavlink_rate_limiter.h +++ b/src/modules/mavlink/mavlink_rate_limiter.h @@ -1,8 +1,41 @@ -/* - * mavlink_rate_limiter.h +/**************************************************************************** * - * Created on: 26.02.2014 - * Author: ton + * 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_rate_limiter.h + * Message rate limiter definition. + * + * @author Anton Babushkin */ #ifndef MAVLINK_RATE_LIMITER_H_ @@ -11,7 +44,8 @@ #include -class MavlinkRateLimiter { +class MavlinkRateLimiter +{ private: hrt_abstime _last_sent; hrt_abstime _interval; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index a3546e954b..f85773ae02 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -566,8 +566,9 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) float heading_rad = gps.cog * M_DEG_TO_RAD_F * 1e-2f; /* go back to -PI..PI */ - if (heading_rad > M_PI_F) + if (heading_rad > M_PI_F) { heading_rad -= 2.0f * M_PI_F; + } hil_gps.timestamp_velocity = gps.time_usec; hil_gps.vel_n_m_s = gps.vn * 1e-2f; // from cm to m @@ -606,7 +607,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed); } - // publish global position + // publish global position if (pub_hil_global_pos > 0) { orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos); // global position packet @@ -627,10 +628,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) if (pub_hil_local_pos > 0) { float x; float y; - bool landed = (float)(hil_state.alt)/1000.0f < (alt0 + 0.1f); // XXX improve? - double lat = hil_state.lat*1e-7; - double lon = hil_state.lon*1e-7; - map_projection_project(lat, lon, &x, &y); + bool landed = (float)(hil_state.alt) / 1000.0f < (alt0 + 0.1f); // XXX improve? + double lat = hil_state.lat * 1e-7; + double lon = hil_state.lon * 1e-7; + map_projection_project(lat, lon, &x, &y); hil_local_pos.timestamp = timestamp; hil_local_pos.xy_valid = true; hil_local_pos.z_valid = true; @@ -638,10 +639,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) hil_local_pos.v_z_valid = true; hil_local_pos.x = x; hil_local_pos.y = y; - hil_local_pos.z = alt0 - hil_state.alt/1000.0f; - hil_local_pos.vx = hil_state.vx/100.0f; - hil_local_pos.vy = hil_state.vy/100.0f; - hil_local_pos.vz = hil_state.vz/100.0f; + hil_local_pos.z = alt0 - hil_state.alt / 1000.0f; + hil_local_pos.vx = hil_state.vx / 100.0f; + hil_local_pos.vy = hil_state.vy / 100.0f; + hil_local_pos.vz = hil_state.vz / 100.0f; hil_local_pos.yaw = hil_attitude.yaw; hil_local_pos.xy_global = true; hil_local_pos.z_global = true; @@ -651,6 +652,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) hil_local_pos.ref_alt = alt0; hil_local_pos.landed = landed; orb_publish(ORB_ID(vehicle_local_position), pub_hil_local_pos, &hil_local_pos); + } else { pub_hil_local_pos = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos); lat0 = hil_state.lat; @@ -661,12 +663,13 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) /* Calculate Rotation Matrix */ math::Quaternion q(hil_state.attitude_quaternion); - math::Matrix<3,3> C_nb = q.to_dcm(); + math::Matrix<3, 3> C_nb = q.to_dcm(); math::Vector<3> euler = C_nb.to_euler(); /* set rotation matrix */ - for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) + for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) { hil_attitude.R[i][j] = C_nb(i, j); + } hil_attitude.R_valid = true; @@ -841,9 +844,9 @@ void MavlinkReceiver::print_status() } -void * MavlinkReceiver::start_helper(void *context) +void *MavlinkReceiver::start_helper(void *context) { - MavlinkReceiver *rcv = new MavlinkReceiver((Mavlink*)context); + MavlinkReceiver *rcv = new MavlinkReceiver((Mavlink *)context); return rcv->receive_thread(NULL); } @@ -865,7 +868,7 @@ MavlinkReceiver::receive_start(Mavlink *parent) pthread_attr_setstacksize(&receiveloop_attr, 3000); pthread_t thread; - pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, (void*)parent); + pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, (void *)parent); pthread_attr_destroy(&receiveloop_attr); return thread; diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index fca5de9179..199e426892 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -97,13 +97,13 @@ public: static pthread_t receive_start(Mavlink *parent); - static void * start_helper(void *context); + static void *start_helper(void *context); private: perf_counter_t _loop_perf; /**< loop performance counter */ - Mavlink* _mavlink; + Mavlink *_mavlink; void handle_message(mavlink_message_t *msg); void *receive_thread(void *arg); diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index 703f74b4c3..8694950985 100644 --- a/src/modules/mavlink/mavlink_stream.cpp +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -1,8 +1,41 @@ -/* - * mavlink_stream.cpp +/**************************************************************************** * - * Created on: 24.02.2014 - * Author: ton + * 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_stream.cpp + * Mavlink messages stream implementation. + * + * @author Anton Babushkin */ #include @@ -43,6 +76,7 @@ int MavlinkStream::update(const hrt_abstime t) { uint64_t dt = t - _last_sent; + if (dt > 0 && dt >= _interval) { /* interval expired, send message */ send(t); diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index 9f175adbe5..135e1bce08 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -1,8 +1,41 @@ -/* - * mavlink_stream.h +/**************************************************************************** * - * Created on: 24.02.2014 - * Author: ton + * 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_stream.cpp + * Mavlink messages stream definition. + * + * @author Anton Babushkin */ #ifndef MAVLINK_STREAM_H_ @@ -15,7 +48,8 @@ class MavlinkStream; #include "mavlink_main.h" -class MavlinkStream { +class MavlinkStream +{ private: hrt_abstime _last_sent; From 1b8004cd8ecf7824584aac9e7fed447714feb716 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Mar 2014 16:43:04 +0400 Subject: [PATCH 112/381] mavlink: add new streams in main loop, minor cleanup and fixes --- ROMFS/px4fmu_common/init.d/rcS | 36 +++++---- src/modules/mavlink/mavlink_main.cpp | 76 +++++++++++++++---- src/modules/mavlink/mavlink_main.h | 6 +- src/modules/mavlink/mavlink_messages.cpp | 52 ++++++------- .../mavlink/mavlink_orb_subscription.cpp | 9 ++- .../mavlink/mavlink_orb_subscription.h | 4 +- 6 files changed, 121 insertions(+), 62 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index c3065b6fc8..17f7dd077d 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -117,6 +117,7 @@ then set PWM_MAX none set MKBLCTRL_MODE none set FMU_MODE pwm + set MAVLINK_FLAGS default set MAV_TYPE none # @@ -381,26 +382,33 @@ then # set EXIT_ON_END no - if [ $HIL == yes ] + if [ $MAVLINK_FLAGS == default ] then - sleep 1 - mavlink start -b 230400 -d /dev/ttyACM0 - usleep 5000 - else - if [ $TTYS1_BUSY == yes ] + if [ $HIL == yes ] then - # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else - mavlink start -r 1000 -d /dev/ttyS0 + sleep 1 + set MAVLINK_FLAGS "-d 10000 -d /dev/ttyACM0" usleep 5000 - - # Exit from nsh to free port for mavlink - set EXIT_ON_END yes else - # Start MAVLink on default port: ttyS1 - mavlink start -r 1000 - usleep 5000 + # Normal mode, use baudrate 57600 (default) and data rate 1000 bytes/s + if [ $TTYS1_BUSY == yes ] + then + # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else + set MAVLINK_FLAGS "-r 1000 -d /dev/ttyS0" + usleep 5000 + + # Exit from nsh to free port for mavlink + set EXIT_ON_END yes + else + # Start MAVLink on default port: ttyS1 + set MAVLINK_FLAGS "-r 1000" + usleep 5000 + fi fi fi + + mavlink start $MAVLINK_FLAGS + usleep 5000 # # Start the datamanager diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index c977865538..b996413a85 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -171,6 +171,9 @@ Mavlink::Mavlink() : _subscriptions(nullptr), _streams(nullptr), mission_pub(-1), + mavlink_param_queue_index(0), + _subscribe_to_stream(nullptr), + _subscribe_to_stream_rate(0.0f), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mavlink")) @@ -301,6 +304,7 @@ Mavlink::destroy_all_instances() while (inst_to_del->thread_running) { printf("."); + fflush(stdout); usleep(10000); iterations++; @@ -1443,7 +1447,7 @@ Mavlink::mavlink_missionlib_send_gcs_string(const char *string) } } -MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic, const size_t size) +MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic) { /* check if already subscribed to this topic */ MavlinkOrbSubscription *sub; @@ -1456,7 +1460,7 @@ MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic, cons } /* add new subscription */ - MavlinkOrbSubscription *sub_new = new MavlinkOrbSubscription(topic, size); + MavlinkOrbSubscription *sub_new = new MavlinkOrbSubscription(topic); LL_APPEND(_subscriptions, sub_new); @@ -1509,6 +1513,34 @@ Mavlink::configure_stream(const char *stream_name, const float rate) return ERROR; } +void +Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate) +{ + /* orb subscription must be done from the main thread, + * set _subscribe_to_stream and _subscribe_to_stream_rate fields + * which polled in mavlink main loop */ + if (!_task_should_exit) { + /* wait for previous subscription completion */ + while (_subscribe_to_stream != nullptr) { + usleep(MAIN_LOOP_DELAY / 2); + } + + /* copy stream name */ + unsigned n = strlen(stream_name) + 1; + char *s = new char[n]; + memcpy(s, stream_name, n); + + /* set subscription task */ + _subscribe_to_stream_rate = rate; + _subscribe_to_stream = s; + + /* wait for subscription */ + do { + usleep(MAIN_LOOP_DELAY / 2); + } while (_subscribe_to_stream != nullptr); + } +} + int Mavlink::task_main(int argc, char *argv[]) { @@ -1686,8 +1718,8 @@ Mavlink::task_main(int argc, char *argv[]) thread_running = true; - MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update), sizeof(parameter_update_s)); - MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status), sizeof(vehicle_status_s)); + MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update)); + MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status)); struct vehicle_status_s *status = (struct vehicle_status_s *) status_sub->get_data(); @@ -1726,6 +1758,9 @@ Mavlink::task_main(int argc, char *argv[]) break; } + /* don't send parameters on startup without request */ + mavlink_param_queue_index = param_count(); + MavlinkRateLimiter slow_rate_limiter(2000.0f / rate_mult); MavlinkRateLimiter fast_rate_limiter(100.0f / rate_mult); @@ -1752,6 +1787,24 @@ Mavlink::task_main(int argc, char *argv[]) } } + /* check for requested subscriptions */ + if (_subscribe_to_stream != nullptr) { + if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) { + if (_subscribe_to_stream_rate > 0.0f) { + warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, device_name, _subscribe_to_stream_rate); + + } else { + warnx("stream %s on device %s disabled", _subscribe_to_stream, device_name); + } + + } else { + warnx("stream %s not found", _subscribe_to_stream, device_name); + } + delete _subscribe_to_stream; + _subscribe_to_stream = nullptr; + } + + /* update streams */ MavlinkStream *stream; LL_FOREACH(_streams, stream) { stream->update(t); @@ -1794,6 +1847,9 @@ Mavlink::task_main(int argc, char *argv[]) perf_end(_loop_perf); } + delete _subscribe_to_stream; + _subscribe_to_stream = nullptr; + /* wait for threads to complete */ pthread_join(receive_thread, NULL); @@ -1911,17 +1967,7 @@ Mavlink::stream(int argc, char *argv[]) Mavlink *inst = get_instance_for_device(device_name); if (inst != nullptr) { - if (OK == inst->configure_stream(stream_name, rate)) { - if (rate > 0.0f) { - warnx("stream %s on device %s enabled with rate %.1f Hz", stream_name, device_name, rate); - - } else { - warnx("stream %s on device %s disabled", stream_name, device_name); - } - - } else { - warnx("stream %s not found", stream_name, device_name); - } + inst->configure_stream_threadsafe(stream_name, rate); } else { errx(1, "mavlink for device %s is not running", device_name); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 532c9bcee3..ebea53d52e 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -180,7 +180,7 @@ public: */ int set_hil_enabled(bool hil_enabled); - MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic, size_t size); + MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic); mavlink_channel_t get_channel(); @@ -235,6 +235,9 @@ private: bool mavlink_link_termination_allowed; + char * _subscribe_to_stream; + float _subscribe_to_stream_rate; + /** * Send one parameter. * @@ -296,6 +299,7 @@ private: int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); int configure_stream(const char *stream_name, const float rate); + void configure_stream_threadsafe(const char *stream_name, const float rate); static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 820faae1cc..8be7d76d70 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -207,10 +207,10 @@ private: protected: void subscribe(Mavlink *mavlink) { - status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status), sizeof(struct vehicle_status_s)); + status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); status = (struct vehicle_status_s *)status_sub->get_data(); - pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet), sizeof(position_setpoint_triplet_s)); + pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data(); } @@ -255,7 +255,7 @@ private: protected: void subscribe(Mavlink *mavlink) { - status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status), sizeof(struct vehicle_status_s)); + status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); status = (struct vehicle_status_s *)status_sub->get_data(); } @@ -310,7 +310,7 @@ private: protected: void subscribe(Mavlink *mavlink) { - sensor_sub = mavlink->add_orb_subscription(ORB_ID(sensor_combined), sizeof(struct sensor_combined_s)); + sensor_sub = mavlink->add_orb_subscription(ORB_ID(sensor_combined)); sensor = (struct sensor_combined_s *)sensor_sub->get_data(); } @@ -376,7 +376,7 @@ private: protected: void subscribe(Mavlink *mavlink) { - att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude), sizeof(struct vehicle_attitude_s)); + att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); att = (struct vehicle_attitude_s *)att_sub->get_data(); } @@ -412,7 +412,7 @@ private: protected: void subscribe(Mavlink *mavlink) { - att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude), sizeof(struct vehicle_attitude_s)); + att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); att = (struct vehicle_attitude_s *)att_sub->get_data(); } @@ -465,19 +465,19 @@ private: protected: void subscribe(Mavlink *mavlink) { - att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude), sizeof(struct vehicle_attitude_s)); + att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude)); att = (struct vehicle_attitude_s *)att_sub->get_data(); - pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position), sizeof(struct vehicle_global_position_s)); + pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position)); pos = (struct vehicle_global_position_s *)pos_sub->get_data(); - armed_sub = mavlink->add_orb_subscription(ORB_ID(actuator_armed), sizeof(struct actuator_armed_s)); + armed_sub = mavlink->add_orb_subscription(ORB_ID(actuator_armed)); armed = (struct actuator_armed_s *)armed_sub->get_data(); - act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_controls_0), sizeof(struct actuator_controls_s)); + act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_controls_0)); act = (struct actuator_controls_s *)act_sub->get_data(); - airspeed_sub = mavlink->add_orb_subscription(ORB_ID(airspeed), sizeof(struct airspeed_s)); + airspeed_sub = mavlink->add_orb_subscription(ORB_ID(airspeed)); airspeed = (struct airspeed_s *)airspeed_sub->get_data(); } @@ -524,7 +524,7 @@ private: protected: void subscribe(Mavlink *mavlink) { - gps_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position), sizeof(struct vehicle_gps_position_s)); + gps_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position)); gps = (struct vehicle_gps_position_s *)gps_sub->get_data(); } @@ -570,10 +570,10 @@ private: protected: void subscribe(Mavlink *mavlink) { - pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position), sizeof(struct vehicle_global_position_s)); + pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position)); pos = (struct vehicle_global_position_s *)pos_sub->get_data(); - home_sub = mavlink->add_orb_subscription(ORB_ID(home_position), sizeof(struct home_position_s)); + home_sub = mavlink->add_orb_subscription(ORB_ID(home_position)); home = (struct home_position_s *)home_sub->get_data(); } @@ -616,7 +616,7 @@ private: protected: void subscribe(Mavlink *mavlink) { - pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position), sizeof(struct vehicle_local_position_s)); + pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position)); pos = (struct vehicle_local_position_s *)pos_sub->get_data(); } @@ -656,7 +656,7 @@ private: protected: void subscribe(Mavlink *mavlink) { - home_sub = mavlink->add_orb_subscription(ORB_ID(home_position), sizeof(struct home_position_s)); + home_sub = mavlink->add_orb_subscription(ORB_ID(home_position)); home = (struct home_position_s *)home_sub->get_data(); } @@ -707,7 +707,7 @@ protected: ORB_ID(actuator_outputs_3) }; - act_sub = mavlink->add_orb_subscription(act_topics[_n], sizeof(struct actuator_outputs_s)); + act_sub = mavlink->add_orb_subscription(act_topics[_n]); act = (struct actuator_outputs_s *)act_sub->get_data(); } @@ -756,13 +756,13 @@ private: protected: void subscribe(Mavlink *mavlink) { - status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status), sizeof(struct vehicle_status_s)); + status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); status = (struct vehicle_status_s *)status_sub->get_data(); - pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet), sizeof(position_setpoint_triplet_s)); + pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data(); - act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0), sizeof(struct actuator_outputs_s)); + act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0)); act = (struct actuator_outputs_s *)act_sub->get_data(); } @@ -861,7 +861,7 @@ private: protected: void subscribe(Mavlink *mavlink) { - pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet), sizeof(position_setpoint_triplet_s)); + pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)); pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data(); } @@ -899,7 +899,7 @@ private: protected: void subscribe(Mavlink *mavlink) { - pos_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint), sizeof(vehicle_local_position_setpoint_s)); + pos_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint)); pos_sp = (struct vehicle_local_position_setpoint_s *)pos_sp_sub->get_data(); } @@ -937,7 +937,7 @@ private: protected: void subscribe(Mavlink *mavlink) { - att_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint), sizeof(vehicle_attitude_setpoint_s)); + att_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint)); att_sp = (struct vehicle_attitude_setpoint_s *)att_sp_sub->get_data(); } @@ -975,7 +975,7 @@ private: protected: void subscribe(Mavlink *mavlink) { - att_rates_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint), sizeof(vehicle_rates_setpoint_s)); + att_rates_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint)); att_rates_sp = (struct vehicle_rates_setpoint_s *)att_rates_sp_sub->get_data(); } @@ -1013,7 +1013,7 @@ private: protected: void subscribe(Mavlink *mavlink) { - rc_sub = mavlink->add_orb_subscription(ORB_ID(input_rc), sizeof(struct rc_input_values)); + rc_sub = mavlink->add_orb_subscription(ORB_ID(input_rc)); rc = (struct rc_input_values *)rc_sub->get_data(); } @@ -1062,7 +1062,7 @@ private: protected: void subscribe(Mavlink *mavlink) { - manual_sub = mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint), sizeof(struct manual_control_setpoint_s)); + manual_sub = mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint)); manual = (struct manual_control_setpoint_s *)manual_sub->get_data(); } diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp index e1208bca9b..6279e5366a 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.cpp +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -42,13 +42,14 @@ #include #include #include +#include #include "mavlink_orb_subscription.h" -MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic, size_t size) : _topic(topic), _last_check(0), next(nullptr) +MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) : _topic(topic), _last_check(0), next(nullptr) { - _data = malloc(size); - memset(_data, 0, size); + _data = malloc(topic->o_size); + memset(_data, 0, topic->o_size); _fd = orb_subscribe(_topic); } @@ -58,7 +59,7 @@ MavlinkOrbSubscription::~MavlinkOrbSubscription() free(_data); } -const struct orb_metadata * +const orb_id_t MavlinkOrbSubscription::get_topic() { return _topic; diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h index 3cf33ccef2..eacc270349 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.h +++ b/src/modules/mavlink/mavlink_orb_subscription.h @@ -50,12 +50,12 @@ class MavlinkOrbSubscription public: MavlinkOrbSubscription *next; - MavlinkOrbSubscription(const orb_id_t topic, size_t size); + MavlinkOrbSubscription(const orb_id_t topic); ~MavlinkOrbSubscription(); bool update(const hrt_abstime t); void *get_data(); - const struct orb_metadata *get_topic(); + const orb_id_t get_topic(); private: const orb_id_t _topic; From c10ef787539265bc36fb76c855aa19b30ea24b04 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Mar 2014 17:12:46 +0400 Subject: [PATCH 113/381] mavlink: stop fixes --- src/modules/mavlink/mavlink_main.cpp | 11 ++++++++--- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index b996413a85..568666c1ea 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -308,7 +308,7 @@ Mavlink::destroy_all_instances() usleep(10000); iterations++; - if (iterations > 10000) { + if (iterations > 1000) { warnx("ERROR: Couldn't stop all mavlink instances."); return ERROR; } @@ -1850,18 +1850,23 @@ Mavlink::task_main(int argc, char *argv[]) delete _subscribe_to_stream; _subscribe_to_stream = nullptr; + warnv("waiting for UART receive thread"); + /* wait for threads to complete */ pthread_join(receive_thread, NULL); - /* Reset the UART flags to original state */ + /* reset the UART flags to original state */ tcsetattr(_uart, TCSANOW, &uart_config_original); + /* close UART */ + close(_uart); + /* destroy log buffer */ mavlink_logbuffer_destroy(&lb); thread_running = false; - warnx("exiting."); + warnx("exiting"); _mavlink_task = -1; _exit(0); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index f85773ae02..b6e008cbfc 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -795,7 +795,7 @@ MavlinkReceiver::receive_thread(void *arg) { int uart_fd = _mavlink->get_uart_fd(); - const int timeout = 1000; + const int timeout = 500; uint8_t buf[32]; mavlink_message_t msg; From 256cc2b411b1f36397884bfd019b9ac3e4cd1850 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Mar 2014 18:30:30 +0400 Subject: [PATCH 114/381] mavlink: cleanup and refactoring, rcS: EXIT_ON_END fix --- ROMFS/px4fmu_common/init.d/rcS | 3 +- src/modules/mavlink/mavlink_main.cpp | 368 ++++++++++++--------------- src/modules/mavlink/mavlink_main.h | 25 +- 3 files changed, 179 insertions(+), 217 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 17f7dd077d..c96f44e023 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -118,6 +118,7 @@ then set MKBLCTRL_MODE none set FMU_MODE pwm set MAVLINK_FLAGS default + set EXIT_ON_END no set MAV_TYPE none # @@ -380,7 +381,6 @@ then # # MAVLink # - set EXIT_ON_END no if [ $MAVLINK_FLAGS == default ] then @@ -539,6 +539,7 @@ then if [ $EXIT_ON_END == yes ] then + echo "[init] Exit from nsh" exit fi diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 568666c1ea..99bef1f731 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -92,7 +92,7 @@ static const int ERROR = -1; #define MAX_DATA_RATE 10000 // max data rate in bytes/s #define MAIN_LOOP_DELAY 10000 // 100 Hz -static Mavlink *_head = nullptr; +static Mavlink *_mavlink_instances = nullptr; /* TODO: if this is a class member it crashes */ static struct file_operations fops; @@ -160,35 +160,30 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length static void usage(void); Mavlink::Mavlink() : - device_name(DEFAULT_DEVICE_NAME), - _task_should_exit(false), next(nullptr), + _device_name(DEFAULT_DEVICE_NAME), + _task_should_exit(false), _mavlink_fd(-1), - thread_running(false), - _mavlink_task(-1), - _mavlink_incoming_fd(-1), + _task_running(false), _mavlink_hil_enabled(false), _subscriptions(nullptr), _streams(nullptr), - mission_pub(-1), - mavlink_param_queue_index(0), + _mission_pub(-1), + _mavlink_param_queue_index(0), _subscribe_to_stream(nullptr), _subscribe_to_stream_rate(0.0f), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mavlink")) { - wpm = &wpm_s; + _wpm = &_wpm_s; fops.ioctl = (int (*)(file *, int, long unsigned int))&mavlink_dev_ioctl; - // _parameter_handles.min_altitude = param_find("NAV_MIN_ALT"); - } Mavlink::~Mavlink() { - if (_mavlink_task != -1) { - - /* task wakes up every 100ms or so at the longest */ + if (_task_running) { + /* task wakes up every 10ms or so at the longest */ _task_should_exit = true; /* wait for a second for the task to quit at our request */ @@ -200,10 +195,11 @@ Mavlink::~Mavlink() /* if we have given up, kill it */ if (++i > 50) { - task_delete(_mavlink_task); + //TODO store main task handle in Mavlink instance to allow killing task + //task_delete(_mavlink_task); break; } - } while (_mavlink_task != -1); + } while (_task_running); } } @@ -216,12 +212,10 @@ Mavlink::set_mode(enum MAVLINK_MODE mode) int Mavlink::instance_count() { - /* note: a local buffer count will help if this ever is called often */ - Mavlink *inst = ::_head; unsigned inst_index = 0; + Mavlink *inst; - while (inst != nullptr) { - inst = inst->next; + LL_FOREACH(::_mavlink_instances, inst) { inst_index++; } @@ -232,11 +226,11 @@ Mavlink * Mavlink::new_instance() { Mavlink *inst = new Mavlink(); - Mavlink *next = ::_head; + Mavlink *next = ::_mavlink_instances; /* create the first instance at _head */ - if (::_head == nullptr) { - ::_head = inst; + if (::_mavlink_instances == nullptr) { + ::_mavlink_instances = inst; /* afterwards follow the next and append the instance */ } else { @@ -254,19 +248,17 @@ Mavlink::new_instance() Mavlink * Mavlink::get_instance(unsigned instance) { - Mavlink *inst = ::_head; + Mavlink *inst; unsigned inst_index = 0; + LL_FOREACH(::_mavlink_instances, inst) { + if (instance == inst_index) { + return inst; + } - while (inst->next != nullptr && inst_index < instance) { - inst = inst->next; inst_index++; } - if (inst_index < instance) { - inst = nullptr; - } - - return inst; + return nullptr; } Mavlink * @@ -274,8 +266,8 @@ Mavlink::get_instance_for_device(const char *device_name) { Mavlink *inst; - LL_FOREACH(::_head, inst) { - if (strcmp(inst->device_name, device_name) == 0) { + LL_FOREACH(::_mavlink_instances, inst) { + if (strcmp(inst->_device_name, device_name) == 0) { return inst; } } @@ -288,21 +280,20 @@ Mavlink::destroy_all_instances() { /* start deleting from the end */ Mavlink *inst_to_del = nullptr; - Mavlink *next_inst = ::_head; + Mavlink *next_inst = ::_mavlink_instances; unsigned iterations = 0; warnx("waiting for instances to stop"); while (next_inst != nullptr) { - inst_to_del = next_inst; next_inst = inst_to_del->next; /* set flag to stop thread and wait for all threads to finish */ inst_to_del->_task_should_exit = true; - while (inst_to_del->thread_running) { + while (inst_to_del->_task_running) { printf("."); fflush(stdout); usleep(10000); @@ -318,7 +309,7 @@ Mavlink::destroy_all_instances() } /* reset head */ - ::_head = nullptr; + ::_mavlink_instances = nullptr; printf("\n"); warnx("all instances stopped"); @@ -328,12 +319,12 @@ Mavlink::destroy_all_instances() bool Mavlink::instance_exists(const char *device_name, Mavlink *self) { - Mavlink *inst = ::_head; + Mavlink *inst = ::_mavlink_instances; while (inst != nullptr) { /* don't compare with itself */ - if (inst != self && !strcmp(device_name, inst->device_name)) { + if (inst != self && !strcmp(device_name, inst->_device_name)) { return true; } @@ -358,7 +349,7 @@ Mavlink::get_uart_fd(unsigned index) int Mavlink::get_uart_fd() { - return _uart; + return _uart_fd; } mavlink_channel_t @@ -384,12 +375,12 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) struct mavlink_logmessage msg; strncpy(msg.text, txt, sizeof(msg.text)); - Mavlink *inst = ::_head; + Mavlink *inst = ::_mavlink_instances; while (inst != nullptr) { - mavlink_logbuffer_write(&inst->lb, &msg); - inst->total_counter++; + mavlink_logbuffer_write(&inst->_logbuffer, &msg); + inst->_total_counter++; inst = inst->next; } @@ -493,7 +484,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * } /* open uart */ - _uart = open(uart_name, O_RDWR | O_NOCTTY); + _uart_fd = open(uart_name, O_RDWR | O_NOCTTY); /* Try to set baud rate */ struct termios uart_config; @@ -501,14 +492,14 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * *is_usb = false; /* Back up the original uart configuration to restore it after exit */ - if ((termios_state = tcgetattr(_uart, uart_config_original)) < 0) { + if ((termios_state = tcgetattr(_uart_fd, uart_config_original)) < 0) { warnx("ERROR get termios config %s: %d\n", uart_name, termios_state); - close(_uart); + close(_uart_fd); return -1; } /* Fill the struct for the new configuration */ - tcgetattr(_uart, &uart_config); + tcgetattr(_uart_fd, &uart_config); /* Clear ONLCR flag (which appends a CR for every LF) */ uart_config.c_oflag &= ~ONLCR; @@ -519,19 +510,19 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * /* Set baud rate */ 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); - close(_uart); + close(_uart_fd); return -1; } } - if ((termios_state = tcsetattr(_uart, TCSANOW, &uart_config)) < 0) { + if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) { warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name); - close(_uart); + close(_uart_fd); return -1; } - return _uart; + return _uart_fd; } int @@ -583,9 +574,9 @@ extern mavlink_system_t mavlink_system; int Mavlink::mavlink_pm_queued_send() { - if (mavlink_param_queue_index < param_count()) { - mavlink_pm_send_param(param_for_index(mavlink_param_queue_index)); - mavlink_param_queue_index++; + if (_mavlink_param_queue_index < param_count()) { + mavlink_pm_send_param(param_for_index(_mavlink_param_queue_index)); + _mavlink_param_queue_index++; return 0; } else { @@ -595,7 +586,7 @@ int Mavlink::mavlink_pm_queued_send() void Mavlink::mavlink_pm_start_queued_send() { - mavlink_param_queue_index = 0; + _mavlink_param_queue_index = 0; } int Mavlink::mavlink_pm_send_param_for_index(uint16_t index) @@ -730,11 +721,11 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav void Mavlink::publish_mission() { /* Initialize mission publication if necessary */ - if (mission_pub < 0) { - mission_pub = orb_advertise(ORB_ID(mission), &mission); + if (_mission_pub < 0) { + _mission_pub = orb_advertise(ORB_ID(mission), &mission); } else { - orb_publish(ORB_ID(mission), mission_pub, &mission); + orb_publish(ORB_ID(mission), _mission_pub, &mission); } } @@ -863,7 +854,7 @@ void Mavlink::mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8 */ void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq) { - if (seq < wpm->size) { + if (seq < _wpm->size) { mavlink_message_t msg; mavlink_mission_current_t wpc; @@ -872,7 +863,7 @@ void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq) mavlink_msg_mission_current_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpc); mavlink_missionlib_send_message(&msg); - } else if (seq == 0 && wpm->size == 0) { + } else if (seq == 0 && _wpm->size == 0) { /* don't broadcast if no WPs */ @@ -906,7 +897,7 @@ void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t dm_item_t dm_current; - if (wpm->current_dataman_id == 0) { + if (_wpm->current_dataman_id == 0) { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; } else { @@ -929,7 +920,7 @@ void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t if (_verbose) { warnx("Sent waypoint %u to ID %u", wp.seq, wp.target_system); } } else { - mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR); + mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); if (_verbose) { warnx("ERROR: could not read WP%u", seq); } } @@ -937,7 +928,7 @@ void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq) { - if (seq < wpm->max_size) { + if (seq < _wpm->max_size) { mavlink_message_t msg; mavlink_mission_request_t wpr; wpr.target_system = sysid; @@ -978,15 +969,15 @@ void Mavlink::mavlink_wpm_send_waypoint_reached(uint16_t seq) void Mavlink::mavlink_waypoint_eventloop(uint64_t now) { /* check for timed-out operations */ - if (now - wpm->timestamp_lastaction > wpm->timeout && wpm->current_state != MAVLINK_WPM_STATE_IDLE) { + if (now - _wpm->timestamp_lastaction > _wpm->timeout && _wpm->current_state != MAVLINK_WPM_STATE_IDLE) { mavlink_missionlib_send_gcs_string("Operation timeout"); - if (_verbose) { warnx("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", wpm->current_state); } + if (_verbose) { warnx("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", _wpm->current_state); } - wpm->current_state = MAVLINK_WPM_STATE_IDLE; - wpm->current_partner_sysid = 0; - wpm->current_partner_compid = 0; + _wpm->current_state = MAVLINK_WPM_STATE_IDLE; + _wpm->current_partner_sysid = 0; + _wpm->current_partner_compid = 0; } } @@ -1001,14 +992,14 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) mavlink_mission_ack_t wpa; mavlink_msg_mission_ack_decode(msg, &wpa); - if ((msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid) && (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/)) { - wpm->timestamp_lastaction = now; + if ((msg->sysid == _wpm->current_partner_sysid && msg->compid == _wpm->current_partner_compid) && (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/)) { + _wpm->timestamp_lastaction = now; - if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { - if (wpm->current_wp_id == wpm->size - 1) { + if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST || _wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { + if (_wpm->current_wp_id == _wpm->size - 1) { - wpm->current_state = MAVLINK_WPM_STATE_IDLE; - wpm->current_wp_id = 0; + _wpm->current_state = MAVLINK_WPM_STATE_IDLE; + _wpm->current_wp_id = 0; } } @@ -1026,10 +1017,10 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) mavlink_msg_mission_set_current_decode(msg, &wpc); if (wpc.target_system == mavlink_system.sysid /*&& wpc.target_component == mavlink_wpm_comp_id*/) { - wpm->timestamp_lastaction = now; + _wpm->timestamp_lastaction = now; - if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) { - if (wpc.seq < wpm->size) { + if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE) { + if (wpc.seq < _wpm->size) { mission.current_index = wpc.seq; publish_mission(); @@ -1064,22 +1055,22 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) mavlink_msg_mission_request_list_decode(msg, &wprl); if (wprl.target_system == mavlink_system.sysid /*&& wprl.target_component == mavlink_wpm_comp_id*/) { - wpm->timestamp_lastaction = now; + _wpm->timestamp_lastaction = now; - if (wpm->current_state == MAVLINK_WPM_STATE_IDLE || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { - if (wpm->size > 0) { + if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE || _wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { + if (_wpm->size > 0) { - wpm->current_state = MAVLINK_WPM_STATE_SENDLIST; - wpm->current_wp_id = 0; - wpm->current_partner_sysid = msg->sysid; - wpm->current_partner_compid = msg->compid; + _wpm->current_state = MAVLINK_WPM_STATE_SENDLIST; + _wpm->current_wp_id = 0; + _wpm->current_partner_sysid = msg->sysid; + _wpm->current_partner_compid = msg->compid; } else { if (_verbose) { warnx("No waypoints send"); } } - wpm->current_count = wpm->size; - mavlink_wpm_send_waypoint_count(msg->sysid, msg->compid, wpm->current_count); + _wpm->current_count = _wpm->size; + mavlink_wpm_send_waypoint_count(msg->sysid, msg->compid, _wpm->current_count); } else { mavlink_missionlib_send_gcs_string("IGN REQUEST LIST: Busy"); @@ -1100,10 +1091,10 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) mavlink_mission_request_t wpr; mavlink_msg_mission_request_decode(msg, &wpr); - if (msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid && wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) { - wpm->timestamp_lastaction = now; + if (msg->sysid == _wpm->current_partner_sysid && msg->compid == _wpm->current_partner_compid && wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) { + _wpm->timestamp_lastaction = now; - if (wpr.seq >= wpm->size) { + if (wpr.seq >= _wpm->size) { mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list"); @@ -1116,12 +1107,12 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) * Ensure that we are in the correct state and that the first request has id 0 * and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint) */ - if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { + if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { if (wpr.seq == 0) { if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } - wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; + _wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0"); @@ -1131,20 +1122,20 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) break; } - } else if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { + } else if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { - if (wpr.seq == wpm->current_wp_id) { + if (wpr.seq == _wpm->current_wp_id) { if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } - } else if (wpr.seq == wpm->current_wp_id + 1) { + } else if (wpr.seq == _wpm->current_wp_id + 1) { if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected"); - if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).", wpr.seq, wpm->current_wp_id, wpm->current_wp_id + 1); } + if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).", wpr.seq, _wpm->current_wp_id, _wpm->current_wp_id + 1); } break; } @@ -1153,20 +1144,20 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); - if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).", wpm->current_state); } + if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).", _wpm->current_state); } break; } - wpm->current_wp_id = wpr.seq; - wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; + _wpm->current_wp_id = wpr.seq; + _wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; - if (wpr.seq < wpm->size) { + if (wpr.seq < _wpm->size) { - mavlink_wpm_send_waypoint(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id); + mavlink_wpm_send_waypoint(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); } else { - mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR); + mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); if (_verbose) { warnx("ERROR: Waypoint %u out of bounds", wpr.seq); } } @@ -1174,11 +1165,11 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } else { //we we're target but already communicating with someone else - if ((wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) && !(msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid)) { + if ((wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) && !(msg->sysid == _wpm->current_partner_sysid && msg->compid == _wpm->current_partner_compid)) { mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); - if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.", msg->sysid, wpm->current_partner_sysid); } + if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.", msg->sysid, _wpm->current_partner_sysid); } } else { @@ -1196,14 +1187,14 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) mavlink_msg_mission_count_decode(msg, &wpc); if (wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_wpm_comp_id*/) { - wpm->timestamp_lastaction = now; + _wpm->timestamp_lastaction = now; - if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) { + if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE) { if (wpc.count > NUM_MISSIONS_SUPPORTED) { if (_verbose) { warnx("Too many waypoints: %d, supported: %d", wpc.count, NUM_MISSIONS_SUPPORTED); } - mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_NO_SPACE); + mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_NO_SPACE); break; } @@ -1217,17 +1208,17 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid); } - wpm->current_state = MAVLINK_WPM_STATE_GETLIST; - wpm->current_wp_id = 0; - wpm->current_partner_sysid = msg->sysid; - wpm->current_partner_compid = msg->compid; - wpm->current_count = wpc.count; + _wpm->current_state = MAVLINK_WPM_STATE_GETLIST; + _wpm->current_wp_id = 0; + _wpm->current_partner_sysid = msg->sysid; + _wpm->current_partner_compid = msg->compid; + _wpm->current_count = wpc.count; - mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id); + mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); - } else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST) { + } else if (_wpm->current_state == MAVLINK_WPM_STATE_GETLIST) { - if (wpm->current_wp_id == 0) { + if (_wpm->current_wp_id == 0) { mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN"); if (_verbose) { warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) again from %u", wpc.count, msg->sysid); } @@ -1235,7 +1226,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); - if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.", wpm->current_wp_id); } + if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.", _wpm->current_wp_id); } } } else { @@ -1259,14 +1250,14 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (wp.target_system == mavlink_system.sysid && wp.target_component == _mavlink_wpm_comp_id) { - wpm->timestamp_lastaction = now; + _wpm->timestamp_lastaction = now; /* * ensure that we are in the correct state and that the first waypoint has id 0 * and the following waypoints have the correct ids */ - if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST) { + if (_wpm->current_state == MAVLINK_WPM_STATE_GETLIST) { if (wp.seq != 0) { mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP not 0"); @@ -1274,30 +1265,30 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) break; } - } else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { + } else if (_wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { - if (wp.seq >= wpm->current_count) { + if (wp.seq >= _wpm->current_count) { mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP out of bounds"); warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was out of bounds.", wp.seq); break; } - if (wp.seq != wpm->current_wp_id) { - warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was not the expected %u.", wp.seq, wpm->current_wp_id); - mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id); + if (wp.seq != _wpm->current_wp_id) { + warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was not the expected %u.", wp.seq, _wpm->current_wp_id); + mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); break; } } - wpm->current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS; + _wpm->current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS; struct mission_item_s mission_item; int ret = map_mavlink_mission_item_to_mission_item(&wp, &mission_item); if (ret != OK) { - mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, ret); - wpm->current_state = MAVLINK_WPM_STATE_IDLE; + mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, ret); + _wpm->current_state = MAVLINK_WPM_STATE_IDLE; break; } @@ -1305,7 +1296,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) dm_item_t dm_next; - if (wpm->current_dataman_id == 0) { + if (_wpm->current_dataman_id == 0) { dm_next = DM_KEY_WAYPOINTS_OFFBOARD_1; mission.dataman_id = 1; @@ -1315,8 +1306,8 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } if (dm_write(dm_next, wp.seq, DM_PERSIST_IN_FLIGHT_RESET, &mission_item, len) != len) { - mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR); - wpm->current_state = MAVLINK_WPM_STATE_IDLE; + mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); + _wpm->current_state = MAVLINK_WPM_STATE_IDLE; break; } @@ -1327,25 +1318,25 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) // XXX ignore current set mission.current_index = -1; - wpm->current_wp_id = wp.seq + 1; + _wpm->current_wp_id = wp.seq + 1; - if (wpm->current_wp_id == wpm->current_count && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { + if (_wpm->current_wp_id == _wpm->current_count && _wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { - if (_verbose) { warnx("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE", wpm->current_count); } + if (_verbose) { warnx("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE", _wpm->current_count); } - mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED); + mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ACCEPTED); - mission.count = wpm->current_count; + mission.count = _wpm->current_count; publish_mission(); - wpm->current_dataman_id = mission.dataman_id; - wpm->size = wpm->current_count; + _wpm->current_dataman_id = mission.dataman_id; + _wpm->size = _wpm->current_count; - wpm->current_state = MAVLINK_WPM_STATE_IDLE; + _wpm->current_state = MAVLINK_WPM_STATE_IDLE; } else { - mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id); + mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); } } else { @@ -1363,10 +1354,10 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */) { - if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) { - wpm->timestamp_lastaction = now; + if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE) { + _wpm->timestamp_lastaction = now; - wpm->size = 0; + _wpm->size = 0; /* prepare mission topic */ mission.dataman_id = -1; @@ -1375,10 +1366,10 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) publish_mission(); if (dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_0) == OK && dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_1) == OK) { - mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED); + 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); + mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); } @@ -1389,7 +1380,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) } - } else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm->current_state != MAVLINK_WPM_STATE_IDLE) { + } else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && _wpm->current_state != MAVLINK_WPM_STATE_IDLE) { mavlink_missionlib_send_gcs_string("REJ. WP CLERR CMD: target id mismatch"); @@ -1549,7 +1540,7 @@ Mavlink::task_main(int argc, char *argv[]) fflush(stdout); /* initialize mavlink text message buffering */ - mavlink_logbuffer_init(&lb, 5); + mavlink_logbuffer_init(&_logbuffer, 5); int ch; _baudrate = 57600; @@ -1589,7 +1580,7 @@ Mavlink::task_main(int argc, char *argv[]) break; case 'd': - device_name = optarg; + _device_name = optarg; break; // case 'e': @@ -1636,8 +1627,8 @@ Mavlink::task_main(int argc, char *argv[]) _datarate = MAX_DATA_RATE; } - if (Mavlink::instance_exists(device_name, this)) { - errx(1, "mavlink instance for %s already running", device_name); + if (Mavlink::instance_exists(_device_name, this)) { + errx(1, "mavlink instance for %s already running", _device_name); } /* inform about mode */ @@ -1680,7 +1671,7 @@ Mavlink::task_main(int argc, char *argv[]) } warnx("data rate: %d bytes/s", _datarate); - warnx("port: %s, baudrate: %d", device_name, _baudrate); + warnx("port: %s, baudrate: %d", _device_name, _baudrate); /* flush stdout in case MAVLink is about to take it over */ fflush(stdout); @@ -1689,16 +1680,14 @@ Mavlink::task_main(int argc, char *argv[]) bool usb_uart; /* default values for arguments */ - _uart = mavlink_open_uart(_baudrate, device_name, &uart_config_original, &usb_uart); + _uart_fd = mavlink_open_uart(_baudrate, _device_name, &uart_config_original, &usb_uart); - if (_uart < 0) { - err(1, "could not open %s", device_name); + if (_uart_fd < 0) { + err(1, "could not open %s", _device_name); } /* create the device node that's used for sending text log messages, etc. */ - if (instance_count() == 1) { - register_driver(MAVLINK_LOG_DEVICE, &fops, 0666, NULL); - } + register_driver(MAVLINK_LOG_DEVICE, &fops, 0666, NULL); /* initialize logging device */ _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); @@ -1707,16 +1696,16 @@ Mavlink::task_main(int argc, char *argv[]) mavlink_update_system(); /* start the MAVLink receiver */ - receive_thread = MavlinkReceiver::receive_start(this); + _receive_thread = MavlinkReceiver::receive_start(this); /* initialize waypoint manager */ - mavlink_wpm_init(wpm); + mavlink_wpm_init(_wpm); int mission_result_sub = orb_subscribe(ORB_ID(mission_result)); struct mission_result_s mission_result; memset(&mission_result, 0, sizeof(mission_result)); - thread_running = true; + _task_running = true; MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update)); MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status)); @@ -1759,7 +1748,7 @@ Mavlink::task_main(int argc, char *argv[]) } /* don't send parameters on startup without request */ - mavlink_param_queue_index = param_count(); + _mavlink_param_queue_index = param_count(); MavlinkRateLimiter slow_rate_limiter(2000.0f / rate_mult); MavlinkRateLimiter fast_rate_limiter(100.0f / rate_mult); @@ -1791,15 +1780,16 @@ Mavlink::task_main(int argc, char *argv[]) if (_subscribe_to_stream != nullptr) { if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) { if (_subscribe_to_stream_rate > 0.0f) { - warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, device_name, _subscribe_to_stream_rate); + warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name, _subscribe_to_stream_rate); } else { - warnx("stream %s on device %s disabled", _subscribe_to_stream, device_name); + warnx("stream %s on device %s disabled", _subscribe_to_stream, _device_name); } } else { - warnx("stream %s not found", _subscribe_to_stream, device_name); + warnx("stream %s not found", _subscribe_to_stream, _device_name); } + delete _subscribe_to_stream; _subscribe_to_stream = nullptr; } @@ -1834,9 +1824,9 @@ Mavlink::task_main(int argc, char *argv[]) mavlink_pm_queued_send(); mavlink_waypoint_eventloop(hrt_absolute_time()); - if (!mavlink_logbuffer_is_empty(&lb)) { + if (!mavlink_logbuffer_is_empty(&_logbuffer)) { struct mavlink_logmessage msg; - int lb_ret = mavlink_logbuffer_read(&lb, &msg); + int lb_ret = mavlink_logbuffer_read(&_logbuffer, &msg); if (lb_ret == OK) { mavlink_missionlib_send_gcs_string(msg.text); @@ -1850,25 +1840,23 @@ Mavlink::task_main(int argc, char *argv[]) delete _subscribe_to_stream; _subscribe_to_stream = nullptr; - warnv("waiting for UART receive thread"); + warnx("waiting for UART receive thread"); /* wait for threads to complete */ - pthread_join(receive_thread, NULL); + pthread_join(_receive_thread, NULL); /* reset the UART flags to original state */ - tcsetattr(_uart, TCSANOW, &uart_config_original); + tcsetattr(_uart_fd, TCSANOW, &uart_config_original); /* close UART */ - close(_uart); + close(_uart_fd); /* destroy log buffer */ - mavlink_logbuffer_destroy(&lb); - - thread_running = false; + mavlink_logbuffer_destroy(&_logbuffer); warnx("exiting"); - _mavlink_task = -1; + _task_running = false; _exit(0); } @@ -1876,6 +1864,7 @@ int Mavlink::start_helper(int argc, char *argv[]) { /* create the instance in task context */ Mavlink *instance = Mavlink::new_instance(); + /* this will actually only return once MAVLink exits */ return instance->task_main(argc, argv); } @@ -1887,37 +1876,12 @@ Mavlink::start(int argc, char *argv[]) char buf[32]; sprintf(buf, "mavlink_if%d", Mavlink::instance_count()); - /*mavlink->_mavlink_task = */task_spawn_cmd(buf, - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 2048, - (main_t)&Mavlink::start_helper, - (const char **)argv); - - // while (!this->is_running()) { - // usleep(200); - // } - - // if (mavlink->_mavlink_task < 0) { - // warn("task start failed"); - // return -errno; - // } - - // if (mavlink::g_mavlink != nullptr) { - // errx(1, "already running"); - // } - - // mavlink::g_mavlink = new Mavlink; - - // if (mavlink::g_mavlink == nullptr) { - // errx(1, "alloc failed"); - // } - - // if (OK != mavlink::g_mavlink->start()) { - // delete mavlink::g_mavlink; - // mavlink::g_mavlink = nullptr; - // err(1, "start failed"); - // } + task_spawn_cmd(buf, + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2048, + (main_t)&Mavlink::start_helper, + (const char **)argv); return OK; } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index ebea53d52e..94bef7e726 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -142,7 +142,7 @@ public: int get_uart_fd(); - const char *device_name; + const char *_device_name; enum MAVLINK_MODE { MODE_CUSTOM = 0, @@ -191,10 +191,7 @@ protected: private: int _mavlink_fd; - bool thread_running; - int _mavlink_task; /**< task handle for sensor task */ - - int _mavlink_incoming_fd; /**< file descriptor on which to receive incoming strings */ + bool _task_running; perf_counter_t _loop_perf; /**< loop performance counter */ @@ -204,7 +201,7 @@ private: MavlinkOrbSubscription *_subscriptions; MavlinkStream *_streams; - orb_advert_t mission_pub; + orb_advert_t _mission_pub; struct mission_s mission; uint8_t missionlib_msg_buf[sizeof(mavlink_message_t)]; MAVLINK_MODE _mode; @@ -212,17 +209,17 @@ private: uint8_t _mavlink_wpm_comp_id; mavlink_channel_t _channel; - struct mavlink_logbuffer lb; - unsigned int total_counter; + struct mavlink_logbuffer _logbuffer; + unsigned int _total_counter; - pthread_t receive_thread; + pthread_t _receive_thread; /* Allocate storage space for waypoints */ - mavlink_wpm_storage wpm_s; - mavlink_wpm_storage *wpm; + mavlink_wpm_storage _wpm_s; + mavlink_wpm_storage *_wpm; bool _verbose; - int _uart; + int _uart_fd; int _baudrate; int _datarate; @@ -231,11 +228,11 @@ private: * logic will send parameters from the current index * to len - 1, the end of the param list. */ - unsigned int mavlink_param_queue_index; + unsigned int _mavlink_param_queue_index; bool mavlink_link_termination_allowed; - char * _subscribe_to_stream; + char *_subscribe_to_stream; float _subscribe_to_stream_rate; /** From 6948defdb26711fd8336b5e0173664bf98517406 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Mar 2014 19:27:53 +0400 Subject: [PATCH 115/381] mavlink: HIL fixes, performance optimization --- ROMFS/px4fmu_common/init.d/rc.usb | 2 +- ROMFS/px4fmu_common/init.d/rcS | 2 +- src/modules/mavlink/mavlink_main.cpp | 21 +++++++++++++-------- src/modules/mavlink/mavlink_main.h | 4 +++- 4 files changed, 18 insertions(+), 11 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index 558be42755..0de2d42958 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -5,7 +5,7 @@ echo "Starting MAVLink on this USB console" -mavlink start -r 10000 -d /dev/ttyACM0 +mavlink start -r 5000 -d /dev/ttyACM0 # Exit shell to make it available to MAVLink exit diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index c96f44e023..48532e7af6 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -387,7 +387,7 @@ then if [ $HIL == yes ] then sleep 1 - set MAVLINK_FLAGS "-d 10000 -d /dev/ttyACM0" + set MAVLINK_FLAGS "-r 10000 -d /dev/ttyACM0 -m hil" usleep 5000 else # Normal mode, use baudrate 57600 (default) and data rate 1000 bytes/s diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 99bef1f731..fb29c9c716 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -90,7 +90,7 @@ static const int ERROR = -1; #define DEFAULT_DEVICE_NAME "/dev/ttyS1" #define MAX_DATA_RATE 10000 // max data rate in bytes/s -#define MAIN_LOOP_DELAY 10000 // 100 Hz +#define MAIN_LOOP_DELAY 10000 // 100 Hz @ 1000 bytes/s data rate static Mavlink *_mavlink_instances = nullptr; @@ -166,6 +166,7 @@ Mavlink::Mavlink() : _mavlink_fd(-1), _task_running(false), _mavlink_hil_enabled(false), + _main_loop_delay(1000), _subscriptions(nullptr), _streams(nullptr), _mission_pub(-1), @@ -1723,7 +1724,7 @@ Mavlink::task_main(int argc, char *argv[]) switch (_mode) { case MODE_OFFBOARD: configure_stream("SYS_STATUS", 1.0f); - configure_stream("GPS_GLOBAL_ORIGIN", 0.5f * rate_mult); + configure_stream("GPS_GLOBAL_ORIGIN", 0.5f); configure_stream("HIGHRES_IMU", 1.0f * rate_mult); configure_stream("ATTITUDE", 10.0f * rate_mult); configure_stream("GPS_RAW_INT", 1.0f * rate_mult); @@ -1733,14 +1734,15 @@ Mavlink::task_main(int argc, char *argv[]) break; case MODE_HIL: + /* HIL mode normally runs at high data rate, rate_mult ~ 10 */ configure_stream("SYS_STATUS", 1.0f); - configure_stream("GPS_GLOBAL_ORIGIN", 0.5f * rate_mult); + configure_stream("GPS_GLOBAL_ORIGIN", 0.5f); configure_stream("HIGHRES_IMU", 1.0f * rate_mult); - configure_stream("ATTITUDE", 10.0f * rate_mult); + configure_stream("ATTITUDE", 2.0f * rate_mult); configure_stream("GPS_RAW_INT", 1.0f * rate_mult); - configure_stream("GLOBAL_POSITION_INT", 5.0f * rate_mult); - configure_stream("LOCAL_POSITION_NED", 5.0f * rate_mult); - configure_stream("HIL_CONTROLS", 20.0f * rate_mult); + configure_stream("GLOBAL_POSITION_INT", 1.0f * rate_mult); + configure_stream("LOCAL_POSITION_NED", 1.0f * rate_mult); + configure_stream("HIL_CONTROLS", 15.0f * rate_mult); break; default: @@ -1753,9 +1755,12 @@ Mavlink::task_main(int argc, char *argv[]) MavlinkRateLimiter slow_rate_limiter(2000.0f / rate_mult); MavlinkRateLimiter fast_rate_limiter(100.0f / rate_mult); + /* set main loop delay depending on data rate to minimize CPU overhead */ + _main_loop_delay = MAIN_LOOP_DELAY / rate_mult; + while (!_task_should_exit) { /* main loop */ - usleep(MAIN_LOOP_DELAY); + usleep(_main_loop_delay); perf_begin(_loop_perf); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 94bef7e726..b52c12796d 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -196,7 +196,9 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ /* states */ - bool _mavlink_hil_enabled; /**< Hardware in the loop mode */ + bool _mavlink_hil_enabled; /**< Hardware In the Loop mode */ + + unsigned _main_loop_delay; /**< mainloop delay, depends on data rate */ MavlinkOrbSubscription *_subscriptions; MavlinkStream *_streams; From a1ea89ea2d3665709f5249ebd7cc3fbadaca603a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Mar 2014 19:42:29 +0400 Subject: [PATCH 116/381] mavlink: more precise message streams timing --- src/modules/mavlink/mavlink_main.cpp | 8 ++++---- src/modules/mavlink/mavlink_rate_limiter.cpp | 4 ++-- src/modules/mavlink/mavlink_stream.cpp | 2 +- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index fb29c9c716..e6c474aa73 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1462,8 +1462,8 @@ MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic) int Mavlink::configure_stream(const char *stream_name, const float rate) { - /* calculate interval in ms, 0 means disabled stream */ - unsigned int interval = (rate > 0.0f) ? (1000.0f / rate) : 0; + /* calculate interval in us, 0 means disabled stream */ + unsigned int interval = (rate > 0.0f) ? (1000000.0f / rate) : 0; /* search if stream exists */ MavlinkStream *stream; @@ -1752,8 +1752,8 @@ Mavlink::task_main(int argc, char *argv[]) /* don't send parameters on startup without request */ _mavlink_param_queue_index = param_count(); - MavlinkRateLimiter slow_rate_limiter(2000.0f / rate_mult); - MavlinkRateLimiter fast_rate_limiter(100.0f / rate_mult); + MavlinkRateLimiter slow_rate_limiter(2000000.0f / rate_mult); + MavlinkRateLimiter fast_rate_limiter(100000.0f / rate_mult); /* set main loop delay depending on data rate to minimize CPU overhead */ _main_loop_delay = MAIN_LOOP_DELAY / rate_mult; diff --git a/src/modules/mavlink/mavlink_rate_limiter.cpp b/src/modules/mavlink/mavlink_rate_limiter.cpp index f6ed6e6627..9cdda8b17c 100644 --- a/src/modules/mavlink/mavlink_rate_limiter.cpp +++ b/src/modules/mavlink/mavlink_rate_limiter.cpp @@ -44,7 +44,7 @@ MavlinkRateLimiter::MavlinkRateLimiter() : _last_sent(0), _interval(1000000) { } -MavlinkRateLimiter::MavlinkRateLimiter(unsigned int interval) : _last_sent(0), _interval(interval * 1000) +MavlinkRateLimiter::MavlinkRateLimiter(unsigned int interval) : _last_sent(0), _interval(interval) { } @@ -55,7 +55,7 @@ MavlinkRateLimiter::~MavlinkRateLimiter() void MavlinkRateLimiter::set_interval(unsigned int interval) { - _interval = interval * 1000; + _interval = interval; } bool diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index 8694950985..bb19d7e336 100644 --- a/src/modules/mavlink/mavlink_stream.cpp +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -57,7 +57,7 @@ MavlinkStream::~MavlinkStream() void MavlinkStream::set_interval(const unsigned int interval) { - _interval = interval * 1000; + _interval = interval; } /** From 85001b52aeddaab0f5a1faea8eade8294d447e9f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Mar 2014 20:09:02 +0400 Subject: [PATCH 117/381] mavlink: VFR_HUD message added to default message sets --- src/modules/mavlink/mavlink_main.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index e6c474aa73..c10c95dbdb 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1727,10 +1727,10 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("GPS_GLOBAL_ORIGIN", 0.5f); configure_stream("HIGHRES_IMU", 1.0f * rate_mult); configure_stream("ATTITUDE", 10.0f * rate_mult); + configure_stream("VFR_HUD", 10.0f * rate_mult); configure_stream("GPS_RAW_INT", 1.0f * rate_mult); - configure_stream("GLOBAL_POSITION_INT", 5.0f * rate_mult); - configure_stream("LOCAL_POSITION_NED", 5.0f * rate_mult); - configure_stream("SERVO_OUTPUT_RAW_0", 1.0f * rate_mult); + configure_stream("GLOBAL_POSITION_INT", 3.0f * rate_mult); + configure_stream("LOCAL_POSITION_NED", 3.0f * rate_mult); break; case MODE_HIL: @@ -1739,6 +1739,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("GPS_GLOBAL_ORIGIN", 0.5f); configure_stream("HIGHRES_IMU", 1.0f * rate_mult); configure_stream("ATTITUDE", 2.0f * rate_mult); + configure_stream("VFR_HUD", 2.0f * rate_mult); configure_stream("GPS_RAW_INT", 1.0f * rate_mult); configure_stream("GLOBAL_POSITION_INT", 1.0f * rate_mult); configure_stream("LOCAL_POSITION_NED", 1.0f * rate_mult); From 63bdb749adc26550b0cfb6dd8b4f0e0a7173ba10 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Mar 2014 22:41:27 +0400 Subject: [PATCH 118/381] mavlink: unused include removed --- src/modules/mavlink/mavlink_receiver.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index b6e008cbfc..032958b74f 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -76,7 +76,6 @@ __BEGIN_DECLS #include "mavlink_bridge_header.h" -// #include "waypoints.h" #include "mavlink_receiver.h" #include "mavlink_main.h" #include "util.h" From 624ff010185c16dffacc8d000aae2748ff1cd0a1 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 2 Mar 2014 12:53:46 +0400 Subject: [PATCH 119/381] mavlink: HIL fixes, send 0 when disarmed --- src/modules/mavlink/mavlink_messages.cpp | 88 +++++++++--------------- 1 file changed, 34 insertions(+), 54 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 8be7d76d70..c22289772c 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -778,65 +778,45 @@ protected: uint32_t mavlink_custom_mode; get_mavlink_mode_state(status, pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); - /* HIL message as per MAVLink spec */ + /* set number of valid outputs depending on vehicle type */ + unsigned n; + switch (mavlink_system.type) { + case MAV_TYPE_QUADROTOR: + n = 4; + break; + + case MAV_TYPE_HEXAROTOR: + n = 6; + break; + + default: + n = 8; + break; + } /* scale / assign outputs depending on system type */ - if (mavlink_system.type == MAV_TYPE_QUADROTOR) { - mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - ((act->output[0] - 900.0f) / 600.0f) / 2.0f, - ((act->output[1] - 900.0f) / 600.0f) / 2.0f, - ((act->output[2] - 900.0f) / 600.0f) / 2.0f, - ((act->output[3] - 900.0f) / 600.0f) / 2.0f, - -1, - -1, - -1, - -1, - mavlink_base_mode, - 0); + float out[8]; - } else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) { - mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - ((act->output[0] - 900.0f) / 600.0f) / 2.0f, - ((act->output[1] - 900.0f) / 600.0f) / 2.0f, - ((act->output[2] - 900.0f) / 600.0f) / 2.0f, - ((act->output[3] - 900.0f) / 600.0f) / 2.0f, - ((act->output[4] - 900.0f) / 600.0f) / 2.0f, - ((act->output[5] - 900.0f) / 600.0f) / 2.0f, - -1, - -1, - mavlink_base_mode, - 0); + for (unsigned i = 0; i < 8; i++) { + if (i < n) { + if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { + /* scale fake PWM out 900..1200 us to 0..1*/ + out[i] = (act->output[i] - 900.0f) / 1200.0f; - } else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) { - mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - ((act->output[0] - 900.0f) / 600.0f) / 2.0f, - ((act->output[1] - 900.0f) / 600.0f) / 2.0f, - ((act->output[2] - 900.0f) / 600.0f) / 2.0f, - ((act->output[3] - 900.0f) / 600.0f) / 2.0f, - ((act->output[4] - 900.0f) / 600.0f) / 2.0f, - ((act->output[5] - 900.0f) / 600.0f) / 2.0f, - ((act->output[6] - 900.0f) / 600.0f) / 2.0f, - ((act->output[7] - 900.0f) / 600.0f) / 2.0f, - mavlink_base_mode, - 0); - - } else { - mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - (act->output[0] - 1500.0f) / 500.0f, - (act->output[1] - 1500.0f) / 500.0f, - (act->output[2] - 1500.0f) / 500.0f, - (act->output[3] - 1000.0f) / 1000.0f, - (act->output[4] - 1500.0f) / 500.0f, - (act->output[5] - 1500.0f) / 500.0f, - (act->output[6] - 1500.0f) / 500.0f, - (act->output[7] - 1500.0f) / 500.0f, - mavlink_base_mode, - 0); + } else { + /* send 0 when disarmed */ + out[i] = 0.0f; + } + } else { + out[i] = -1.0f; + } } + + mavlink_msg_hil_controls_send(_channel, + hrt_absolute_time(), + out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], + mavlink_base_mode, + 0); } }; From a16742f2eb740adef925cc0d1c90faceaceb97b9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 3 Mar 2014 14:23:49 +0100 Subject: [PATCH 120/381] Fixes to altitude units --- .../fw_att_pos_estimator_main.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index d598052222..082c695dc9 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -701,23 +701,24 @@ FixedwingEstimator::task_main() velNED[0] = _gps.vel_n_m_s; velNED[1] = _gps.vel_e_m_s; velNED[2] = _gps.vel_d_m_s; + + double lat = _gps.lat * 1e-7; + double lon = _gps.lon * 1e-7; + float alt = _gps.alt * 1e-3; + InitialiseFilter(velNED); // Initialize projection _local_pos.ref_lat = _gps.lat; _local_pos.ref_lon = _gps.lon; - _local_pos.ref_alt = _gps.alt; + _local_pos.ref_alt = alt; _local_pos.ref_timestamp = _gps.timestamp_position; // Store _baro_ref = baroHgt; - _baro_gps_offset = baroHgt - _gps.alt; + _baro_gps_offset = baroHgt - alt; // XXX this is not multithreading safe - double lat = _gps.lat * 1e-7; - double lon = _gps.lon * 1e-7; - float alt = _gps.alt * 1e-3; - map_projection_init(lat, lon); mavlink_log_info(_mavlink_fd, "[position estimator] init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt); From b809fad3b6d1e14ad7fb32174136e1c2c21f1da2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 3 Mar 2014 14:24:17 +0100 Subject: [PATCH 121/381] Fixes to startup scripts, back to nominal defaults --- ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil | 2 +- ROMFS/px4fmu_common/init.d/rcS | 2 +- 2 files changed, 2 insertions(+), 2 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 36194ad682..d114fe21a2 100644 --- a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil +++ b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil @@ -7,7 +7,7 @@ sh /etc/init.d/rc.fw_defaults -echo "HIL Rascal 110 starting.." +echo "X Plane HIL starting.." set HIL yes diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index b471a2a5c4..2356dba541 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -385,7 +385,7 @@ then then sleep 1 mavlink start -b 230400 -d /dev/ttyACM0 - gps start -f + gps start usleep 5000 else if [ $TTYS1_BUSY == yes ] From cb8095c5ac932bbf49137491d92b4cf064021058 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 3 Mar 2014 19:56:37 +0400 Subject: [PATCH 122/381] navigator: fixed wrong merge --- src/modules/navigator/navigator_main.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index ff1932c66c..c45cafc1ba 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1112,6 +1112,8 @@ Navigator::set_mission_item() ret = _mission.get_current_mission_item(&_mission_item, &onboard, &index); if (ret == OK) { + _mission.report_current_offboard_mission_item(); + _mission_item_valid = true; position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); From 3107f4d62cb07de70619093be57ce2b634763eba Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 4 Mar 2014 00:26:26 +0400 Subject: [PATCH 123/381] mavlink: UART receiver major cleanup --- src/modules/mavlink/mavlink_receiver.cpp | 824 ++++++++++++----------- src/modules/mavlink/mavlink_receiver.h | 57 +- 2 files changed, 444 insertions(+), 437 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 032958b74f..8a80277386 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -82,39 +82,48 @@ __BEGIN_DECLS __END_DECLS +static const float mg2ms2 = 9.8f / 1000.0f; + MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _mavlink(parent), - pub_hil_global_pos(-1), - pub_hil_local_pos(-1), - pub_hil_attitude(-1), - pub_hil_gps(-1), - pub_hil_sensors(-1), - pub_hil_gyro(-1), - pub_hil_accel(-1), - pub_hil_mag(-1), - pub_hil_baro(-1), - pub_hil_airspeed(-1), - pub_hil_battery(-1), - hil_counter(0), - hil_frames(0), - old_timestamp(0), - cmd_pub(-1), - flow_pub(-1), - offboard_control_sp_pub(-1), - vicon_position_pub(-1), - telemetry_status_pub(-1), - lat0(0), - lon0(0), - alt0(0.0) -{ + _manual_sub(-1), + + _global_pos_pub(-1), + _local_pos_pub(-1), + _attitude_pub(-1), + _gps_pub(-1), + _sensors_pub(-1), + _gyro_pub(-1), + _accel_pub(-1), + _mag_pub(-1), + _baro_pub(-1), + _airspeed_pub(-1), + _battery_pub(-1), + _cmd_pub(-1), + _flow_pub(-1), + _offboard_control_sp_pub(-1), + _vicon_position_pub(-1), + _telemetry_status_pub(-1), + _rc_pub(-1), + _manual_pub(-1), + + _hil_counter(0), + _hil_frames(0), + _old_timestamp(0), + _hil_local_proj_inited(0), + _hil_local_alt0(0.0) +{ + memset(&hil_local_pos, 0, sizeof(hil_local_pos)); } void MavlinkReceiver::handle_message(mavlink_message_t *msg) { - if (msg->msgid == MAVLINK_MSG_ID_COMMAND_LONG) { + uint64_t timestamp = hrt_absolute_time(); + if (msg->msgid == MAVLINK_MSG_ID_COMMAND_LONG) { + /* command */ mavlink_command_long_t cmd_mavlink; mavlink_msg_command_long_decode(msg, &cmd_mavlink); @@ -131,6 +140,8 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) _mavlink->_task_should_exit = true; } else { + struct vehicle_command_s vcmd; + memset(&vcmd, 0, sizeof(vcmd)); /* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ vcmd.param1 = cmd_mavlink.param1; @@ -149,24 +160,25 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) vcmd.confirmation = cmd_mavlink.confirmation; /* check if topic is advertised */ - if (cmd_pub <= 0) { - cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); + if (_cmd_pub <= 0) { + _cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); } else { /* publish */ - orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd); + orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd); } } } - } - if (msg->msgid == MAVLINK_MSG_ID_OPTICAL_FLOW) { + } else if (msg->msgid == MAVLINK_MSG_ID_OPTICAL_FLOW) { + /* optical flow */ mavlink_optical_flow_t flow; mavlink_msg_optical_flow_decode(msg, &flow); struct optical_flow_s f; + memset(&f, 0, sizeof(f)); - f.timestamp = flow.time_usec; + f.timestamp = timestamp; f.flow_raw_x = flow.flow_x; f.flow_raw_y = flow.flow_y; f.flow_comp_x_m = flow.flow_comp_m_x; @@ -175,21 +187,21 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) f.quality = flow.quality; f.sensor_id = flow.sensor_id; - /* check if topic is advertised */ - if (flow_pub <= 0) { - flow_pub = orb_advertise(ORB_ID(optical_flow), &f); + if (_flow_pub <= 0) { + _flow_pub = orb_advertise(ORB_ID(optical_flow), &f); } else { - /* publish */ - orb_publish(ORB_ID(optical_flow), flow_pub, &f); + orb_publish(ORB_ID(optical_flow), _flow_pub, &f); } - } - if (msg->msgid == MAVLINK_MSG_ID_SET_MODE) { - /* Set mode on request */ + } else if (msg->msgid == MAVLINK_MSG_ID_SET_MODE) { + /* set mode on request */ mavlink_set_mode_t new_mode; mavlink_msg_set_mode_decode(msg, &new_mode); + struct vehicle_command_s vcmd; + memset(&vcmd, 0, sizeof(vcmd)); + union px4_custom_mode custom_mode; custom_mode.data = new_mode.custom_mode; /* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ @@ -208,21 +220,22 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) vcmd.confirmation = 1; /* check if topic is advertised */ - if (cmd_pub <= 0) { - cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); + if (_cmd_pub <= 0) { + _cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); } else { /* create command */ - orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd); + orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd); } - } - /* Handle Vicon position estimates */ - if (msg->msgid == MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE) { + } else if (msg->msgid == MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE) { + /* vicon */ mavlink_vicon_position_estimate_t pos; mavlink_msg_vicon_position_estimate_decode(msg, &pos); - vicon_position.timestamp = hrt_absolute_time(); + struct vehicle_vicon_position_s vicon_position; + memset(&vicon_position, 0, sizeof(vicon_position)); + vicon_position.timestamp = timestamp; vicon_position.x = pos.x; vicon_position.y = pos.y; @@ -232,21 +245,21 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) vicon_position.pitch = pos.pitch; vicon_position.yaw = pos.yaw; - if (vicon_position_pub <= 0) { - vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position); + if (_vicon_position_pub <= 0) { + _vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position); } else { - orb_publish(ORB_ID(vehicle_vicon_position), vicon_position_pub, &vicon_position); + orb_publish(ORB_ID(vehicle_vicon_position), _vicon_position_pub, &vicon_position); } - } - /* Handle quadrotor motor setpoints */ - - if (msg->msgid == MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST) { + } else if (msg->msgid == MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST) { + /* offboard control */ mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t quad_motors_setpoint; mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &quad_motors_setpoint); if (mavlink_system.sysid < 4) { + struct offboard_control_setpoint_s offboard_control_sp; + memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); /* switch to a receiving link mode */ //TODO why do we need this? @@ -297,29 +310,25 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) offboard_control_sp.armed = ml_armed; offboard_control_sp.mode = static_cast(ml_mode); - offboard_control_sp.timestamp = hrt_absolute_time(); + offboard_control_sp.timestamp = timestamp; - /* check if topic has to be advertised */ - 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 { - /* Publish */ - orb_publish(ORB_ID(offboard_control_setpoint), offboard_control_sp_pub, &offboard_control_sp); + orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp); } } - } - - /* handle status updates of the radio */ - if (msg->msgid == MAVLINK_MSG_ID_RADIO_STATUS) { - - struct telemetry_status_s tstatus; + } else if (msg->msgid == MAVLINK_MSG_ID_RADIO_STATUS) { + /* telemetry status */ mavlink_radio_status_t rstatus; mavlink_msg_radio_status_decode(msg, &rstatus); - /* publish telemetry status topic */ - tstatus.timestamp = hrt_absolute_time(); + struct telemetry_status_s tstatus; + memset(&tstatus, 0, sizeof(tstatus)); + + tstatus.timestamp = timestamp; tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO; tstatus.rssi = rstatus.rssi; tstatus.remote_rssi = rstatus.remrssi; @@ -329,11 +338,59 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) tstatus.rxerrors = rstatus.rxerrors; tstatus.fixed = rstatus.fixed; - if (telemetry_status_pub <= 0) { - telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus); + if (_telemetry_status_pub <= 0) { + _telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus); } else { - orb_publish(ORB_ID(telemetry_status), telemetry_status_pub, &tstatus); + orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus); + } + + } else if (msg->msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) { + /* manual control */ + mavlink_manual_control_t man; + mavlink_msg_manual_control_decode(msg, &man); + + /* rc channels */ + { + struct rc_channels_s rc; + memset(&rc, 0, sizeof(rc)); + + rc.timestamp = timestamp; + rc.chan_count = 4; + + rc.chan[0].scaled = man.x / 1000.0f; + rc.chan[1].scaled = man.y / 1000.0f; + rc.chan[2].scaled = man.r / 1000.0f; + rc.chan[3].scaled = man.z / 1000.0f; + + if (_rc_pub == 0) { + _rc_pub = orb_advertise(ORB_ID(rc_channels), &rc); + + } else { + orb_publish(ORB_ID(rc_channels), _rc_pub, &rc); + } + } + + /* manual control */ + { + struct manual_control_setpoint_s manual; + memset(&manual, 0, sizeof(manual)); + + /* get a copy first, to prevent altering values that are not sent by the mavlink command */ + orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &manual); + + manual.timestamp = timestamp; + manual.roll = man.x / 1000.0f; + manual.pitch = man.y / 1000.0f; + manual.yaw = man.r / 1000.0f; + manual.throttle = man.z / 1000.0f; + + if (_manual_pub == 0) { + _manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual); + + } else { + orb_publish(ORB_ID(manual_control_setpoint), _manual_pub, &manual); + } } } @@ -346,270 +403,301 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) */ if (_mavlink->get_hil_enabled()) { - - uint64_t timestamp = hrt_absolute_time(); - if (msg->msgid == MAVLINK_MSG_ID_HIL_SENSOR) { - + /* HIL sensors */ mavlink_hil_sensor_t imu; mavlink_msg_hil_sensor_decode(msg, &imu); - /* sensors general */ - hil_sensors.timestamp = hrt_absolute_time(); + /* airspeed */ + { + struct airspeed_s airspeed; + memset(&airspeed, 0, sizeof(airspeed)); - /* hil gyro */ - static const float mrad2rad = 1.0e-3f; - hil_sensors.gyro_raw[0] = imu.xgyro / mrad2rad; - hil_sensors.gyro_raw[1] = imu.ygyro / mrad2rad; - hil_sensors.gyro_raw[2] = imu.zgyro / mrad2rad; - hil_sensors.gyro_rad_s[0] = imu.xgyro; - hil_sensors.gyro_rad_s[1] = imu.ygyro; - hil_sensors.gyro_rad_s[2] = imu.zgyro; - hil_sensors.gyro_counter = hil_counter; + float ias = calc_indicated_airspeed(imu.diff_pressure * 1e2f); + // XXX need to fix this + float tas = ias; + + airspeed.timestamp = timestamp; + airspeed.indicated_airspeed_m_s = ias; + airspeed.true_airspeed_m_s = tas; + + if (_airspeed_pub < 0) { + _airspeed_pub = orb_advertise(ORB_ID(airspeed), &airspeed); + + } else { + orb_publish(ORB_ID(airspeed), _airspeed_pub, &airspeed); + } + } + + /* gyro */ + { + struct gyro_report gyro; + memset(&gyro, 0, sizeof(gyro)); + + gyro.timestamp = timestamp; + gyro.x_raw = imu.xgyro * 1000.0f; + gyro.y_raw = imu.ygyro * 1000.0f; + gyro.z_raw = imu.zgyro * 1000.0f; + gyro.x = imu.xgyro; + gyro.y = imu.ygyro; + gyro.z = imu.zgyro; + gyro.temperature = imu.temperature; + + if (_gyro_pub < 0) { + _gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro); + + } else { + orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &gyro); + } + } /* accelerometer */ - static const float mg2ms2 = 9.8f / 1000.0f; - hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2; - hil_sensors.accelerometer_raw[1] = imu.yacc / mg2ms2; - hil_sensors.accelerometer_raw[2] = imu.zacc / mg2ms2; - hil_sensors.accelerometer_m_s2[0] = imu.xacc; - hil_sensors.accelerometer_m_s2[1] = imu.yacc; - hil_sensors.accelerometer_m_s2[2] = imu.zacc; - hil_sensors.accelerometer_mode = 0; // TODO what is this? - hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16 - hil_sensors.accelerometer_counter = hil_counter; + { + struct accel_report accel; + memset(&accel, 0, sizeof(accel)); - /* adc */ - hil_sensors.adc_voltage_v[0] = 0.0f; - hil_sensors.adc_voltage_v[1] = 0.0f; - hil_sensors.adc_voltage_v[2] = 0.0f; + accel.timestamp = timestamp; + accel.x_raw = imu.xacc / mg2ms2; + accel.y_raw = imu.yacc / mg2ms2; + accel.z_raw = imu.zacc / mg2ms2; + accel.x = imu.xacc; + accel.y = imu.yacc; + accel.z = imu.zacc; + accel.temperature = imu.temperature; + + if (_accel_pub < 0) { + _accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel); + + } else { + orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel); + } + } /* magnetometer */ - float mga2ga = 1.0e-3f; - hil_sensors.magnetometer_raw[0] = imu.xmag / mga2ga; - hil_sensors.magnetometer_raw[1] = imu.ymag / mga2ga; - hil_sensors.magnetometer_raw[2] = imu.zmag / mga2ga; - hil_sensors.magnetometer_ga[0] = imu.xmag; - hil_sensors.magnetometer_ga[1] = imu.ymag; - hil_sensors.magnetometer_ga[2] = imu.zmag; - hil_sensors.magnetometer_range_ga = 32.7f; // int16 - hil_sensors.magnetometer_mode = 0; // TODO what is this - hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f; - hil_sensors.magnetometer_counter = hil_counter; + { + struct mag_report mag; + memset(&mag, 0, sizeof(mag)); + + mag.timestamp = timestamp; + mag.x_raw = imu.xmag * 1000.0f; + mag.y_raw = imu.ymag * 1000.0f; + mag.z_raw = imu.zmag * 1000.0f; + mag.x = imu.xmag; + mag.y = imu.ymag; + mag.z = imu.zmag; + + if (_mag_pub < 0) { + _mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag); + + } else { + orb_publish(ORB_ID(sensor_mag), _mag_pub, &mag); + } + } /* baro */ - hil_sensors.baro_pres_mbar = imu.abs_pressure; - hil_sensors.baro_alt_meter = imu.pressure_alt; - hil_sensors.baro_temp_celcius = imu.temperature; - hil_sensors.baro_counter = hil_counter; + { + struct baro_report baro; + memset(&baro, 0, sizeof(baro)); - /* differential pressure */ - hil_sensors.differential_pressure_pa = imu.diff_pressure * 1e2f; //from hPa to Pa - hil_sensors.differential_pressure_counter = hil_counter; + baro.timestamp = timestamp; + baro.pressure = imu.abs_pressure; + baro.altitude = imu.pressure_alt; + baro.temperature = imu.temperature; - /* airspeed from differential pressure, ambient pressure and temp */ - struct airspeed_s airspeed; + if (_baro_pub < 0) { + _baro_pub = orb_advertise(ORB_ID(sensor_baro), &baro); - float ias = calc_indicated_airspeed(hil_sensors.differential_pressure_pa); - // XXX need to fix this - float tas = ias; - - airspeed.timestamp = hrt_absolute_time(); - airspeed.indicated_airspeed_m_s = ias; - airspeed.true_airspeed_m_s = tas; - - if (pub_hil_airspeed < 0) { - pub_hil_airspeed = orb_advertise(ORB_ID(airspeed), &airspeed); - - } else { - orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed); + } else { + orb_publish(ORB_ID(sensor_baro), _baro_pub, &baro); + } } - //warnx("SENSOR: IAS: %6.2f TAS: %6.2f", airspeed.indicated_airspeed_m_s, airspeed.true_airspeed_m_s); + /* sensor combined */ + { + struct sensor_combined_s hil_sensors; + memset(&hil_sensors, 0, sizeof(hil_sensors)); - /* individual sensor publications */ - struct gyro_report gyro; - gyro.x_raw = imu.xgyro / mrad2rad; - gyro.y_raw = imu.ygyro / mrad2rad; - gyro.z_raw = imu.zgyro / mrad2rad; - gyro.x = imu.xgyro; - gyro.y = imu.ygyro; - gyro.z = imu.zgyro; - gyro.temperature = imu.temperature; - gyro.timestamp = hrt_absolute_time(); + hil_sensors.timestamp = timestamp; - if (pub_hil_gyro < 0) { - pub_hil_gyro = orb_advertise(ORB_ID(sensor_gyro), &gyro); + hil_sensors.gyro_raw[0] = imu.xgyro * 1000.0f; + hil_sensors.gyro_raw[1] = imu.ygyro * 1000.0f; + hil_sensors.gyro_raw[2] = imu.zgyro * 1000.0f; + hil_sensors.gyro_rad_s[0] = imu.xgyro; + hil_sensors.gyro_rad_s[1] = imu.ygyro; + hil_sensors.gyro_rad_s[2] = imu.zgyro; + hil_sensors.gyro_counter = _hil_counter; - } else { - orb_publish(ORB_ID(sensor_gyro), pub_hil_gyro, &gyro); + hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2; + hil_sensors.accelerometer_raw[1] = imu.yacc / mg2ms2; + hil_sensors.accelerometer_raw[2] = imu.zacc / mg2ms2; + hil_sensors.accelerometer_m_s2[0] = imu.xacc; + hil_sensors.accelerometer_m_s2[1] = imu.yacc; + hil_sensors.accelerometer_m_s2[2] = imu.zacc; + hil_sensors.accelerometer_mode = 0; // TODO what is this? + hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16 + hil_sensors.accelerometer_counter = _hil_counter; + + hil_sensors.adc_voltage_v[0] = 0.0f; + hil_sensors.adc_voltage_v[1] = 0.0f; + hil_sensors.adc_voltage_v[2] = 0.0f; + + hil_sensors.magnetometer_raw[0] = imu.xmag * 1000.0f; + hil_sensors.magnetometer_raw[1] = imu.ymag * 1000.0f; + hil_sensors.magnetometer_raw[2] = imu.zmag * 1000.0f; + hil_sensors.magnetometer_ga[0] = imu.xmag; + hil_sensors.magnetometer_ga[1] = imu.ymag; + hil_sensors.magnetometer_ga[2] = imu.zmag; + hil_sensors.magnetometer_range_ga = 32.7f; // int16 + hil_sensors.magnetometer_mode = 0; // TODO what is this + hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f; + hil_sensors.magnetometer_counter = _hil_counter; + + hil_sensors.baro_pres_mbar = imu.abs_pressure; + hil_sensors.baro_alt_meter = imu.pressure_alt; + hil_sensors.baro_temp_celcius = imu.temperature; + hil_sensors.baro_counter = _hil_counter; + + hil_sensors.differential_pressure_pa = imu.diff_pressure * 1e2f; //from hPa to Pa + hil_sensors.differential_pressure_counter = _hil_counter; + + /* publish combined sensor topic */ + if (_sensors_pub > 0) { + orb_publish(ORB_ID(sensor_combined), _sensors_pub, &hil_sensors); + + } else { + _sensors_pub = orb_advertise(ORB_ID(sensor_combined), &hil_sensors); + } } - struct accel_report accel; + /* battery status */ + { + struct battery_status_s hil_battery_status; + memset(&hil_battery_status, 0, sizeof(hil_battery_status)); - accel.x_raw = imu.xacc / mg2ms2; + hil_battery_status.timestamp = timestamp; + hil_battery_status.voltage_v = 11.1f; + hil_battery_status.current_a = 10.0f; - accel.y_raw = imu.yacc / mg2ms2; + if (_battery_pub > 0) { + orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); - accel.z_raw = imu.zacc / mg2ms2; - - accel.x = imu.xacc; - - accel.y = imu.yacc; - - accel.z = imu.zacc; - - accel.temperature = imu.temperature; - - accel.timestamp = hrt_absolute_time(); - - if (pub_hil_accel < 0) { - pub_hil_accel = orb_advertise(ORB_ID(sensor_accel), &accel); - - } else { - orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel); + } else { + _baro_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); + } } - struct mag_report mag; + /* increment counters */ + _hil_counter++; + _hil_frames++; - mag.x_raw = imu.xmag / mga2ga; - - mag.y_raw = imu.ymag / mga2ga; - - mag.z_raw = imu.zmag / mga2ga; - - mag.x = imu.xmag; - - mag.y = imu.ymag; - - mag.z = imu.zmag; - - mag.timestamp = hrt_absolute_time(); - - if (pub_hil_mag < 0) { - pub_hil_mag = orb_advertise(ORB_ID(sensor_mag), &mag); - - } else { - orb_publish(ORB_ID(sensor_mag), pub_hil_mag, &mag); + /* print HIL sensors rate */ + if ((timestamp - _old_timestamp) > 10000000) { + printf("receiving HIL sensors at %d hz\n", _hil_frames / 10); + _old_timestamp = timestamp; + _hil_frames = 0; } - struct baro_report baro; - - baro.pressure = imu.abs_pressure; - - baro.altitude = imu.pressure_alt; - - baro.temperature = imu.temperature; - - baro.timestamp = hrt_absolute_time(); - - if (pub_hil_baro < 0) { - pub_hil_baro = orb_advertise(ORB_ID(sensor_baro), &baro); - - } else { - orb_publish(ORB_ID(sensor_baro), pub_hil_baro, &baro); - } - - /* publish combined sensor topic */ - if (pub_hil_sensors > 0) { - orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors); - - } else { - pub_hil_sensors = orb_advertise(ORB_ID(sensor_combined), &hil_sensors); - } - - /* fill in HIL battery status */ - hil_battery_status.timestamp = hrt_absolute_time(); - hil_battery_status.voltage_v = 11.1f; - hil_battery_status.current_a = 10.0f; - - /* lazily publish the battery voltage */ - if (pub_hil_battery > 0) { - orb_publish(ORB_ID(battery_status), pub_hil_battery, &hil_battery_status); - - } else { - pub_hil_battery = orb_advertise(ORB_ID(battery_status), &hil_battery_status); - } - - // increment counters - hil_counter++; - hil_frames++; - - // output - if ((timestamp - old_timestamp) > 10000000) { - printf("receiving hil sensor at %d hz\n", hil_frames / 10); - old_timestamp = timestamp; - hil_frames = 0; - } - } - - if (msg->msgid == MAVLINK_MSG_ID_HIL_GPS) { - + } else if (msg->msgid == MAVLINK_MSG_ID_HIL_GPS) { + /* HIL GPS */ mavlink_hil_gps_t gps; mavlink_msg_hil_gps_decode(msg, &gps); - /* gps */ - hil_gps.timestamp_position = gps.time_usec; + struct vehicle_gps_position_s hil_gps; + memset(&hil_gps, 0, sizeof(hil_gps)); + + hil_gps.timestamp_time = timestamp; hil_gps.time_gps_usec = gps.time_usec; + + hil_gps.timestamp_position = timestamp; hil_gps.lat = gps.lat; hil_gps.lon = gps.lon; hil_gps.alt = gps.alt; hil_gps.eph_m = (float)gps.eph * 1e-2f; // from cm to m hil_gps.epv_m = (float)gps.epv * 1e-2f; // from cm to m - hil_gps.timestamp_variance = gps.time_usec; + + hil_gps.timestamp_variance = timestamp; hil_gps.s_variance_m_s = 5.0f; hil_gps.p_variance_m = hil_gps.eph_m * hil_gps.eph_m; + + hil_gps.timestamp_velocity = timestamp; hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s - - /* gps.cog is in degrees 0..360 * 100, heading is -PI..+PI */ - float heading_rad = gps.cog * M_DEG_TO_RAD_F * 1e-2f; - - /* go back to -PI..PI */ - if (heading_rad > M_PI_F) { - heading_rad -= 2.0f * M_PI_F; - } - - hil_gps.timestamp_velocity = gps.time_usec; hil_gps.vel_n_m_s = gps.vn * 1e-2f; // from cm to m hil_gps.vel_e_m_s = gps.ve * 1e-2f; // from cm to m hil_gps.vel_d_m_s = gps.vd * 1e-2f; // from cm to m hil_gps.vel_ned_valid = true; - /* COG (course over ground) is spec'ed as -PI..+PI */ - hil_gps.cog_rad = heading_rad; + hil_gps.cog_rad = _wrap_pi(gps.cog * M_DEG_TO_RAD_F * 1e-2f); + + hil_gps.timestamp_satellites = timestamp; hil_gps.fix_type = gps.fix_type; hil_gps.satellites_visible = gps.satellites_visible; - /* publish GPS measurement data */ - if (pub_hil_gps > 0) { - orb_publish(ORB_ID(vehicle_gps_position), pub_hil_gps, &hil_gps); + if (_gps_pub > 0) { + orb_publish(ORB_ID(vehicle_gps_position), _gps_pub, &hil_gps); } else { - pub_hil_gps = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps); + _gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps); } - } - - if (msg->msgid == MAVLINK_MSG_ID_HIL_STATE_QUATERNION) { - + } else if (msg->msgid == MAVLINK_MSG_ID_HIL_STATE_QUATERNION) { + /* HIL state quaternion */ mavlink_hil_state_quaternion_t hil_state; mavlink_msg_hil_state_quaternion_decode(msg, &hil_state); - struct airspeed_s airspeed; - airspeed.timestamp = hrt_absolute_time(); - airspeed.indicated_airspeed_m_s = hil_state.ind_airspeed * 1e-2f; - airspeed.true_airspeed_m_s = hil_state.true_airspeed * 1e-2f; + /* airspeed */ + { + struct airspeed_s airspeed; + memset(&airspeed, 0, sizeof(airspeed)); - if (pub_hil_airspeed < 0) { - pub_hil_airspeed = orb_advertise(ORB_ID(airspeed), &airspeed); + airspeed.timestamp = timestamp; + airspeed.indicated_airspeed_m_s = hil_state.ind_airspeed * 1e-2f; + airspeed.true_airspeed_m_s = hil_state.true_airspeed * 1e-2f; - } else { - orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed); + if (_airspeed_pub < 0) { + _airspeed_pub = orb_advertise(ORB_ID(airspeed), &airspeed); + + } else { + orb_publish(ORB_ID(airspeed), _airspeed_pub, &airspeed); + } } - // publish global position - if (pub_hil_global_pos > 0) { - orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos); - // global position packet + /* attitude */ + struct vehicle_attitude_s hil_attitude; + { + memset(&hil_attitude, 0, sizeof(hil_attitude)); + math::Quaternion q(hil_state.attitude_quaternion); + math::Matrix<3, 3> C_nb = q.to_dcm(); + math::Vector<3> euler = C_nb.to_euler(); + + hil_attitude.timestamp = timestamp; + memcpy(hil_attitude.R, C_nb.data, sizeof(hil_attitude.R)); + hil_attitude.R_valid = true; + + hil_attitude.q[0] = q(0); + hil_attitude.q[1] = q(1); + hil_attitude.q[2] = q(2); + hil_attitude.q[3] = q(3); + hil_attitude.q_valid = true; + + hil_attitude.roll = euler(0); + hil_attitude.pitch = euler(1); + hil_attitude.yaw = euler(2); + hil_attitude.rollspeed = hil_state.rollspeed; + hil_attitude.pitchspeed = hil_state.pitchspeed; + hil_attitude.yawspeed = hil_state.yawspeed; + + if (_attitude_pub > 0) { + orb_publish(ORB_ID(vehicle_attitude), _attitude_pub, &hil_attitude); + + } else { + _attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude); + } + } + + /* global position */ + { + struct vehicle_global_position_s hil_global_pos; + memset(&hil_global_pos, 0, sizeof(hil_global_pos)); + hil_global_pos.timestamp = timestamp; hil_global_pos.global_valid = true; hil_global_pos.lat = hil_state.lat; @@ -618,19 +706,31 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) hil_global_pos.vel_n = hil_state.vx / 100.0f; hil_global_pos.vel_e = hil_state.vy / 100.0f; hil_global_pos.vel_d = hil_state.vz / 100.0f; + hil_global_pos.yaw = hil_attitude.yaw; - } else { - pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); + if (_global_pos_pub > 0) { + orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &hil_global_pos); + + } else { + _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); + } } - // publish local position - if (pub_hil_local_pos > 0) { + /* local position */ + { + if (!_hil_local_proj_inited) { + _hil_local_proj_inited = true; + _hil_local_alt0 = hil_state.alt / 1000.0f; + map_projection_init(hil_state.lat, hil_state.lon); + hil_local_pos.ref_timestamp = timestamp; + hil_local_pos.ref_lat = hil_state.lat; + hil_local_pos.ref_lon = hil_state.lon; + hil_local_pos.ref_alt = _hil_local_alt0; + } + float x; float y; - bool landed = (float)(hil_state.alt) / 1000.0f < (alt0 + 0.1f); // XXX improve? - double lat = hil_state.lat * 1e-7; - double lon = hil_state.lon * 1e-7; - map_projection_project(lat, lon, &x, &y); + map_projection_project(hil_state.lat * 1e-7, hil_state.lon * 1e-7, &x, &y); hil_local_pos.timestamp = timestamp; hil_local_pos.xy_valid = true; hil_local_pos.z_valid = true; @@ -638,148 +738,62 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) hil_local_pos.v_z_valid = true; hil_local_pos.x = x; hil_local_pos.y = y; - hil_local_pos.z = alt0 - hil_state.alt / 1000.0f; + hil_local_pos.z = _hil_local_alt0 - hil_state.alt / 1000.0f; hil_local_pos.vx = hil_state.vx / 100.0f; hil_local_pos.vy = hil_state.vy / 100.0f; hil_local_pos.vz = hil_state.vz / 100.0f; hil_local_pos.yaw = hil_attitude.yaw; hil_local_pos.xy_global = true; hil_local_pos.z_global = true; - hil_local_pos.ref_timestamp = timestamp; - hil_local_pos.ref_lat = hil_state.lat; - hil_local_pos.ref_lon = hil_state.lon; - hil_local_pos.ref_alt = alt0; + + bool landed = (float)(hil_state.alt) / 1000.0f < (_hil_local_alt0 + 0.1f); // XXX improve? hil_local_pos.landed = landed; - orb_publish(ORB_ID(vehicle_local_position), pub_hil_local_pos, &hil_local_pos); - } else { - pub_hil_local_pos = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos); - lat0 = hil_state.lat; - lon0 = hil_state.lon; - alt0 = hil_state.alt / 1000.0f; - map_projection_init(hil_state.lat, hil_state.lon); - } + if (_local_pos_pub > 0) { + orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &hil_local_pos); - /* Calculate Rotation Matrix */ - math::Quaternion q(hil_state.attitude_quaternion); - math::Matrix<3, 3> C_nb = q.to_dcm(); - math::Vector<3> euler = C_nb.to_euler(); - - /* set rotation matrix */ - for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) { - hil_attitude.R[i][j] = C_nb(i, j); + } else { + _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos); } - - hil_attitude.R_valid = true; - - /* set quaternion */ - hil_attitude.q[0] = q(0); - hil_attitude.q[1] = q(1); - hil_attitude.q[2] = q(2); - hil_attitude.q[3] = q(3); - hil_attitude.q_valid = true; - - hil_attitude.roll = euler(0); - hil_attitude.pitch = euler(1); - hil_attitude.yaw = euler(2); - hil_attitude.rollspeed = hil_state.rollspeed; - hil_attitude.pitchspeed = hil_state.pitchspeed; - hil_attitude.yawspeed = hil_state.yawspeed; - - /* set timestamp and notify processes (broadcast) */ - hil_attitude.timestamp = hrt_absolute_time(); - - if (pub_hil_attitude > 0) { - orb_publish(ORB_ID(vehicle_attitude), pub_hil_attitude, &hil_attitude); - - } else { - pub_hil_attitude = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude); } - struct accel_report accel; + /* accelerometer */ + { + struct accel_report accel; + memset(&accel, 0, sizeof(accel)); - accel.x_raw = hil_state.xacc / 9.81f * 1e3f; + accel.timestamp = timestamp; + accel.x_raw = hil_state.xacc / 9.81f * 1e3f; + accel.y_raw = hil_state.yacc / 9.81f * 1e3f; + accel.z_raw = hil_state.zacc / 9.81f * 1e3f; + accel.x = hil_state.xacc; + accel.y = hil_state.yacc; + accel.z = hil_state.zacc; + accel.temperature = 25.0f; - accel.y_raw = hil_state.yacc / 9.81f * 1e3f; + if (_accel_pub < 0) { + _accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel); - accel.z_raw = hil_state.zacc / 9.81f * 1e3f; - - accel.x = hil_state.xacc; - - accel.y = hil_state.yacc; - - accel.z = hil_state.zacc; - - accel.temperature = 25.0f; - - accel.timestamp = hrt_absolute_time(); - - if (pub_hil_accel < 0) { - pub_hil_accel = orb_advertise(ORB_ID(sensor_accel), &accel); - - } else { - orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel); + } else { + orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel); + } } - /* fill in HIL battery status */ - hil_battery_status.timestamp = hrt_absolute_time(); - hil_battery_status.voltage_v = 11.1f; - hil_battery_status.current_a = 10.0f; + /* battery status */ + { + struct battery_status_s hil_battery_status; + memset(&hil_battery_status, 0, sizeof(hil_battery_status)); - /* lazily publish the battery voltage */ - if (pub_hil_battery > 0) { - orb_publish(ORB_ID(battery_status), pub_hil_battery, &hil_battery_status); + hil_battery_status.timestamp = timestamp; + hil_battery_status.voltage_v = 11.1f; + hil_battery_status.current_a = 10.0f; - } else { - pub_hil_battery = orb_advertise(ORB_ID(battery_status), &hil_battery_status); - } - } + if (_battery_pub > 0) { + orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); - if (msg->msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) { - mavlink_manual_control_t man; - mavlink_msg_manual_control_decode(msg, &man); - - struct rc_channels_s rc_hil; - memset(&rc_hil, 0, sizeof(rc_hil)); - static orb_advert_t rc_pub = 0; - - rc_hil.timestamp = hrt_absolute_time(); - rc_hil.chan_count = 4; - - rc_hil.chan[0].scaled = man.x / 1000.0f; - rc_hil.chan[1].scaled = man.y / 1000.0f; - rc_hil.chan[2].scaled = man.r / 1000.0f; - rc_hil.chan[3].scaled = man.z / 1000.0f; - - struct manual_control_setpoint_s mc; - static orb_advert_t mc_pub = 0; - - int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - - /* get a copy first, to prevent altering values that are not sent by the mavlink command */ - orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &mc); - - mc.timestamp = rc_hil.timestamp; - mc.roll = man.x / 1000.0f; - mc.pitch = man.y / 1000.0f; - mc.yaw = man.r / 1000.0f; - mc.throttle = man.z / 1000.0f; - - /* fake RC channels with manual control input from simulator */ - - - if (rc_pub == 0) { - rc_pub = orb_advertise(ORB_ID(rc_channels), &rc_hil); - - } else { - orb_publish(ORB_ID(rc_channels), rc_pub, &rc_hil); - } - - if (mc_pub == 0) { - mc_pub = orb_advertise(ORB_ID(manual_control_setpoint), &mc); - - } else { - orb_publish(ORB_ID(manual_control_setpoint), mc_pub, &mc); + } else { + _battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); + } } } } @@ -799,11 +813,13 @@ MavlinkReceiver::receive_thread(void *arg) mavlink_message_t msg; - /* Set thread name */ + /* set thread name */ char thread_name[18]; sprintf(thread_name, "mavlink_uart_rcv_%d", _mavlink->get_channel()); prctl(PR_SET_NAME, thread_name, getpid()); + _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + struct pollfd fds[1]; fds[0].fd = uart_fd; fds[0].events = POLLIN; diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 199e426892..b97919e9da 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -100,7 +100,6 @@ public: static void *start_helper(void *context); private: - perf_counter_t _loop_perf; /**< loop performance counter */ Mavlink *_mavlink; @@ -109,37 +108,29 @@ private: void *receive_thread(void *arg); mavlink_status_t status; - struct vehicle_vicon_position_s vicon_position; - struct vehicle_command_s vcmd; - struct offboard_control_setpoint_s offboard_control_sp; - struct vehicle_global_position_s hil_global_pos; struct vehicle_local_position_s hil_local_pos; - struct vehicle_attitude_s hil_attitude; - struct vehicle_gps_position_s hil_gps; - struct sensor_combined_s hil_sensors; - struct battery_status_s hil_battery_status; - struct position_setpoint_triplet_s pos_sp_triplet; - orb_advert_t pub_hil_global_pos; - orb_advert_t pub_hil_local_pos; - orb_advert_t pub_hil_attitude; - orb_advert_t pub_hil_gps; - orb_advert_t pub_hil_sensors; - orb_advert_t pub_hil_gyro; - orb_advert_t pub_hil_accel; - orb_advert_t pub_hil_mag; - orb_advert_t pub_hil_baro; - orb_advert_t pub_hil_airspeed; - orb_advert_t pub_hil_battery; - int hil_counter; - int hil_frames; - uint64_t old_timestamp; - orb_advert_t cmd_pub; - orb_advert_t flow_pub; - orb_advert_t offboard_control_sp_pub; - orb_advert_t vicon_position_pub; - orb_advert_t telemetry_status_pub; - int32_t lat0; - int32_t lon0; - float alt0; - + int _manual_sub; + orb_advert_t _global_pos_pub; + orb_advert_t _local_pos_pub; + orb_advert_t _attitude_pub; + orb_advert_t _gps_pub; + orb_advert_t _sensors_pub; + orb_advert_t _gyro_pub; + orb_advert_t _accel_pub; + orb_advert_t _mag_pub; + orb_advert_t _baro_pub; + orb_advert_t _airspeed_pub; + orb_advert_t _battery_pub; + orb_advert_t _cmd_pub; + orb_advert_t _flow_pub; + orb_advert_t _offboard_control_sp_pub; + orb_advert_t _vicon_position_pub; + orb_advert_t _telemetry_status_pub; + orb_advert_t _rc_pub; + orb_advert_t _manual_pub; + int _hil_counter; + int _hil_frames; + uint64_t _old_timestamp; + bool _hil_local_proj_inited; + float _hil_local_alt0; }; From 14ddf1804aa9d698724fda62288b210fd850d2ac Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 4 Mar 2014 00:48:20 +0400 Subject: [PATCH 124/381] mavlink: code style fixed --- src/modules/mavlink/mavlink_messages.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index c22289772c..d4c77d1fd3 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -780,6 +780,7 @@ protected: /* set number of valid outputs depending on vehicle type */ unsigned n; + switch (mavlink_system.type) { case MAV_TYPE_QUADROTOR: n = 4; @@ -807,6 +808,7 @@ protected: /* send 0 when disarmed */ out[i] = 0.0f; } + } else { out[i] = -1.0f; } From 190eb6205dc3e610d223878c4b85a8e587fc6323 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 4 Mar 2014 11:45:58 +0400 Subject: [PATCH 125/381] mavlink: OPTICAL_FLOW stream implemented --- src/modules/mavlink/mavlink_messages.cpp | 51 +++++++++++++++++++----- 1 file changed, 40 insertions(+), 11 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index d4c77d1fd3..7475160d5e 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1063,6 +1063,45 @@ protected: }; +class MavlinkStreamOpticalFlow : public MavlinkStream +{ +public: + const char *get_name() + { + return "OPTICAL_FLOW"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamOpticalFlow(); + } + +private: + MavlinkOrbSubscription *flow_sub; + struct optical_flow_s *flow; + +protected: + void subscribe(Mavlink *mavlink) + { + flow_sub = mavlink->add_orb_subscription(ORB_ID(optical_flow)); + flow = (struct optical_flow_s *)flow_sub->get_data(); + } + + void send(const hrt_abstime t) + { + flow_sub->update(t); + + mavlink_msg_optical_flow_send(_channel, + flow->timestamp, + flow->sensor_id, + flow->flow_raw_x, flow->flow_raw_y, + flow->flow_comp_x_m, flow->flow_comp_y_m, + flow->quality, + flow->ground_distance_m); + } +}; + + MavlinkStream *streams_list[] = { new MavlinkStreamHeartbeat(), new MavlinkStreamSysStatus(), @@ -1085,6 +1124,7 @@ MavlinkStream *streams_list[] = { new MavlinkStreamRollPitchYawRatesThrustSetpoint(), new MavlinkStreamRCChannelsRaw(), new MavlinkStreamManualControl(), + new MavlinkStreamOpticalFlow(), nullptr }; @@ -1143,17 +1183,6 @@ MavlinkStream *streams_list[] = { //} // //void -//MavlinkOrbListener::l_optical_flow(const struct listener *l) -//{ -// struct optical_flow_s flow; -// -// orb_copy(ORB_ID(optical_flow), l->mavlink->get_subs()->optical_flow, &flow); -// -// mavlink_msg_optical_flow_send(l->mavlink->get_chan(), flow.timestamp, flow.sensor_id, flow.flow_raw_x, flow.flow_raw_y, -// flow.flow_comp_x_m, flow.flow_comp_y_m, flow.quality, flow.ground_distance_m); -//} -// -//void //MavlinkOrbListener::l_nav_cap(const struct listener *l) //{ // From 2ec4ee6fc08f5368a52028de83f420ffeb249698 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 4 Mar 2014 12:33:03 +0400 Subject: [PATCH 126/381] mavlink_receiver: split message handlers to separate methods --- src/modules/mavlink/mavlink_receiver.cpp | 1299 ++++++++++++---------- src/modules/mavlink/mavlink_receiver.h | 11 + 2 files changed, 692 insertions(+), 618 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 8a80277386..ef1a747da0 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -36,6 +36,7 @@ * MAVLink protocol message receive and dispatch * * @author Lorenz Meier + * @author Anton Babushkin */ /* XXX trim includes */ @@ -120,278 +121,37 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : void MavlinkReceiver::handle_message(mavlink_message_t *msg) { - uint64_t timestamp = hrt_absolute_time(); + switch (msg->msgid) { + case MAVLINK_MSG_ID_COMMAND_LONG: + handle_message_command_long(msg); + break; - if (msg->msgid == MAVLINK_MSG_ID_COMMAND_LONG) { - /* command */ - mavlink_command_long_t cmd_mavlink; - mavlink_msg_command_long_decode(msg, &cmd_mavlink); + case MAVLINK_MSG_ID_OPTICAL_FLOW: + handle_message_optical_flow(msg); + break; - if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid) - || (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) { - //check for MAVLINK terminate command - if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) { - /* This is the link shutdown command, terminate mavlink */ - printf("[mavlink] Terminating .. \n"); - fflush(stdout); - usleep(50000); + case MAVLINK_MSG_ID_SET_MODE: + handle_message_set_mode(msg); + break; - /* terminate other threads and this thread */ - _mavlink->_task_should_exit = true; + case MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE: + handle_message_vicon_position_estimate(msg); + break; - } else { - struct vehicle_command_s vcmd; - memset(&vcmd, 0, sizeof(vcmd)); + case MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST: + handle_message_quad_swarm_roll_pitch_yaw_thrust(msg); + break; - /* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ - vcmd.param1 = cmd_mavlink.param1; - vcmd.param2 = cmd_mavlink.param2; - vcmd.param3 = cmd_mavlink.param3; - vcmd.param4 = cmd_mavlink.param4; - vcmd.param5 = cmd_mavlink.param5; - vcmd.param6 = cmd_mavlink.param6; - vcmd.param7 = cmd_mavlink.param7; - // XXX do proper translation - vcmd.command = (enum VEHICLE_CMD)cmd_mavlink.command; - vcmd.target_system = cmd_mavlink.target_system; - vcmd.target_component = cmd_mavlink.target_component; - vcmd.source_system = msg->sysid; - vcmd.source_component = msg->compid; - vcmd.confirmation = cmd_mavlink.confirmation; + case MAVLINK_MSG_ID_RADIO_STATUS: + handle_message_radio_status(msg); + break; - /* check if topic is advertised */ - if (_cmd_pub <= 0) { - _cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); + case MAVLINK_MSG_ID_MANUAL_CONTROL: + handle_message_manual_control(msg); + break; - } else { - /* publish */ - orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd); - } - } - } - - } else if (msg->msgid == MAVLINK_MSG_ID_OPTICAL_FLOW) { - /* optical flow */ - mavlink_optical_flow_t flow; - mavlink_msg_optical_flow_decode(msg, &flow); - - struct optical_flow_s f; - memset(&f, 0, sizeof(f)); - - f.timestamp = timestamp; - 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); - } - - } else if (msg->msgid == MAVLINK_MSG_ID_SET_MODE) { - /* set mode on request */ - mavlink_set_mode_t new_mode; - mavlink_msg_set_mode_decode(msg, &new_mode); - - struct vehicle_command_s vcmd; - memset(&vcmd, 0, sizeof(vcmd)); - - union px4_custom_mode custom_mode; - custom_mode.data = new_mode.custom_mode; - /* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ - vcmd.param1 = new_mode.base_mode; - vcmd.param2 = custom_mode.main_mode; - vcmd.param3 = 0; - vcmd.param4 = 0; - vcmd.param5 = 0; - vcmd.param6 = 0; - vcmd.param7 = 0; - vcmd.command = VEHICLE_CMD_DO_SET_MODE; - vcmd.target_system = new_mode.target_system; - vcmd.target_component = MAV_COMP_ID_ALL; - vcmd.source_system = msg->sysid; - vcmd.source_component = msg->compid; - vcmd.confirmation = 1; - - /* check if topic is advertised */ - if (_cmd_pub <= 0) { - _cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); - - } else { - /* create command */ - orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd); - } - - } else if (msg->msgid == MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE) { - /* vicon */ - mavlink_vicon_position_estimate_t pos; - mavlink_msg_vicon_position_estimate_decode(msg, &pos); - - struct vehicle_vicon_position_s vicon_position; - memset(&vicon_position, 0, sizeof(vicon_position)); - vicon_position.timestamp = timestamp; - - vicon_position.x = pos.x; - vicon_position.y = pos.y; - vicon_position.z = pos.z; - - vicon_position.roll = pos.roll; - vicon_position.pitch = pos.pitch; - vicon_position.yaw = pos.yaw; - - if (_vicon_position_pub <= 0) { - _vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position); - - } else { - orb_publish(ORB_ID(vehicle_vicon_position), _vicon_position_pub, &vicon_position); - } - - } else if (msg->msgid == MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST) { - /* offboard control */ - mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t quad_motors_setpoint; - mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &quad_motors_setpoint); - - if (mavlink_system.sysid < 4) { - struct offboard_control_setpoint_s offboard_control_sp; - memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); - - /* switch to a receiving link mode */ - //TODO why do we need this? - //_mavlink->set_mode(Mavlink::MODE_TX_HEARTBEAT_ONLY); - - /* - * rate control mode - defined by MAVLink - */ - - uint8_t ml_mode = 0; - bool ml_armed = false; - - switch (quad_motors_setpoint.mode) { - case 0: - ml_armed = false; - break; - - case 1: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES; - ml_armed = true; - - break; - - case 2: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; - ml_armed = true; - - break; - - case 3: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY; - break; - - case 4: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION; - break; - } - - offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1] / (float)INT16_MAX; - offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1] / (float)INT16_MAX; - offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1] / (float)INT16_MAX; - offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / (float)UINT16_MAX; - - if (quad_motors_setpoint.thrust[mavlink_system.sysid - 1] == 0) { - ml_armed = false; - } - - offboard_control_sp.armed = ml_armed; - offboard_control_sp.mode = static_cast(ml_mode); - - offboard_control_sp.timestamp = timestamp; - - 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 if (msg->msgid == MAVLINK_MSG_ID_RADIO_STATUS) { - /* telemetry status */ - mavlink_radio_status_t rstatus; - mavlink_msg_radio_status_decode(msg, &rstatus); - - struct telemetry_status_s tstatus; - memset(&tstatus, 0, sizeof(tstatus)); - - tstatus.timestamp = timestamp; - tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO; - tstatus.rssi = rstatus.rssi; - tstatus.remote_rssi = rstatus.remrssi; - tstatus.txbuf = rstatus.txbuf; - tstatus.noise = rstatus.noise; - tstatus.remote_noise = rstatus.remnoise; - tstatus.rxerrors = rstatus.rxerrors; - tstatus.fixed = rstatus.fixed; - - if (_telemetry_status_pub <= 0) { - _telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus); - - } else { - orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus); - } - - } else if (msg->msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) { - /* manual control */ - mavlink_manual_control_t man; - mavlink_msg_manual_control_decode(msg, &man); - - /* rc channels */ - { - struct rc_channels_s rc; - memset(&rc, 0, sizeof(rc)); - - rc.timestamp = timestamp; - rc.chan_count = 4; - - rc.chan[0].scaled = man.x / 1000.0f; - rc.chan[1].scaled = man.y / 1000.0f; - rc.chan[2].scaled = man.r / 1000.0f; - rc.chan[3].scaled = man.z / 1000.0f; - - if (_rc_pub == 0) { - _rc_pub = orb_advertise(ORB_ID(rc_channels), &rc); - - } else { - orb_publish(ORB_ID(rc_channels), _rc_pub, &rc); - } - } - - /* manual control */ - { - struct manual_control_setpoint_s manual; - memset(&manual, 0, sizeof(manual)); - - /* get a copy first, to prevent altering values that are not sent by the mavlink command */ - orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &manual); - - manual.timestamp = timestamp; - manual.roll = man.x / 1000.0f; - manual.pitch = man.y / 1000.0f; - manual.yaw = man.r / 1000.0f; - manual.throttle = man.z / 1000.0f; - - if (_manual_pub == 0) { - _manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual); - - } else { - orb_publish(ORB_ID(manual_control_setpoint), _manual_pub, &manual); - } - } + default: + break; } /* @@ -401,400 +161,703 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) * in the system mode. Either send a set mode * COMMAND_LONG message or a SET_MODE message */ - if (_mavlink->get_hil_enabled()) { - if (msg->msgid == MAVLINK_MSG_ID_HIL_SENSOR) { - /* HIL sensors */ - mavlink_hil_sensor_t imu; - mavlink_msg_hil_sensor_decode(msg, &imu); + switch (msg->msgid) { + case MAVLINK_MSG_ID_HIL_SENSOR: + handle_message_hil_sensor(msg); + break; - /* airspeed */ - { - struct airspeed_s airspeed; - memset(&airspeed, 0, sizeof(airspeed)); + case MAVLINK_MSG_ID_HIL_GPS: + handle_message_hil_gps(msg); + break; - float ias = calc_indicated_airspeed(imu.diff_pressure * 1e2f); - // XXX need to fix this - float tas = ias; + case MAVLINK_MSG_ID_HIL_STATE_QUATERNION: + handle_message_hil_state_quaternion(msg); + break; - airspeed.timestamp = timestamp; - airspeed.indicated_airspeed_m_s = ias; - airspeed.true_airspeed_m_s = tas; + default: + break; + } + } +} - if (_airspeed_pub < 0) { - _airspeed_pub = orb_advertise(ORB_ID(airspeed), &airspeed); +void +MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) +{ + /* command */ + mavlink_command_long_t cmd_mavlink; + mavlink_msg_command_long_decode(msg, &cmd_mavlink); - } else { - orb_publish(ORB_ID(airspeed), _airspeed_pub, &airspeed); - } - } + if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid) + || (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) { + //check for MAVLINK terminate command + if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) { + /* This is the link shutdown command, terminate mavlink */ + warnx("terminated by remote command"); + fflush(stdout); + usleep(50000); - /* gyro */ - { - struct gyro_report gyro; - memset(&gyro, 0, sizeof(gyro)); + /* terminate other threads and this thread */ + _mavlink->_task_should_exit = true; - gyro.timestamp = timestamp; - gyro.x_raw = imu.xgyro * 1000.0f; - gyro.y_raw = imu.ygyro * 1000.0f; - gyro.z_raw = imu.zgyro * 1000.0f; - gyro.x = imu.xgyro; - gyro.y = imu.ygyro; - gyro.z = imu.zgyro; - gyro.temperature = imu.temperature; + } else { + struct vehicle_command_s vcmd; + memset(&vcmd, 0, sizeof(vcmd)); - if (_gyro_pub < 0) { - _gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro); + /* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ + vcmd.param1 = cmd_mavlink.param1; + vcmd.param2 = cmd_mavlink.param2; + vcmd.param3 = cmd_mavlink.param3; + vcmd.param4 = cmd_mavlink.param4; + vcmd.param5 = cmd_mavlink.param5; + vcmd.param6 = cmd_mavlink.param6; + vcmd.param7 = cmd_mavlink.param7; + // XXX do proper translation + vcmd.command = (enum VEHICLE_CMD)cmd_mavlink.command; + vcmd.target_system = cmd_mavlink.target_system; + vcmd.target_component = cmd_mavlink.target_component; + vcmd.source_system = msg->sysid; + vcmd.source_component = msg->compid; + vcmd.confirmation = cmd_mavlink.confirmation; - } else { - orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &gyro); - } - } - - /* accelerometer */ - { - struct accel_report accel; - memset(&accel, 0, sizeof(accel)); - - accel.timestamp = timestamp; - accel.x_raw = imu.xacc / mg2ms2; - accel.y_raw = imu.yacc / mg2ms2; - accel.z_raw = imu.zacc / mg2ms2; - accel.x = imu.xacc; - accel.y = imu.yacc; - accel.z = imu.zacc; - accel.temperature = imu.temperature; - - if (_accel_pub < 0) { - _accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel); - - } else { - orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel); - } - } - - /* magnetometer */ - { - struct mag_report mag; - memset(&mag, 0, sizeof(mag)); - - mag.timestamp = timestamp; - mag.x_raw = imu.xmag * 1000.0f; - mag.y_raw = imu.ymag * 1000.0f; - mag.z_raw = imu.zmag * 1000.0f; - mag.x = imu.xmag; - mag.y = imu.ymag; - mag.z = imu.zmag; - - if (_mag_pub < 0) { - _mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag); - - } else { - orb_publish(ORB_ID(sensor_mag), _mag_pub, &mag); - } - } - - /* baro */ - { - struct baro_report baro; - memset(&baro, 0, sizeof(baro)); - - baro.timestamp = timestamp; - baro.pressure = imu.abs_pressure; - baro.altitude = imu.pressure_alt; - baro.temperature = imu.temperature; - - if (_baro_pub < 0) { - _baro_pub = orb_advertise(ORB_ID(sensor_baro), &baro); - - } else { - orb_publish(ORB_ID(sensor_baro), _baro_pub, &baro); - } - } - - /* sensor combined */ - { - struct sensor_combined_s hil_sensors; - memset(&hil_sensors, 0, sizeof(hil_sensors)); - - hil_sensors.timestamp = timestamp; - - hil_sensors.gyro_raw[0] = imu.xgyro * 1000.0f; - hil_sensors.gyro_raw[1] = imu.ygyro * 1000.0f; - hil_sensors.gyro_raw[2] = imu.zgyro * 1000.0f; - hil_sensors.gyro_rad_s[0] = imu.xgyro; - hil_sensors.gyro_rad_s[1] = imu.ygyro; - hil_sensors.gyro_rad_s[2] = imu.zgyro; - hil_sensors.gyro_counter = _hil_counter; - - hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2; - hil_sensors.accelerometer_raw[1] = imu.yacc / mg2ms2; - hil_sensors.accelerometer_raw[2] = imu.zacc / mg2ms2; - hil_sensors.accelerometer_m_s2[0] = imu.xacc; - hil_sensors.accelerometer_m_s2[1] = imu.yacc; - hil_sensors.accelerometer_m_s2[2] = imu.zacc; - hil_sensors.accelerometer_mode = 0; // TODO what is this? - hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16 - hil_sensors.accelerometer_counter = _hil_counter; - - hil_sensors.adc_voltage_v[0] = 0.0f; - hil_sensors.adc_voltage_v[1] = 0.0f; - hil_sensors.adc_voltage_v[2] = 0.0f; - - hil_sensors.magnetometer_raw[0] = imu.xmag * 1000.0f; - hil_sensors.magnetometer_raw[1] = imu.ymag * 1000.0f; - hil_sensors.magnetometer_raw[2] = imu.zmag * 1000.0f; - hil_sensors.magnetometer_ga[0] = imu.xmag; - hil_sensors.magnetometer_ga[1] = imu.ymag; - hil_sensors.magnetometer_ga[2] = imu.zmag; - hil_sensors.magnetometer_range_ga = 32.7f; // int16 - hil_sensors.magnetometer_mode = 0; // TODO what is this - hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f; - hil_sensors.magnetometer_counter = _hil_counter; - - hil_sensors.baro_pres_mbar = imu.abs_pressure; - hil_sensors.baro_alt_meter = imu.pressure_alt; - hil_sensors.baro_temp_celcius = imu.temperature; - hil_sensors.baro_counter = _hil_counter; - - hil_sensors.differential_pressure_pa = imu.diff_pressure * 1e2f; //from hPa to Pa - hil_sensors.differential_pressure_counter = _hil_counter; - - /* publish combined sensor topic */ - if (_sensors_pub > 0) { - orb_publish(ORB_ID(sensor_combined), _sensors_pub, &hil_sensors); - - } else { - _sensors_pub = orb_advertise(ORB_ID(sensor_combined), &hil_sensors); - } - } - - /* battery status */ - { - struct battery_status_s hil_battery_status; - memset(&hil_battery_status, 0, sizeof(hil_battery_status)); - - hil_battery_status.timestamp = timestamp; - hil_battery_status.voltage_v = 11.1f; - hil_battery_status.current_a = 10.0f; - - if (_battery_pub > 0) { - orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); - - } else { - _baro_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); - } - } - - /* increment counters */ - _hil_counter++; - _hil_frames++; - - /* print HIL sensors rate */ - if ((timestamp - _old_timestamp) > 10000000) { - printf("receiving HIL sensors at %d hz\n", _hil_frames / 10); - _old_timestamp = timestamp; - _hil_frames = 0; - } - - } else if (msg->msgid == MAVLINK_MSG_ID_HIL_GPS) { - /* HIL GPS */ - mavlink_hil_gps_t gps; - mavlink_msg_hil_gps_decode(msg, &gps); - - struct vehicle_gps_position_s hil_gps; - memset(&hil_gps, 0, sizeof(hil_gps)); - - hil_gps.timestamp_time = timestamp; - hil_gps.time_gps_usec = gps.time_usec; - - hil_gps.timestamp_position = timestamp; - hil_gps.lat = gps.lat; - hil_gps.lon = gps.lon; - hil_gps.alt = gps.alt; - hil_gps.eph_m = (float)gps.eph * 1e-2f; // from cm to m - hil_gps.epv_m = (float)gps.epv * 1e-2f; // from cm to m - - hil_gps.timestamp_variance = timestamp; - hil_gps.s_variance_m_s = 5.0f; - hil_gps.p_variance_m = hil_gps.eph_m * hil_gps.eph_m; - - hil_gps.timestamp_velocity = timestamp; - hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s - hil_gps.vel_n_m_s = gps.vn * 1e-2f; // from cm to m - hil_gps.vel_e_m_s = gps.ve * 1e-2f; // from cm to m - hil_gps.vel_d_m_s = gps.vd * 1e-2f; // from cm to m - hil_gps.vel_ned_valid = true; - hil_gps.cog_rad = _wrap_pi(gps.cog * M_DEG_TO_RAD_F * 1e-2f); - - hil_gps.timestamp_satellites = timestamp; - hil_gps.fix_type = gps.fix_type; - hil_gps.satellites_visible = gps.satellites_visible; - - if (_gps_pub > 0) { - orb_publish(ORB_ID(vehicle_gps_position), _gps_pub, &hil_gps); + /* check if topic is advertised */ + if (_cmd_pub <= 0) { + _cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); } else { - _gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps); + /* publish */ + orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd); } + } + } +} - } else if (msg->msgid == MAVLINK_MSG_ID_HIL_STATE_QUATERNION) { - /* HIL state quaternion */ - mavlink_hil_state_quaternion_t hil_state; - mavlink_msg_hil_state_quaternion_decode(msg, &hil_state); +void +MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg) +{ + /* optical flow */ + mavlink_optical_flow_t flow; + mavlink_msg_optical_flow_decode(msg, &flow); - /* airspeed */ - { - struct airspeed_s airspeed; - memset(&airspeed, 0, sizeof(airspeed)); + struct optical_flow_s f; + memset(&f, 0, sizeof(f)); - airspeed.timestamp = timestamp; - airspeed.indicated_airspeed_m_s = hil_state.ind_airspeed * 1e-2f; - airspeed.true_airspeed_m_s = hil_state.true_airspeed * 1e-2f; + f.timestamp = hrt_absolute_time(); + 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 (_airspeed_pub < 0) { - _airspeed_pub = orb_advertise(ORB_ID(airspeed), &airspeed); + if (_flow_pub <= 0) { + _flow_pub = orb_advertise(ORB_ID(optical_flow), &f); - } else { - orb_publish(ORB_ID(airspeed), _airspeed_pub, &airspeed); - } - } + } else { + orb_publish(ORB_ID(optical_flow), _flow_pub, &f); + } +} - /* attitude */ - struct vehicle_attitude_s hil_attitude; - { - memset(&hil_attitude, 0, sizeof(hil_attitude)); - math::Quaternion q(hil_state.attitude_quaternion); - math::Matrix<3, 3> C_nb = q.to_dcm(); - math::Vector<3> euler = C_nb.to_euler(); +void +MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg) +{ + mavlink_set_mode_t new_mode; + mavlink_msg_set_mode_decode(msg, &new_mode); - hil_attitude.timestamp = timestamp; - memcpy(hil_attitude.R, C_nb.data, sizeof(hil_attitude.R)); - hil_attitude.R_valid = true; + struct vehicle_command_s vcmd; + memset(&vcmd, 0, sizeof(vcmd)); - hil_attitude.q[0] = q(0); - hil_attitude.q[1] = q(1); - hil_attitude.q[2] = q(2); - hil_attitude.q[3] = q(3); - hil_attitude.q_valid = true; + union px4_custom_mode custom_mode; + custom_mode.data = new_mode.custom_mode; + /* copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ + vcmd.param1 = new_mode.base_mode; + vcmd.param2 = custom_mode.main_mode; + vcmd.param3 = 0; + vcmd.param4 = 0; + vcmd.param5 = 0; + vcmd.param6 = 0; + vcmd.param7 = 0; + vcmd.command = VEHICLE_CMD_DO_SET_MODE; + vcmd.target_system = new_mode.target_system; + vcmd.target_component = MAV_COMP_ID_ALL; + vcmd.source_system = msg->sysid; + vcmd.source_component = msg->compid; + vcmd.confirmation = 1; - hil_attitude.roll = euler(0); - hil_attitude.pitch = euler(1); - hil_attitude.yaw = euler(2); - hil_attitude.rollspeed = hil_state.rollspeed; - hil_attitude.pitchspeed = hil_state.pitchspeed; - hil_attitude.yawspeed = hil_state.yawspeed; + if (_cmd_pub <= 0) { + _cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); - if (_attitude_pub > 0) { - orb_publish(ORB_ID(vehicle_attitude), _attitude_pub, &hil_attitude); + } else { + orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd); + } +} - } else { - _attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude); - } - } +void +MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg) +{ + mavlink_vicon_position_estimate_t pos; + mavlink_msg_vicon_position_estimate_decode(msg, &pos); - /* global position */ - { - struct vehicle_global_position_s hil_global_pos; - memset(&hil_global_pos, 0, sizeof(hil_global_pos)); + struct vehicle_vicon_position_s vicon_position; + memset(&vicon_position, 0, sizeof(vicon_position)); - hil_global_pos.timestamp = timestamp; - hil_global_pos.global_valid = true; - hil_global_pos.lat = hil_state.lat; - hil_global_pos.lon = hil_state.lon; - hil_global_pos.alt = hil_state.alt / 1000.0f; - hil_global_pos.vel_n = hil_state.vx / 100.0f; - hil_global_pos.vel_e = hil_state.vy / 100.0f; - hil_global_pos.vel_d = hil_state.vz / 100.0f; - hil_global_pos.yaw = hil_attitude.yaw; + vicon_position.timestamp = hrt_absolute_time(); + vicon_position.x = pos.x; + vicon_position.y = pos.y; + vicon_position.z = pos.z; + vicon_position.roll = pos.roll; + vicon_position.pitch = pos.pitch; + vicon_position.yaw = pos.yaw; - if (_global_pos_pub > 0) { - orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &hil_global_pos); + if (_vicon_position_pub <= 0) { + _vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position); - } else { - _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); - } - } + } else { + orb_publish(ORB_ID(vehicle_vicon_position), _vicon_position_pub, &vicon_position); + } +} - /* local position */ - { - if (!_hil_local_proj_inited) { - _hil_local_proj_inited = true; - _hil_local_alt0 = hil_state.alt / 1000.0f; - map_projection_init(hil_state.lat, hil_state.lon); - hil_local_pos.ref_timestamp = timestamp; - hil_local_pos.ref_lat = hil_state.lat; - hil_local_pos.ref_lon = hil_state.lon; - hil_local_pos.ref_alt = _hil_local_alt0; - } +void +MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg) +{ + mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t quad_motors_setpoint; + mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &quad_motors_setpoint); - float x; - float y; - map_projection_project(hil_state.lat * 1e-7, hil_state.lon * 1e-7, &x, &y); - hil_local_pos.timestamp = timestamp; - hil_local_pos.xy_valid = true; - hil_local_pos.z_valid = true; - hil_local_pos.v_xy_valid = true; - hil_local_pos.v_z_valid = true; - hil_local_pos.x = x; - hil_local_pos.y = y; - hil_local_pos.z = _hil_local_alt0 - hil_state.alt / 1000.0f; - hil_local_pos.vx = hil_state.vx / 100.0f; - hil_local_pos.vy = hil_state.vy / 100.0f; - hil_local_pos.vz = hil_state.vz / 100.0f; - hil_local_pos.yaw = hil_attitude.yaw; - hil_local_pos.xy_global = true; - hil_local_pos.z_global = true; + if (mavlink_system.sysid < 4) { + struct offboard_control_setpoint_s offboard_control_sp; + memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); - bool landed = (float)(hil_state.alt) / 1000.0f < (_hil_local_alt0 + 0.1f); // XXX improve? - hil_local_pos.landed = landed; + uint8_t ml_mode = 0; + bool ml_armed = false; - if (_local_pos_pub > 0) { - orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &hil_local_pos); + switch (quad_motors_setpoint.mode) { + case 0: + ml_armed = false; + break; - } else { - _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos); - } - } + case 1: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES; + ml_armed = true; - /* accelerometer */ - { - struct accel_report accel; - memset(&accel, 0, sizeof(accel)); + break; - accel.timestamp = timestamp; - accel.x_raw = hil_state.xacc / 9.81f * 1e3f; - accel.y_raw = hil_state.yacc / 9.81f * 1e3f; - accel.z_raw = hil_state.zacc / 9.81f * 1e3f; - accel.x = hil_state.xacc; - accel.y = hil_state.yacc; - accel.z = hil_state.zacc; - accel.temperature = 25.0f; + case 2: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; + ml_armed = true; - if (_accel_pub < 0) { - _accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel); + break; - } else { - orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel); - } - } + case 3: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY; + break; - /* battery status */ - { - struct battery_status_s hil_battery_status; - memset(&hil_battery_status, 0, sizeof(hil_battery_status)); + case 4: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION; + break; + } - hil_battery_status.timestamp = timestamp; - hil_battery_status.voltage_v = 11.1f; - hil_battery_status.current_a = 10.0f; + offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1] / (float)INT16_MAX; + offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1] / (float)INT16_MAX; + offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1] / (float)INT16_MAX; + offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / (float)UINT16_MAX; - if (_battery_pub > 0) { - orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); + if (quad_motors_setpoint.thrust[mavlink_system.sysid - 1] == 0) { + ml_armed = false; + } - } else { - _battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); - } - } + offboard_control_sp.armed = ml_armed; + offboard_control_sp.mode = static_cast(ml_mode); + + 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) +{ + mavlink_radio_status_t rstatus; + mavlink_msg_radio_status_decode(msg, &rstatus); + + struct telemetry_status_s tstatus; + memset(&tstatus, 0, sizeof(tstatus)); + + tstatus.timestamp = hrt_absolute_time(); + tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO; + tstatus.rssi = rstatus.rssi; + tstatus.remote_rssi = rstatus.remrssi; + tstatus.txbuf = rstatus.txbuf; + tstatus.noise = rstatus.noise; + tstatus.remote_noise = rstatus.remnoise; + tstatus.rxerrors = rstatus.rxerrors; + tstatus.fixed = rstatus.fixed; + + if (_telemetry_status_pub <= 0) { + _telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus); + + } else { + orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus); + } +} + +void +MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) +{ + mavlink_manual_control_t man; + mavlink_msg_manual_control_decode(msg, &man); + + /* rc channels */ + { + struct rc_channels_s rc; + memset(&rc, 0, sizeof(rc)); + + rc.timestamp = hrt_absolute_time(); + rc.chan_count = 4; + + rc.chan[0].scaled = man.x / 1000.0f; + rc.chan[1].scaled = man.y / 1000.0f; + rc.chan[2].scaled = man.r / 1000.0f; + rc.chan[3].scaled = man.z / 1000.0f; + + if (_rc_pub == 0) { + _rc_pub = orb_advertise(ORB_ID(rc_channels), &rc); + + } else { + orb_publish(ORB_ID(rc_channels), _rc_pub, &rc); + } + } + + /* manual control */ + { + struct manual_control_setpoint_s manual; + memset(&manual, 0, sizeof(manual)); + + /* get a copy first, to prevent altering values that are not sent by the mavlink command */ + orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &manual); + + manual.timestamp = hrt_absolute_time(); + manual.roll = man.x / 1000.0f; + manual.pitch = man.y / 1000.0f; + manual.yaw = man.r / 1000.0f; + manual.throttle = man.z / 1000.0f; + + if (_manual_pub == 0) { + _manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual); + + } else { + orb_publish(ORB_ID(manual_control_setpoint), _manual_pub, &manual); + } + } +} + +void +MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) +{ + mavlink_hil_sensor_t imu; + mavlink_msg_hil_sensor_decode(msg, &imu); + + uint64_t timestamp = hrt_absolute_time(); + + /* airspeed */ + { + struct airspeed_s airspeed; + memset(&airspeed, 0, sizeof(airspeed)); + + float ias = calc_indicated_airspeed(imu.diff_pressure * 1e2f); + // XXX need to fix this + float tas = ias; + + airspeed.timestamp = timestamp; + airspeed.indicated_airspeed_m_s = ias; + airspeed.true_airspeed_m_s = tas; + + if (_airspeed_pub < 0) { + _airspeed_pub = orb_advertise(ORB_ID(airspeed), &airspeed); + + } else { + orb_publish(ORB_ID(airspeed), _airspeed_pub, &airspeed); + } + } + + /* gyro */ + { + struct gyro_report gyro; + memset(&gyro, 0, sizeof(gyro)); + + gyro.timestamp = timestamp; + gyro.x_raw = imu.xgyro * 1000.0f; + gyro.y_raw = imu.ygyro * 1000.0f; + gyro.z_raw = imu.zgyro * 1000.0f; + gyro.x = imu.xgyro; + gyro.y = imu.ygyro; + gyro.z = imu.zgyro; + gyro.temperature = imu.temperature; + + if (_gyro_pub < 0) { + _gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro); + + } else { + orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &gyro); + } + } + + /* accelerometer */ + { + struct accel_report accel; + memset(&accel, 0, sizeof(accel)); + + accel.timestamp = timestamp; + accel.x_raw = imu.xacc / mg2ms2; + accel.y_raw = imu.yacc / mg2ms2; + accel.z_raw = imu.zacc / mg2ms2; + accel.x = imu.xacc; + accel.y = imu.yacc; + accel.z = imu.zacc; + accel.temperature = imu.temperature; + + if (_accel_pub < 0) { + _accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel); + + } else { + orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel); + } + } + + /* magnetometer */ + { + struct mag_report mag; + memset(&mag, 0, sizeof(mag)); + + mag.timestamp = timestamp; + mag.x_raw = imu.xmag * 1000.0f; + mag.y_raw = imu.ymag * 1000.0f; + mag.z_raw = imu.zmag * 1000.0f; + mag.x = imu.xmag; + mag.y = imu.ymag; + mag.z = imu.zmag; + + if (_mag_pub < 0) { + _mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag); + + } else { + orb_publish(ORB_ID(sensor_mag), _mag_pub, &mag); + } + } + + /* baro */ + { + struct baro_report baro; + memset(&baro, 0, sizeof(baro)); + + baro.timestamp = timestamp; + baro.pressure = imu.abs_pressure; + baro.altitude = imu.pressure_alt; + baro.temperature = imu.temperature; + + if (_baro_pub < 0) { + _baro_pub = orb_advertise(ORB_ID(sensor_baro), &baro); + + } else { + orb_publish(ORB_ID(sensor_baro), _baro_pub, &baro); + } + } + + /* sensor combined */ + { + struct sensor_combined_s hil_sensors; + memset(&hil_sensors, 0, sizeof(hil_sensors)); + + hil_sensors.timestamp = timestamp; + + hil_sensors.gyro_raw[0] = imu.xgyro * 1000.0f; + hil_sensors.gyro_raw[1] = imu.ygyro * 1000.0f; + hil_sensors.gyro_raw[2] = imu.zgyro * 1000.0f; + hil_sensors.gyro_rad_s[0] = imu.xgyro; + hil_sensors.gyro_rad_s[1] = imu.ygyro; + hil_sensors.gyro_rad_s[2] = imu.zgyro; + hil_sensors.gyro_counter = _hil_counter; + + hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2; + hil_sensors.accelerometer_raw[1] = imu.yacc / mg2ms2; + hil_sensors.accelerometer_raw[2] = imu.zacc / mg2ms2; + hil_sensors.accelerometer_m_s2[0] = imu.xacc; + hil_sensors.accelerometer_m_s2[1] = imu.yacc; + hil_sensors.accelerometer_m_s2[2] = imu.zacc; + hil_sensors.accelerometer_mode = 0; // TODO what is this? + hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16 + hil_sensors.accelerometer_counter = _hil_counter; + + hil_sensors.adc_voltage_v[0] = 0.0f; + hil_sensors.adc_voltage_v[1] = 0.0f; + hil_sensors.adc_voltage_v[2] = 0.0f; + + hil_sensors.magnetometer_raw[0] = imu.xmag * 1000.0f; + hil_sensors.magnetometer_raw[1] = imu.ymag * 1000.0f; + hil_sensors.magnetometer_raw[2] = imu.zmag * 1000.0f; + hil_sensors.magnetometer_ga[0] = imu.xmag; + hil_sensors.magnetometer_ga[1] = imu.ymag; + hil_sensors.magnetometer_ga[2] = imu.zmag; + hil_sensors.magnetometer_range_ga = 32.7f; // int16 + hil_sensors.magnetometer_mode = 0; // TODO what is this + hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f; + hil_sensors.magnetometer_counter = _hil_counter; + + hil_sensors.baro_pres_mbar = imu.abs_pressure; + hil_sensors.baro_alt_meter = imu.pressure_alt; + hil_sensors.baro_temp_celcius = imu.temperature; + hil_sensors.baro_counter = _hil_counter; + + hil_sensors.differential_pressure_pa = imu.diff_pressure * 1e2f; //from hPa to Pa + hil_sensors.differential_pressure_counter = _hil_counter; + + /* publish combined sensor topic */ + if (_sensors_pub > 0) { + orb_publish(ORB_ID(sensor_combined), _sensors_pub, &hil_sensors); + + } else { + _sensors_pub = orb_advertise(ORB_ID(sensor_combined), &hil_sensors); + } + } + + /* battery status */ + { + struct battery_status_s hil_battery_status; + memset(&hil_battery_status, 0, sizeof(hil_battery_status)); + + hil_battery_status.timestamp = timestamp; + hil_battery_status.voltage_v = 11.1f; + hil_battery_status.current_a = 10.0f; + + if (_battery_pub > 0) { + orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); + + } else { + _baro_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); + } + } + + /* increment counters */ + _hil_counter++; + _hil_frames++; + + /* print HIL sensors rate */ + if ((timestamp - _old_timestamp) > 10000000) { + printf("receiving HIL sensors at %d hz\n", _hil_frames / 10); + _old_timestamp = timestamp; + _hil_frames = 0; + } +} + +void +MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg) +{ + mavlink_hil_gps_t gps; + mavlink_msg_hil_gps_decode(msg, &gps); + + uint64_t timestamp = hrt_absolute_time(); + + struct vehicle_gps_position_s hil_gps; + memset(&hil_gps, 0, sizeof(hil_gps)); + + hil_gps.timestamp_time = timestamp; + hil_gps.time_gps_usec = gps.time_usec; + + hil_gps.timestamp_position = timestamp; + hil_gps.lat = gps.lat; + hil_gps.lon = gps.lon; + hil_gps.alt = gps.alt; + hil_gps.eph_m = (float)gps.eph * 1e-2f; // from cm to m + hil_gps.epv_m = (float)gps.epv * 1e-2f; // from cm to m + + hil_gps.timestamp_variance = timestamp; + hil_gps.s_variance_m_s = 5.0f; + hil_gps.p_variance_m = hil_gps.eph_m * hil_gps.eph_m; + + hil_gps.timestamp_velocity = timestamp; + hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s + hil_gps.vel_n_m_s = gps.vn * 1e-2f; // from cm to m + hil_gps.vel_e_m_s = gps.ve * 1e-2f; // from cm to m + hil_gps.vel_d_m_s = gps.vd * 1e-2f; // from cm to m + hil_gps.vel_ned_valid = true; + hil_gps.cog_rad = _wrap_pi(gps.cog * M_DEG_TO_RAD_F * 1e-2f); + + hil_gps.timestamp_satellites = timestamp; + hil_gps.fix_type = gps.fix_type; + hil_gps.satellites_visible = gps.satellites_visible; + + if (_gps_pub > 0) { + orb_publish(ORB_ID(vehicle_gps_position), _gps_pub, &hil_gps); + + } else { + _gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps); + } +} + +void +MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) +{ + mavlink_hil_state_quaternion_t hil_state; + mavlink_msg_hil_state_quaternion_decode(msg, &hil_state); + + uint64_t timestamp = hrt_absolute_time(); + + /* airspeed */ + { + struct airspeed_s airspeed; + memset(&airspeed, 0, sizeof(airspeed)); + + airspeed.timestamp = timestamp; + airspeed.indicated_airspeed_m_s = hil_state.ind_airspeed * 1e-2f; + airspeed.true_airspeed_m_s = hil_state.true_airspeed * 1e-2f; + + if (_airspeed_pub < 0) { + _airspeed_pub = orb_advertise(ORB_ID(airspeed), &airspeed); + + } else { + orb_publish(ORB_ID(airspeed), _airspeed_pub, &airspeed); + } + } + + /* attitude */ + struct vehicle_attitude_s hil_attitude; + { + memset(&hil_attitude, 0, sizeof(hil_attitude)); + math::Quaternion q(hil_state.attitude_quaternion); + math::Matrix<3, 3> C_nb = q.to_dcm(); + math::Vector<3> euler = C_nb.to_euler(); + + hil_attitude.timestamp = timestamp; + memcpy(hil_attitude.R, C_nb.data, sizeof(hil_attitude.R)); + hil_attitude.R_valid = true; + + hil_attitude.q[0] = q(0); + hil_attitude.q[1] = q(1); + hil_attitude.q[2] = q(2); + hil_attitude.q[3] = q(3); + hil_attitude.q_valid = true; + + hil_attitude.roll = euler(0); + hil_attitude.pitch = euler(1); + hil_attitude.yaw = euler(2); + hil_attitude.rollspeed = hil_state.rollspeed; + hil_attitude.pitchspeed = hil_state.pitchspeed; + hil_attitude.yawspeed = hil_state.yawspeed; + + if (_attitude_pub > 0) { + orb_publish(ORB_ID(vehicle_attitude), _attitude_pub, &hil_attitude); + + } else { + _attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude); + } + } + + /* global position */ + { + struct vehicle_global_position_s hil_global_pos; + memset(&hil_global_pos, 0, sizeof(hil_global_pos)); + + hil_global_pos.timestamp = timestamp; + hil_global_pos.global_valid = true; + hil_global_pos.lat = hil_state.lat; + hil_global_pos.lon = hil_state.lon; + hil_global_pos.alt = hil_state.alt / 1000.0f; + hil_global_pos.vel_n = hil_state.vx / 100.0f; + hil_global_pos.vel_e = hil_state.vy / 100.0f; + hil_global_pos.vel_d = hil_state.vz / 100.0f; + hil_global_pos.yaw = hil_attitude.yaw; + + if (_global_pos_pub > 0) { + orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &hil_global_pos); + + } else { + _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); + } + } + + /* local position */ + { + if (!_hil_local_proj_inited) { + _hil_local_proj_inited = true; + _hil_local_alt0 = hil_state.alt / 1000.0f; + map_projection_init(hil_state.lat, hil_state.lon); + hil_local_pos.ref_timestamp = timestamp; + hil_local_pos.ref_lat = hil_state.lat; + hil_local_pos.ref_lon = hil_state.lon; + hil_local_pos.ref_alt = _hil_local_alt0; + } + + float x; + float y; + map_projection_project(hil_state.lat * 1e-7, hil_state.lon * 1e-7, &x, &y); + hil_local_pos.timestamp = timestamp; + hil_local_pos.xy_valid = true; + hil_local_pos.z_valid = true; + hil_local_pos.v_xy_valid = true; + hil_local_pos.v_z_valid = true; + hil_local_pos.x = x; + hil_local_pos.y = y; + hil_local_pos.z = _hil_local_alt0 - hil_state.alt / 1000.0f; + hil_local_pos.vx = hil_state.vx / 100.0f; + hil_local_pos.vy = hil_state.vy / 100.0f; + hil_local_pos.vz = hil_state.vz / 100.0f; + hil_local_pos.yaw = hil_attitude.yaw; + hil_local_pos.xy_global = true; + hil_local_pos.z_global = true; + + bool landed = (float)(hil_state.alt) / 1000.0f < (_hil_local_alt0 + 0.1f); // XXX improve? + hil_local_pos.landed = landed; + + if (_local_pos_pub > 0) { + orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &hil_local_pos); + + } else { + _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos); + } + } + + /* accelerometer */ + { + struct accel_report accel; + memset(&accel, 0, sizeof(accel)); + + accel.timestamp = timestamp; + accel.x_raw = hil_state.xacc / 9.81f * 1e3f; + accel.y_raw = hil_state.yacc / 9.81f * 1e3f; + accel.z_raw = hil_state.zacc / 9.81f * 1e3f; + accel.x = hil_state.xacc; + accel.y = hil_state.yacc; + accel.z = hil_state.zacc; + accel.temperature = 25.0f; + + if (_accel_pub < 0) { + _accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel); + + } else { + orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel); + } + } + + /* battery status */ + { + struct battery_status_s hil_battery_status; + memset(&hil_battery_status, 0, sizeof(hil_battery_status)); + + hil_battery_status.timestamp = timestamp; + hil_battery_status.voltage_v = 11.1f; + hil_battery_status.current_a = 10.0f; + + if (_battery_pub > 0) { + orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); + + } else { + _battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); } } } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index b97919e9da..beaae20587 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -105,6 +105,17 @@ private: Mavlink *_mavlink; void handle_message(mavlink_message_t *msg); + void handle_message_command_long(mavlink_message_t *msg); + void handle_message_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_quad_swarm_roll_pitch_yaw_thrust(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_hil_sensor(mavlink_message_t *msg); + void handle_message_hil_gps(mavlink_message_t *msg); + void handle_message_hil_state_quaternion(mavlink_message_t *msg); + void *receive_thread(void *arg); mavlink_status_t status; From e1742eea7d951aa704665c2b5db4d1c6788aafa0 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 4 Mar 2014 13:03:17 +0400 Subject: [PATCH 127/381] mavlink: code style fixed --- src/modules/mavlink/mavlink_messages.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 7475160d5e..4d83afe825 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1092,12 +1092,12 @@ protected: flow_sub->update(t); mavlink_msg_optical_flow_send(_channel, - flow->timestamp, - flow->sensor_id, - flow->flow_raw_x, flow->flow_raw_y, - flow->flow_comp_x_m, flow->flow_comp_y_m, - flow->quality, - flow->ground_distance_m); + flow->timestamp, + flow->sensor_id, + flow->flow_raw_x, flow->flow_raw_y, + flow->flow_comp_x_m, flow->flow_comp_y_m, + flow->quality, + flow->ground_distance_m); } }; From 046def571d21856f24f6afc805450c003858afea Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 4 Mar 2014 23:11:24 +0400 Subject: [PATCH 128/381] sensors: publish manual controls on actuator_controls_1 topic instead of actuator_controls_3 --- src/modules/sensors/sensors.cpp | 28 ++++++++++++++-------------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index b176d7417a..068fb3d0bd 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -195,7 +195,7 @@ private: orb_advert_t _sensor_pub; /**< combined sensor data topic */ orb_advert_t _manual_control_pub; /**< manual control signal topic */ - orb_advert_t _actuator_group_3_pub; /**< manual control as actuator topic */ + orb_advert_t _actuator_group_1_pub; /**< manual control as actuator topic */ orb_advert_t _rc_pub; /**< raw r/c control topic */ orb_advert_t _battery_pub; /**< battery status */ orb_advert_t _airspeed_pub; /**< airspeed */ @@ -459,7 +459,7 @@ Sensors::Sensors() : /* publications */ _sensor_pub(-1), _manual_control_pub(-1), - _actuator_group_3_pub(-1), + _actuator_group_1_pub(-1), _rc_pub(-1), _battery_pub(-1), _airspeed_pub(-1), @@ -1279,7 +1279,7 @@ Sensors::rc_poll() return; struct manual_control_setpoint_s manual_control; - struct actuator_controls_s actuator_group_3; + struct actuator_controls_s actuator_group_1; /* initialize to default values */ manual_control.roll = NAN; @@ -1460,14 +1460,14 @@ Sensors::rc_poll() } /* copy from mapped manual control to control group 3 */ - actuator_group_3.control[0] = manual_control.roll; - actuator_group_3.control[1] = manual_control.pitch; - actuator_group_3.control[2] = manual_control.yaw; - actuator_group_3.control[3] = manual_control.throttle; - actuator_group_3.control[4] = manual_control.flaps; - actuator_group_3.control[5] = manual_control.aux1; - actuator_group_3.control[6] = manual_control.aux2; - actuator_group_3.control[7] = manual_control.aux3; + actuator_group_1.control[0] = manual_control.roll; + actuator_group_1.control[1] = manual_control.pitch; + actuator_group_1.control[2] = manual_control.yaw; + actuator_group_1.control[3] = manual_control.throttle; + actuator_group_1.control[4] = manual_control.flaps; + actuator_group_1.control[5] = manual_control.aux1; + actuator_group_1.control[6] = manual_control.aux2; + actuator_group_1.control[7] = manual_control.aux3; /* check if ready for publishing */ if (_rc_pub > 0) { @@ -1487,11 +1487,11 @@ Sensors::rc_poll() } /* check if ready for publishing */ - if (_actuator_group_3_pub > 0) { - orb_publish(ORB_ID(actuator_controls_3), _actuator_group_3_pub, &actuator_group_3); + if (_actuator_group_1_pub > 0) { + orb_publish(ORB_ID(actuator_controls_1), _actuator_group_1_pub, &actuator_group_1); } else { - _actuator_group_3_pub = orb_advertise(ORB_ID(actuator_controls_3), &actuator_group_3); + _actuator_group_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &actuator_group_1); } } From a50957c3cec92167594b7bd82561f72100ccebdd Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 4 Mar 2014 23:13:20 +0400 Subject: [PATCH 129/381] px4io: print all control groups in "px4io status" --- src/drivers/px4io/px4io.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 7c7b3dcb7d..4c48194d4d 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1921,12 +1921,14 @@ PX4IO::print_status() io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE)); #endif printf("debuglevel %u\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG)); - printf("controls"); + for (unsigned group = 0; group < PX4IO_CONTROL_GROUPS; group++) { + printf("controls %u:", group); - for (unsigned i = 0; i < _max_controls; i++) - printf(" %u", io_reg_get(PX4IO_PAGE_CONTROLS, i)); + for (unsigned i = 0; i < _max_controls; i++) + printf(" %u", io_reg_get(PX4IO_PAGE_CONTROLS, group * PX4IO_PROTOCOL_MAX_CONTROL_COUNT + i)); - printf("\n"); + printf("\n"); + } for (unsigned i = 0; i < _max_rc_input; i++) { unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i; From 529013ae1c9298c44e808a8fa97336b242623ad8 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 5 Mar 2014 00:00:17 +0400 Subject: [PATCH 130/381] Reverse: sensors: publish manual controls on actuator_controls_1 topic instead of actuator_controls_3 --- src/modules/sensors/sensors.cpp | 28 ++++++++++++++-------------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 068fb3d0bd..b176d7417a 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -195,7 +195,7 @@ private: orb_advert_t _sensor_pub; /**< combined sensor data topic */ orb_advert_t _manual_control_pub; /**< manual control signal topic */ - orb_advert_t _actuator_group_1_pub; /**< manual control as actuator topic */ + orb_advert_t _actuator_group_3_pub; /**< manual control as actuator topic */ orb_advert_t _rc_pub; /**< raw r/c control topic */ orb_advert_t _battery_pub; /**< battery status */ orb_advert_t _airspeed_pub; /**< airspeed */ @@ -459,7 +459,7 @@ Sensors::Sensors() : /* publications */ _sensor_pub(-1), _manual_control_pub(-1), - _actuator_group_1_pub(-1), + _actuator_group_3_pub(-1), _rc_pub(-1), _battery_pub(-1), _airspeed_pub(-1), @@ -1279,7 +1279,7 @@ Sensors::rc_poll() return; struct manual_control_setpoint_s manual_control; - struct actuator_controls_s actuator_group_1; + struct actuator_controls_s actuator_group_3; /* initialize to default values */ manual_control.roll = NAN; @@ -1460,14 +1460,14 @@ Sensors::rc_poll() } /* copy from mapped manual control to control group 3 */ - actuator_group_1.control[0] = manual_control.roll; - actuator_group_1.control[1] = manual_control.pitch; - actuator_group_1.control[2] = manual_control.yaw; - actuator_group_1.control[3] = manual_control.throttle; - actuator_group_1.control[4] = manual_control.flaps; - actuator_group_1.control[5] = manual_control.aux1; - actuator_group_1.control[6] = manual_control.aux2; - actuator_group_1.control[7] = manual_control.aux3; + actuator_group_3.control[0] = manual_control.roll; + actuator_group_3.control[1] = manual_control.pitch; + actuator_group_3.control[2] = manual_control.yaw; + actuator_group_3.control[3] = manual_control.throttle; + actuator_group_3.control[4] = manual_control.flaps; + actuator_group_3.control[5] = manual_control.aux1; + actuator_group_3.control[6] = manual_control.aux2; + actuator_group_3.control[7] = manual_control.aux3; /* check if ready for publishing */ if (_rc_pub > 0) { @@ -1487,11 +1487,11 @@ Sensors::rc_poll() } /* check if ready for publishing */ - if (_actuator_group_1_pub > 0) { - orb_publish(ORB_ID(actuator_controls_1), _actuator_group_1_pub, &actuator_group_1); + if (_actuator_group_3_pub > 0) { + orb_publish(ORB_ID(actuator_controls_3), _actuator_group_3_pub, &actuator_group_3); } else { - _actuator_group_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &actuator_group_1); + _actuator_group_3_pub = orb_advertise(ORB_ID(actuator_controls_3), &actuator_group_3); } } From af548db7cc0a53b1c07a74342ec528e6247b0fcb Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 5 Mar 2014 00:01:27 +0400 Subject: [PATCH 131/381] px4iofirmware: define 4 actuator control groups --- src/modules/px4iofirmware/mixer.cpp | 2 +- src/modules/px4iofirmware/px4io.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 6a44294619..9558198f35 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -267,7 +267,7 @@ mixer_callback(uintptr_t handle, uint8_t control_index, float &control) { - if (control_group > 3) + if (control_group >= PX4IO_CONTROL_GROUPS) return -1; switch (source) { diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index bb224f3880..54c5663a5b 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -53,7 +53,7 @@ */ #define PX4IO_SERVO_COUNT 8 #define PX4IO_CONTROL_CHANNELS 8 -#define PX4IO_CONTROL_GROUPS 2 +#define PX4IO_CONTROL_GROUPS 4 #define PX4IO_RC_INPUT_CHANNELS 18 #define PX4IO_RC_MAPPED_CONTROL_CHANNELS 8 /**< This is the maximum number of channels mapped/used */ From 149bf4582c67296097106566ec98a85229be9509 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 5 Mar 2014 00:05:17 +0400 Subject: [PATCH 132/381] px4io: hardcode number of control groups in "px4io status" --- 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 4c48194d4d..136a6e29e9 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1921,7 +1921,7 @@ PX4IO::print_status() io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE)); #endif printf("debuglevel %u\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG)); - for (unsigned group = 0; group < PX4IO_CONTROL_GROUPS; group++) { + for (unsigned group = 0; group < 4; group++) { printf("controls %u:", group); for (unsigned i = 0; i < _max_controls; i++) From a8ff126dfe214245d1f5860f2bc9c17b6cdac34b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 5 Mar 2014 20:00:13 +0400 Subject: [PATCH 133/381] px4io: print actuator control registers as int16 instead of uint16 --- 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 136a6e29e9..8e990aa6f0 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1925,7 +1925,7 @@ PX4IO::print_status() printf("controls %u:", group); for (unsigned i = 0; i < _max_controls; i++) - printf(" %u", io_reg_get(PX4IO_PAGE_CONTROLS, group * PX4IO_PROTOCOL_MAX_CONTROL_COUNT + i)); + printf(" %d", (int16_t) io_reg_get(PX4IO_PAGE_CONTROLS, group * PX4IO_PROTOCOL_MAX_CONTROL_COUNT + i)); printf("\n"); } From b40d3e1e18bd8032eee97abdd9a4f68d87303f65 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 5 Mar 2014 22:30:43 +0400 Subject: [PATCH 134/381] Change NFILE_DESCRIPTORS back to 32 --- nuttx-configs/px4fmu-v1/nsh/defconfig | 2 +- nuttx-configs/px4fmu-v2/nsh/defconfig | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index 7aa71f1826..dab62df5fd 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -405,7 +405,7 @@ CONFIG_SIG_SIGWORK=4 CONFIG_MAX_TASKS=32 CONFIG_MAX_TASK_ARGS=10 CONFIG_NPTHREAD_KEYS=4 -CONFIG_NFILE_DESCRIPTORS=64 +CONFIG_NFILE_DESCRIPTORS=32 CONFIG_NFILE_STREAMS=8 CONFIG_NAME_MAX=32 CONFIG_PREALLOC_MQ_MSGS=4 diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index d42b23ab0b..e560c805a0 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -439,7 +439,7 @@ CONFIG_SIG_SIGWORK=4 CONFIG_MAX_TASKS=32 CONFIG_MAX_TASK_ARGS=10 CONFIG_NPTHREAD_KEYS=4 -CONFIG_NFILE_DESCRIPTORS=64 +CONFIG_NFILE_DESCRIPTORS=32 CONFIG_NFILE_STREAMS=8 CONFIG_NAME_MAX=32 CONFIG_PREALLOC_MQ_MSGS=4 From 8425b9bef21e310d1cbd29aad65d34e9dd974d55 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 5 Mar 2014 22:46:22 +0400 Subject: [PATCH 135/381] Increase NFILE_DESCRIPTORS to 36 --- nuttx-configs/px4fmu-v1/nsh/defconfig | 2 +- nuttx-configs/px4fmu-v2/nsh/defconfig | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index dab62df5fd..8cb8736404 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -405,7 +405,7 @@ CONFIG_SIG_SIGWORK=4 CONFIG_MAX_TASKS=32 CONFIG_MAX_TASK_ARGS=10 CONFIG_NPTHREAD_KEYS=4 -CONFIG_NFILE_DESCRIPTORS=32 +CONFIG_NFILE_DESCRIPTORS=36 CONFIG_NFILE_STREAMS=8 CONFIG_NAME_MAX=32 CONFIG_PREALLOC_MQ_MSGS=4 diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index e560c805a0..0fceb2043f 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -439,7 +439,7 @@ CONFIG_SIG_SIGWORK=4 CONFIG_MAX_TASKS=32 CONFIG_MAX_TASK_ARGS=10 CONFIG_NPTHREAD_KEYS=4 -CONFIG_NFILE_DESCRIPTORS=32 +CONFIG_NFILE_DESCRIPTORS=36 CONFIG_NFILE_STREAMS=8 CONFIG_NAME_MAX=32 CONFIG_PREALLOC_MQ_MSGS=4 From 40deb405c1e9232475c4b5e7bda8080f85fec19a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 6 Mar 2014 08:35:14 +0100 Subject: [PATCH 136/381] Modified estimator to fix internal GCC compiler error (hilarious 64bit handling), use float instead of double routines to avoid implicit casts --- src/modules/fw_att_pos_estimator/estimator.cpp | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index c9e809280d..cbbb5d7a0c 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -1617,12 +1617,18 @@ void StoreStates(uint64_t timestamp_ms) // Output the state vector stored at the time that best matches that specified by msec void RecallStates(float (&statesForFusion)[n_states], uint64_t msec) { - int64_t bestTimeDelta = 200; + int bestTimeDelta = 200; unsigned bestStoreIndex = 0; for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++) { - int64_t timeDelta = (int)msec - statetimeStamp[storeIndex]; - if (timeDelta < 0) timeDelta = -timeDelta; + // The time delta can also end up as negative number, + // since we might compare future to past or past to future + // therefore cast to int64. + int timeDelta = (int64_t)msec - (int64_t)statetimeStamp[storeIndex]; + if (timeDelta < 0) { + timeDelta = -timeDelta; + } + if (timeDelta < bestTimeDelta) { bestStoreIndex = storeIndex; @@ -1739,9 +1745,9 @@ void OnGroundCheck() void calcEarthRateNED(Vector3f &omega, float latitude) { //Define Earth rotation vector in the NED navigation frame - omega.x = earthRate*cos(latitude); - omega.y = 0.0; - omega.z = -earthRate*sin(latitude); + omega.x = earthRate*cosf(latitude); + omega.y = 0.0f; + omega.z = -earthRate*sinf(latitude); } void CovarianceInit() From e505f4fae5596b2b53c120a7cb2a03d2d974c83a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 7 Mar 2014 11:17:02 +0400 Subject: [PATCH 137/381] sdlog2: use orb_check() instead of poll() to minimize polling overhead, bugs and compiler warnings fixed --- src/modules/sdlog2/sdlog2.c | 920 +++++++++++++++--------------------- 1 file changed, 374 insertions(+), 546 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 1365d9e705..fa25d74f85 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -159,6 +159,8 @@ static void *logwriter_thread(void *arg); */ __EXPORT int sdlog2_main(int argc, char *argv[]); +static bool copy_if_updated(orb_id_t topic, int handle, void *buffer); + /** * Mainloop of sd log deamon. */ @@ -402,7 +404,7 @@ static void *logwriter_thread(void *arg) int log_fd = open_log_file(); if (log_fd < 0) - return; + return NULL; struct logbuffer_s *logbuf = (struct logbuffer_s *)arg; @@ -483,7 +485,7 @@ static void *logwriter_thread(void *arg) fsync(log_fd); close(log_fd); - return; + return NULL; } void sdlog2_start_log() @@ -628,6 +630,19 @@ int write_parameters(int fd) return written; } +bool copy_if_updated(orb_id_t topic, int handle, void *buffer) +{ + bool updated; + + orb_check(handle, &updated); + + if (updated) { + orb_copy(topic, handle, buffer); + } + + return updated; +} + int sdlog2_thread_main(int argc, char *argv[]) { mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); @@ -636,9 +651,8 @@ int sdlog2_thread_main(int argc, char *argv[]) warnx("failed to open MAVLink log stream, start mavlink app first"); } - /* log buffer size */ + useconds_t sleep_delay = 10000; // default log rate: 100 Hz int log_buffer_size = LOG_BUFFER_SIZE_DEFAULT; - logging_enabled = false; log_on_start = false; log_when_armed = false; @@ -656,12 +670,10 @@ int sdlog2_thread_main(int argc, char *argv[]) case 'r': { unsigned long r = strtoul(optarg, NULL, 10); - if (r == 0) { - sleep_delay = 0; - - } else { - sleep_delay = 1000000 / r; + if (r <= 0) { + r = 1; } + sleep_delay = 1000000 / r; } break; @@ -711,7 +723,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int mkdir_ret = mkdir(log_root, S_IRWXU | S_IRWXG | S_IRWXO); if (mkdir_ret != 0 && errno != EEXIST) { - err("failed creating log root dir: %s", log_root); + err(1, "failed creating log root dir: %s", log_root); } /* copy conversion scripts */ @@ -733,8 +745,10 @@ int sdlog2_thread_main(int argc, char *argv[]) } struct vehicle_status_s buf_status; + struct vehicle_gps_position_s buf_gps_pos; memset(&buf_status, 0, sizeof(buf_status)); + memset(&buf_gps_pos, 0, sizeof(buf_gps_pos)); /* warning! using union here to save memory, elements should be used separately! */ union { @@ -749,7 +763,6 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_local_position_setpoint_s local_pos_sp; struct vehicle_global_position_s global_pos; struct position_setpoint_triplet_s triplet; - struct vehicle_gps_position_s gps_pos; struct vehicle_vicon_position_s vicon_pos; struct optical_flow_s flow; struct rc_channels_s rc; @@ -763,30 +776,6 @@ int sdlog2_thread_main(int argc, char *argv[]) memset(&buf, 0, sizeof(buf)); - struct { - int cmd_sub; - int status_sub; - int sensor_sub; - int att_sub; - int att_sp_sub; - int rates_sp_sub; - int act_outputs_sub; - int act_controls_sub; - int local_pos_sub; - int local_pos_sp_sub; - int global_pos_sub; - int triplet_sub; - int gps_pos_sub; - int vicon_pos_sub; - int flow_sub; - int rc_sub; - int airspeed_sub; - int esc_sub; - int global_vel_sp_sub; - int battery_sub; - int telemetry_sub; - } subs; - /* log message buffer: header + body */ #pragma pack(push, 1) struct { @@ -821,154 +810,51 @@ int sdlog2_thread_main(int argc, char *argv[]) #pragma pack(pop) memset(&log_msg.body, 0, sizeof(log_msg.body)); - /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ - /* number of messages */ - const ssize_t fdsc = 25; - /* Sanity check variable and index */ - ssize_t fdsc_count = 0; - /* file descriptors to wait for */ - struct pollfd fds[fdsc]; + struct { + int cmd_sub; + int status_sub; + int sensor_sub; + int att_sub; + int att_sp_sub; + int rates_sp_sub; + int act_outputs_sub; + int act_controls_sub; + int local_pos_sub; + int local_pos_sp_sub; + int global_pos_sub; + int triplet_sub; + int gps_pos_sub; + int vicon_pos_sub; + int flow_sub; + int rc_sub; + int airspeed_sub; + int esc_sub; + int global_vel_sp_sub; + int battery_sub; + int telemetry_sub; + } subs; - /* --- VEHICLE COMMAND --- */ subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); - fds[fdsc_count].fd = subs.cmd_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- VEHICLE STATUS --- */ subs.status_sub = orb_subscribe(ORB_ID(vehicle_status)); - fds[fdsc_count].fd = subs.status_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- GPS POSITION --- */ subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); - fds[fdsc_count].fd = subs.gps_pos_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- SENSORS COMBINED --- */ subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); - fds[fdsc_count].fd = subs.sensor_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- ATTITUDE --- */ subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - fds[fdsc_count].fd = subs.att_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- ATTITUDE SETPOINT --- */ subs.att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - fds[fdsc_count].fd = subs.att_sp_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- RATES SETPOINT --- */ subs.rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); - fds[fdsc_count].fd = subs.rates_sp_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- ACTUATOR OUTPUTS --- */ subs.act_outputs_sub = orb_subscribe(ORB_ID_VEHICLE_CONTROLS); - fds[fdsc_count].fd = subs.act_outputs_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- ACTUATOR CONTROL --- */ subs.act_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); - fds[fdsc_count].fd = subs.act_controls_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- LOCAL POSITION --- */ subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); - fds[fdsc_count].fd = subs.local_pos_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- LOCAL POSITION SETPOINT --- */ subs.local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); - fds[fdsc_count].fd = subs.local_pos_sp_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- GLOBAL POSITION --- */ subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - fds[fdsc_count].fd = subs.global_pos_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- GLOBAL POSITION SETPOINT--- */ subs.triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); - fds[fdsc_count].fd = subs.triplet_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- VICON POSITION --- */ subs.vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position)); - fds[fdsc_count].fd = subs.vicon_pos_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- OPTICAL FLOW --- */ subs.flow_sub = orb_subscribe(ORB_ID(optical_flow)); - fds[fdsc_count].fd = subs.flow_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- RC CHANNELS --- */ subs.rc_sub = orb_subscribe(ORB_ID(rc_channels)); - fds[fdsc_count].fd = subs.rc_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- AIRSPEED --- */ subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed)); - fds[fdsc_count].fd = subs.airspeed_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- ESCs --- */ subs.esc_sub = orb_subscribe(ORB_ID(esc_status)); - fds[fdsc_count].fd = subs.esc_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- GLOBAL VELOCITY SETPOINT --- */ subs.global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint)); - fds[fdsc_count].fd = subs.global_vel_sp_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- BATTERY --- */ subs.battery_sub = orb_subscribe(ORB_ID(battery_status)); - fds[fdsc_count].fd = subs.battery_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* --- TELEMETRY STATUS --- */ subs.telemetry_sub = orb_subscribe(ORB_ID(telemetry_status)); - fds[fdsc_count].fd = subs.telemetry_sub; - fds[fdsc_count].events = POLLIN; - fdsc_count++; - - /* WARNING: If you get the error message below, - * then the number of registered messages (fdsc) - * differs from the number of messages in the above list. - */ - if (fdsc_count > fdsc) { - warn("WARNING: Not enough space for poll fds allocated. Check %s:%d", __FILE__, __LINE__); - fdsc_count = fdsc; - } - - /* - * set up poll to block for new data, - * wait for a maximum of 1000 ms - */ - const int poll_timeout = 1000; thread_running = true; @@ -990,8 +876,8 @@ int sdlog2_thread_main(int argc, char *argv[]) if (log_on_start) { /* check GPS topic to get GPS time */ if (log_name_timestamp) { - if (OK == orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos)) { - gps_time = buf.gps_pos.time_gps_usec; + if (copy_if_updated(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos)) { + gps_time = buf_gps_pos.time_gps_usec; } } @@ -999,390 +885,334 @@ int sdlog2_thread_main(int argc, char *argv[]) } while (!main_thread_should_exit) { - /* decide use usleep() or blocking poll() */ - bool use_sleep = sleep_delay > 0 && logging_enabled; - - /* poll all topics if logging enabled or only management (first 3) if not */ - int poll_ret = poll(fds, logging_enabled ? fdsc_count : 3, use_sleep ? 0 : poll_timeout); - - /* handle the poll result */ - if (poll_ret < 0) { - warnx("ERROR: poll error, stop logging"); - main_thread_should_exit = true; - - } else if (poll_ret > 0) { - - /* check all data subscriptions only if logging enabled, - * logging_enabled can be changed while checking vehicle_command and vehicle_status */ - bool check_data = logging_enabled; - int ifds = 0; - int handled_topics = 0; - - /* --- VEHICLE COMMAND - LOG MANAGEMENT --- */ - if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd); - - handle_command(&buf.cmd); - handled_topics++; - } - - /* --- VEHICLE STATUS - LOG MANAGEMENT --- */ - if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_status), subs.status_sub, &buf_status); - - if (log_when_armed) { - handle_status(&buf_status); - } - - handled_topics++; - } - - /* --- GPS POSITION - LOG MANAGEMENT --- */ - if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos); - - if (log_name_timestamp) { - gps_time = buf.gps_pos.time_gps_usec; - } - - handled_topics++; - } - - if (!logging_enabled || !check_data || handled_topics >= poll_ret) { - continue; - } - - ifds = 1; // begin from VEHICLE STATUS again - - pthread_mutex_lock(&logbuffer_mutex); - - /* write time stamp message */ - log_msg.msg_type = LOG_TIME_MSG; - log_msg.body.log_TIME.t = hrt_absolute_time(); - LOGBUFFER_WRITE_AND_COUNT(TIME); - - /* --- VEHICLE STATUS --- */ - if (fds[ifds++].revents & POLLIN) { - /* don't orb_copy, it's already done few lines above */ - log_msg.msg_type = LOG_STAT_MSG; - log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state; - log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state; - log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; - log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning; - log_msg.body.log_STAT.landed = (uint8_t) buf_status.condition_landed; - LOGBUFFER_WRITE_AND_COUNT(STAT); - } - - /* --- GPS POSITION --- */ - if (fds[ifds++].revents & POLLIN) { - /* don't orb_copy, it's already done few lines above */ - log_msg.msg_type = LOG_GPS_MSG; - log_msg.body.log_GPS.gps_time = buf.gps_pos.time_gps_usec; - log_msg.body.log_GPS.fix_type = buf.gps_pos.fix_type; - log_msg.body.log_GPS.eph = buf.gps_pos.eph_m; - log_msg.body.log_GPS.epv = buf.gps_pos.epv_m; - log_msg.body.log_GPS.lat = buf.gps_pos.lat; - log_msg.body.log_GPS.lon = buf.gps_pos.lon; - log_msg.body.log_GPS.alt = buf.gps_pos.alt * 0.001f; - log_msg.body.log_GPS.vel_n = buf.gps_pos.vel_n_m_s; - log_msg.body.log_GPS.vel_e = buf.gps_pos.vel_e_m_s; - log_msg.body.log_GPS.vel_d = buf.gps_pos.vel_d_m_s; - log_msg.body.log_GPS.cog = buf.gps_pos.cog_rad; - LOGBUFFER_WRITE_AND_COUNT(GPS); - } - - /* --- SENSOR COMBINED --- */ - if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor); - bool write_IMU = false; - bool write_SENS = false; - - if (buf.sensor.gyro_counter != gyro_counter) { - gyro_counter = buf.sensor.gyro_counter; - write_IMU = true; - } - - if (buf.sensor.accelerometer_counter != accelerometer_counter) { - accelerometer_counter = buf.sensor.accelerometer_counter; - write_IMU = true; - } - - if (buf.sensor.magnetometer_counter != magnetometer_counter) { - magnetometer_counter = buf.sensor.magnetometer_counter; - write_IMU = true; - } - - if (buf.sensor.baro_counter != baro_counter) { - baro_counter = buf.sensor.baro_counter; - write_SENS = true; - } - - if (buf.sensor.differential_pressure_counter != differential_pressure_counter) { - differential_pressure_counter = buf.sensor.differential_pressure_counter; - write_SENS = true; - } - - if (write_IMU) { - log_msg.msg_type = LOG_IMU_MSG; - log_msg.body.log_IMU.gyro_x = buf.sensor.gyro_rad_s[0]; - log_msg.body.log_IMU.gyro_y = buf.sensor.gyro_rad_s[1]; - log_msg.body.log_IMU.gyro_z = buf.sensor.gyro_rad_s[2]; - log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer_m_s2[0]; - log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer_m_s2[1]; - log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer_m_s2[2]; - log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer_ga[0]; - log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer_ga[1]; - log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer_ga[2]; - LOGBUFFER_WRITE_AND_COUNT(IMU); - } - - if (write_SENS) { - log_msg.msg_type = LOG_SENS_MSG; - log_msg.body.log_SENS.baro_pres = buf.sensor.baro_pres_mbar; - log_msg.body.log_SENS.baro_alt = buf.sensor.baro_alt_meter; - log_msg.body.log_SENS.baro_temp = buf.sensor.baro_temp_celcius; - log_msg.body.log_SENS.diff_pres = buf.sensor.differential_pressure_pa; - LOGBUFFER_WRITE_AND_COUNT(SENS); - } - } - - /* --- ATTITUDE --- */ - if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att); - log_msg.msg_type = LOG_ATT_MSG; - log_msg.body.log_ATT.roll = buf.att.roll; - log_msg.body.log_ATT.pitch = buf.att.pitch; - log_msg.body.log_ATT.yaw = buf.att.yaw; - log_msg.body.log_ATT.roll_rate = buf.att.rollspeed; - log_msg.body.log_ATT.pitch_rate = buf.att.pitchspeed; - log_msg.body.log_ATT.yaw_rate = buf.att.yawspeed; - log_msg.body.log_ATT.gx = buf.att.g_comp[0]; - log_msg.body.log_ATT.gy = buf.att.g_comp[1]; - log_msg.body.log_ATT.gz = buf.att.g_comp[2]; - LOGBUFFER_WRITE_AND_COUNT(ATT); - } - - /* --- ATTITUDE SETPOINT --- */ - if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_attitude_setpoint), subs.att_sp_sub, &buf.att_sp); - log_msg.msg_type = LOG_ATSP_MSG; - log_msg.body.log_ATSP.roll_sp = buf.att_sp.roll_body; - log_msg.body.log_ATSP.pitch_sp = buf.att_sp.pitch_body; - log_msg.body.log_ATSP.yaw_sp = buf.att_sp.yaw_body; - log_msg.body.log_ATSP.thrust_sp = buf.att_sp.thrust; - LOGBUFFER_WRITE_AND_COUNT(ATSP); - } - - /* --- RATES SETPOINT --- */ - if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_rates_setpoint), subs.rates_sp_sub, &buf.rates_sp); - log_msg.msg_type = LOG_ARSP_MSG; - log_msg.body.log_ARSP.roll_rate_sp = buf.rates_sp.roll; - log_msg.body.log_ARSP.pitch_rate_sp = buf.rates_sp.pitch; - log_msg.body.log_ARSP.yaw_rate_sp = buf.rates_sp.yaw; - LOGBUFFER_WRITE_AND_COUNT(ARSP); - } - - /* --- ACTUATOR OUTPUTS --- */ - if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(actuator_outputs_0), subs.act_outputs_sub, &buf.act_outputs); - log_msg.msg_type = LOG_OUT0_MSG; - memcpy(log_msg.body.log_OUT0.output, buf.act_outputs.output, sizeof(log_msg.body.log_OUT0.output)); - LOGBUFFER_WRITE_AND_COUNT(OUT0); - } - - /* --- ACTUATOR CONTROL --- */ - if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.act_controls_sub, &buf.act_controls); - log_msg.msg_type = LOG_ATTC_MSG; - log_msg.body.log_ATTC.roll = buf.act_controls.control[0]; - log_msg.body.log_ATTC.pitch = buf.act_controls.control[1]; - log_msg.body.log_ATTC.yaw = buf.act_controls.control[2]; - log_msg.body.log_ATTC.thrust = buf.act_controls.control[3]; - LOGBUFFER_WRITE_AND_COUNT(ATTC); - } - - /* --- LOCAL POSITION --- */ - if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos); - log_msg.msg_type = LOG_LPOS_MSG; - log_msg.body.log_LPOS.x = buf.local_pos.x; - log_msg.body.log_LPOS.y = buf.local_pos.y; - log_msg.body.log_LPOS.z = buf.local_pos.z; - log_msg.body.log_LPOS.vx = buf.local_pos.vx; - log_msg.body.log_LPOS.vy = buf.local_pos.vy; - log_msg.body.log_LPOS.vz = buf.local_pos.vz; - log_msg.body.log_LPOS.ref_lat = buf.local_pos.ref_lat; - log_msg.body.log_LPOS.ref_lon = buf.local_pos.ref_lon; - log_msg.body.log_LPOS.ref_alt = buf.local_pos.ref_alt; - log_msg.body.log_LPOS.xy_flags = (buf.local_pos.xy_valid ? 1 : 0) | (buf.local_pos.v_xy_valid ? 2 : 0) | (buf.local_pos.xy_global ? 8 : 0); - log_msg.body.log_LPOS.z_flags = (buf.local_pos.z_valid ? 1 : 0) | (buf.local_pos.v_z_valid ? 2 : 0) | (buf.local_pos.z_global ? 8 : 0); - log_msg.body.log_LPOS.landed = buf.local_pos.landed; - LOGBUFFER_WRITE_AND_COUNT(LPOS); - - if (buf.local_pos.dist_bottom_valid) { - dist_bottom_present = true; - } - if (dist_bottom_present) { - log_msg.msg_type = LOG_DIST_MSG; - log_msg.body.log_DIST.bottom = buf.local_pos.dist_bottom; - log_msg.body.log_DIST.bottom_rate = buf.local_pos.dist_bottom_rate; - log_msg.body.log_DIST.flags = (buf.local_pos.dist_bottom_valid ? 1 : 0); - LOGBUFFER_WRITE_AND_COUNT(DIST); - } - } - - /* --- LOCAL POSITION SETPOINT --- */ - if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_local_position_setpoint), subs.local_pos_sp_sub, &buf.local_pos_sp); - log_msg.msg_type = LOG_LPSP_MSG; - log_msg.body.log_LPSP.x = buf.local_pos_sp.x; - log_msg.body.log_LPSP.y = buf.local_pos_sp.y; - log_msg.body.log_LPSP.z = buf.local_pos_sp.z; - log_msg.body.log_LPSP.yaw = buf.local_pos_sp.yaw; - LOGBUFFER_WRITE_AND_COUNT(LPSP); - } - - /* --- GLOBAL POSITION --- */ - if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos); - log_msg.msg_type = LOG_GPOS_MSG; - log_msg.body.log_GPOS.lat = buf.global_pos.lat * 1e7; - log_msg.body.log_GPOS.lon = buf.global_pos.lon * 1e7; - log_msg.body.log_GPOS.alt = buf.global_pos.alt; - log_msg.body.log_GPOS.vel_n = buf.global_pos.vel_n; - log_msg.body.log_GPOS.vel_e = buf.global_pos.vel_e; - log_msg.body.log_GPOS.vel_d = buf.global_pos.vel_d; - log_msg.body.log_GPOS.baro_alt = buf.global_pos.baro_alt; - log_msg.body.log_GPOS.flags = (buf.global_pos.baro_valid ? 1 : 0) | (buf.global_pos.global_valid ? 2 : 0); - LOGBUFFER_WRITE_AND_COUNT(GPOS); - } - - /* --- GLOBAL POSITION SETPOINT --- */ - if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(position_setpoint_triplet), subs.triplet_sub, &buf.triplet); - log_msg.msg_type = LOG_GPSP_MSG; - log_msg.body.log_GPSP.nav_state = buf.triplet.nav_state; - log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7d); - log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7d); - log_msg.body.log_GPSP.alt = buf.triplet.current.alt; - log_msg.body.log_GPSP.yaw = buf.triplet.current.yaw; - log_msg.body.log_GPSP.type = buf.triplet.current.type; - log_msg.body.log_GPSP.loiter_radius = buf.triplet.current.loiter_radius; - log_msg.body.log_GPSP.loiter_direction = buf.triplet.current.loiter_direction; - log_msg.body.log_GPSP.pitch_min = buf.triplet.current.pitch_min; - LOGBUFFER_WRITE_AND_COUNT(GPSP); - } - - /* --- VICON POSITION --- */ - if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos); - // TODO not implemented yet - } - - /* --- FLOW --- */ - if (fds[ifds++].revents & POLLIN) { - orb_copy(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.quality = buf.flow.quality; - log_msg.body.log_FLOW.sensor_id = buf.flow.sensor_id; - LOGBUFFER_WRITE_AND_COUNT(FLOW); - } - - /* --- RC CHANNELS --- */ - if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(rc_channels), subs.rc_sub, &buf.rc); - log_msg.msg_type = LOG_RC_MSG; - /* Copy only the first 8 channels of 14 */ - memcpy(log_msg.body.log_RC.channel, buf.rc.chan, sizeof(log_msg.body.log_RC.channel)); - log_msg.body.log_RC.channel_count = buf.rc.chan_count; - LOGBUFFER_WRITE_AND_COUNT(RC); - } - - /* --- AIRSPEED --- */ - if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(airspeed), subs.airspeed_sub, &buf.airspeed); - log_msg.msg_type = LOG_AIRS_MSG; - log_msg.body.log_AIRS.indicated_airspeed = buf.airspeed.indicated_airspeed_m_s; - log_msg.body.log_AIRS.true_airspeed = buf.airspeed.true_airspeed_m_s; - LOGBUFFER_WRITE_AND_COUNT(AIRS); - } - - /* --- ESCs --- */ - if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(esc_status), subs.esc_sub, &buf.esc); - - for (uint8_t i = 0; i < buf.esc.esc_count; i++) { - log_msg.msg_type = LOG_ESC_MSG; - log_msg.body.log_ESC.counter = buf.esc.counter; - log_msg.body.log_ESC.esc_count = buf.esc.esc_count; - log_msg.body.log_ESC.esc_connectiontype = buf.esc.esc_connectiontype; - log_msg.body.log_ESC.esc_num = i; - log_msg.body.log_ESC.esc_address = buf.esc.esc[i].esc_address; - log_msg.body.log_ESC.esc_version = buf.esc.esc[i].esc_version; - log_msg.body.log_ESC.esc_voltage = buf.esc.esc[i].esc_voltage; - log_msg.body.log_ESC.esc_current = buf.esc.esc[i].esc_current; - log_msg.body.log_ESC.esc_rpm = buf.esc.esc[i].esc_rpm; - log_msg.body.log_ESC.esc_temperature = buf.esc.esc[i].esc_temperature; - log_msg.body.log_ESC.esc_setpoint = buf.esc.esc[i].esc_setpoint; - log_msg.body.log_ESC.esc_setpoint_raw = buf.esc.esc[i].esc_setpoint_raw; - LOGBUFFER_WRITE_AND_COUNT(ESC); - } - } - - /* --- GLOBAL VELOCITY SETPOINT --- */ - if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_global_velocity_setpoint), subs.global_vel_sp_sub, &buf.global_vel_sp); - log_msg.msg_type = LOG_GVSP_MSG; - log_msg.body.log_GVSP.vx = buf.global_vel_sp.vx; - log_msg.body.log_GVSP.vy = buf.global_vel_sp.vy; - log_msg.body.log_GVSP.vz = buf.global_vel_sp.vz; - LOGBUFFER_WRITE_AND_COUNT(GVSP); - } - - /* --- BATTERY --- */ - if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(battery_status), subs.battery_sub, &buf.battery); - log_msg.msg_type = LOG_BATT_MSG; - log_msg.body.log_BATT.voltage = buf.battery.voltage_v; - log_msg.body.log_BATT.voltage_filtered = buf.battery.voltage_filtered_v; - log_msg.body.log_BATT.current = buf.battery.current_a; - log_msg.body.log_BATT.discharged = buf.battery.discharged_mah; - LOGBUFFER_WRITE_AND_COUNT(BATT); - } - - /* --- TELEMETRY --- */ - if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(telemetry_status), subs.telemetry_sub, &buf.telemetry); - log_msg.msg_type = LOG_TELE_MSG; - log_msg.body.log_TELE.rssi = buf.telemetry.rssi; - log_msg.body.log_TELE.remote_rssi = buf.telemetry.remote_rssi; - log_msg.body.log_TELE.noise = buf.telemetry.noise; - log_msg.body.log_TELE.remote_noise = buf.telemetry.remote_noise; - log_msg.body.log_TELE.rxerrors = buf.telemetry.rxerrors; - log_msg.body.log_TELE.fixed = buf.telemetry.fixed; - log_msg.body.log_TELE.txbuf = buf.telemetry.txbuf; - LOGBUFFER_WRITE_AND_COUNT(TELE); - } - - /* signal the other thread new data, but not yet unlock */ - if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) { - /* only request write if several packets can be written at once */ - pthread_cond_signal(&logbuffer_cond); - } - - /* unlock, now the writer thread may run */ - pthread_mutex_unlock(&logbuffer_mutex); + /* --- VEHICLE COMMAND - LOG MANAGEMENT --- */ + if (copy_if_updated(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd)) { + handle_command(&buf.cmd); } - if (use_sleep) { - usleep(sleep_delay); + /* --- VEHICLE STATUS - LOG MANAGEMENT --- */ + bool status_updated = copy_if_updated(ORB_ID(vehicle_status), subs.status_sub, &buf_status); + if (status_updated) { + if (log_when_armed) { + handle_status(&buf_status); + } } + + /* --- GPS POSITION - LOG MANAGEMENT --- */ + bool gps_pos_updated = copy_if_updated(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos); + if (gps_pos_updated && log_name_timestamp) { + gps_time = buf_gps_pos.time_gps_usec; + } + + if (!logging_enabled) { + continue; + } + + pthread_mutex_lock(&logbuffer_mutex); + + /* write time stamp message */ + log_msg.msg_type = LOG_TIME_MSG; + log_msg.body.log_TIME.t = hrt_absolute_time(); + LOGBUFFER_WRITE_AND_COUNT(TIME); + + /* --- VEHICLE STATUS --- */ + if (status_updated) { + log_msg.msg_type = LOG_STAT_MSG; + log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state; + log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state; + log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; + log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning; + log_msg.body.log_STAT.landed = (uint8_t) buf_status.condition_landed; + LOGBUFFER_WRITE_AND_COUNT(STAT); + } + + /* --- GPS POSITION --- */ + if (gps_pos_updated) { + log_msg.msg_type = LOG_GPS_MSG; + log_msg.body.log_GPS.gps_time = buf_gps_pos.time_gps_usec; + log_msg.body.log_GPS.fix_type = buf_gps_pos.fix_type; + log_msg.body.log_GPS.eph = buf_gps_pos.eph_m; + log_msg.body.log_GPS.epv = buf_gps_pos.epv_m; + log_msg.body.log_GPS.lat = buf_gps_pos.lat; + log_msg.body.log_GPS.lon = buf_gps_pos.lon; + log_msg.body.log_GPS.alt = buf_gps_pos.alt * 0.001f; + log_msg.body.log_GPS.vel_n = buf_gps_pos.vel_n_m_s; + log_msg.body.log_GPS.vel_e = buf_gps_pos.vel_e_m_s; + log_msg.body.log_GPS.vel_d = buf_gps_pos.vel_d_m_s; + log_msg.body.log_GPS.cog = buf_gps_pos.cog_rad; + LOGBUFFER_WRITE_AND_COUNT(GPS); + } + + /* --- SENSOR COMBINED --- */ + if (copy_if_updated(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor)) { + bool write_IMU = false; + bool write_SENS = false; + + if (buf.sensor.gyro_counter != gyro_counter) { + gyro_counter = buf.sensor.gyro_counter; + write_IMU = true; + } + + if (buf.sensor.accelerometer_counter != accelerometer_counter) { + accelerometer_counter = buf.sensor.accelerometer_counter; + write_IMU = true; + } + + if (buf.sensor.magnetometer_counter != magnetometer_counter) { + magnetometer_counter = buf.sensor.magnetometer_counter; + write_IMU = true; + } + + if (buf.sensor.baro_counter != baro_counter) { + baro_counter = buf.sensor.baro_counter; + write_SENS = true; + } + + if (buf.sensor.differential_pressure_counter != differential_pressure_counter) { + differential_pressure_counter = buf.sensor.differential_pressure_counter; + write_SENS = true; + } + + if (write_IMU) { + log_msg.msg_type = LOG_IMU_MSG; + log_msg.body.log_IMU.gyro_x = buf.sensor.gyro_rad_s[0]; + log_msg.body.log_IMU.gyro_y = buf.sensor.gyro_rad_s[1]; + log_msg.body.log_IMU.gyro_z = buf.sensor.gyro_rad_s[2]; + log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer_m_s2[0]; + log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer_m_s2[1]; + log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer_m_s2[2]; + log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer_ga[0]; + log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer_ga[1]; + log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer_ga[2]; + LOGBUFFER_WRITE_AND_COUNT(IMU); + } + + if (write_SENS) { + log_msg.msg_type = LOG_SENS_MSG; + log_msg.body.log_SENS.baro_pres = buf.sensor.baro_pres_mbar; + log_msg.body.log_SENS.baro_alt = buf.sensor.baro_alt_meter; + log_msg.body.log_SENS.baro_temp = buf.sensor.baro_temp_celcius; + log_msg.body.log_SENS.diff_pres = buf.sensor.differential_pressure_pa; + LOGBUFFER_WRITE_AND_COUNT(SENS); + } + } + + /* --- ATTITUDE --- */ + if (copy_if_updated(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att)) { + log_msg.msg_type = LOG_ATT_MSG; + log_msg.body.log_ATT.roll = buf.att.roll; + log_msg.body.log_ATT.pitch = buf.att.pitch; + log_msg.body.log_ATT.yaw = buf.att.yaw; + log_msg.body.log_ATT.roll_rate = buf.att.rollspeed; + log_msg.body.log_ATT.pitch_rate = buf.att.pitchspeed; + log_msg.body.log_ATT.yaw_rate = buf.att.yawspeed; + log_msg.body.log_ATT.gx = buf.att.g_comp[0]; + log_msg.body.log_ATT.gy = buf.att.g_comp[1]; + log_msg.body.log_ATT.gz = buf.att.g_comp[2]; + LOGBUFFER_WRITE_AND_COUNT(ATT); + } + + /* --- ATTITUDE SETPOINT --- */ + if (copy_if_updated(ORB_ID(vehicle_attitude_setpoint), subs.att_sp_sub, &buf.att_sp)) { + log_msg.msg_type = LOG_ATSP_MSG; + log_msg.body.log_ATSP.roll_sp = buf.att_sp.roll_body; + log_msg.body.log_ATSP.pitch_sp = buf.att_sp.pitch_body; + log_msg.body.log_ATSP.yaw_sp = buf.att_sp.yaw_body; + log_msg.body.log_ATSP.thrust_sp = buf.att_sp.thrust; + LOGBUFFER_WRITE_AND_COUNT(ATSP); + } + + /* --- RATES SETPOINT --- */ + if (copy_if_updated(ORB_ID(vehicle_rates_setpoint), subs.rates_sp_sub, &buf.rates_sp)) { + log_msg.msg_type = LOG_ARSP_MSG; + log_msg.body.log_ARSP.roll_rate_sp = buf.rates_sp.roll; + log_msg.body.log_ARSP.pitch_rate_sp = buf.rates_sp.pitch; + log_msg.body.log_ARSP.yaw_rate_sp = buf.rates_sp.yaw; + LOGBUFFER_WRITE_AND_COUNT(ARSP); + } + + /* --- ACTUATOR OUTPUTS --- */ + if (copy_if_updated(ORB_ID(actuator_outputs_0), subs.act_outputs_sub, &buf.act_outputs)) { + log_msg.msg_type = LOG_OUT0_MSG; + memcpy(log_msg.body.log_OUT0.output, buf.act_outputs.output, sizeof(log_msg.body.log_OUT0.output)); + LOGBUFFER_WRITE_AND_COUNT(OUT0); + } + + /* --- ACTUATOR CONTROL --- */ + if (copy_if_updated(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.act_controls_sub, &buf.act_controls)) { + log_msg.msg_type = LOG_ATTC_MSG; + log_msg.body.log_ATTC.roll = buf.act_controls.control[0]; + log_msg.body.log_ATTC.pitch = buf.act_controls.control[1]; + log_msg.body.log_ATTC.yaw = buf.act_controls.control[2]; + log_msg.body.log_ATTC.thrust = buf.act_controls.control[3]; + LOGBUFFER_WRITE_AND_COUNT(ATTC); + } + + /* --- LOCAL POSITION --- */ + if (copy_if_updated(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos)) { + log_msg.msg_type = LOG_LPOS_MSG; + log_msg.body.log_LPOS.x = buf.local_pos.x; + log_msg.body.log_LPOS.y = buf.local_pos.y; + log_msg.body.log_LPOS.z = buf.local_pos.z; + log_msg.body.log_LPOS.vx = buf.local_pos.vx; + log_msg.body.log_LPOS.vy = buf.local_pos.vy; + log_msg.body.log_LPOS.vz = buf.local_pos.vz; + log_msg.body.log_LPOS.ref_lat = buf.local_pos.ref_lat; + log_msg.body.log_LPOS.ref_lon = buf.local_pos.ref_lon; + log_msg.body.log_LPOS.ref_alt = buf.local_pos.ref_alt; + log_msg.body.log_LPOS.xy_flags = (buf.local_pos.xy_valid ? 1 : 0) | (buf.local_pos.v_xy_valid ? 2 : 0) | (buf.local_pos.xy_global ? 8 : 0); + log_msg.body.log_LPOS.z_flags = (buf.local_pos.z_valid ? 1 : 0) | (buf.local_pos.v_z_valid ? 2 : 0) | (buf.local_pos.z_global ? 8 : 0); + log_msg.body.log_LPOS.landed = buf.local_pos.landed; + LOGBUFFER_WRITE_AND_COUNT(LPOS); + + if (buf.local_pos.dist_bottom_valid) { + dist_bottom_present = true; + } + if (dist_bottom_present) { + log_msg.msg_type = LOG_DIST_MSG; + log_msg.body.log_DIST.bottom = buf.local_pos.dist_bottom; + log_msg.body.log_DIST.bottom_rate = buf.local_pos.dist_bottom_rate; + log_msg.body.log_DIST.flags = (buf.local_pos.dist_bottom_valid ? 1 : 0); + LOGBUFFER_WRITE_AND_COUNT(DIST); + } + } + + /* --- LOCAL POSITION SETPOINT --- */ + if (copy_if_updated(ORB_ID(vehicle_local_position_setpoint), subs.local_pos_sp_sub, &buf.local_pos_sp)) { + log_msg.msg_type = LOG_LPSP_MSG; + log_msg.body.log_LPSP.x = buf.local_pos_sp.x; + log_msg.body.log_LPSP.y = buf.local_pos_sp.y; + log_msg.body.log_LPSP.z = buf.local_pos_sp.z; + log_msg.body.log_LPSP.yaw = buf.local_pos_sp.yaw; + LOGBUFFER_WRITE_AND_COUNT(LPSP); + } + + /* --- GLOBAL POSITION --- */ + if (copy_if_updated(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos)) { + log_msg.msg_type = LOG_GPOS_MSG; + log_msg.body.log_GPOS.lat = buf.global_pos.lat * 1e7; + log_msg.body.log_GPOS.lon = buf.global_pos.lon * 1e7; + log_msg.body.log_GPOS.alt = buf.global_pos.alt; + log_msg.body.log_GPOS.vel_n = buf.global_pos.vel_n; + log_msg.body.log_GPOS.vel_e = buf.global_pos.vel_e; + log_msg.body.log_GPOS.vel_d = buf.global_pos.vel_d; + log_msg.body.log_GPOS.baro_alt = buf.global_pos.baro_alt; + log_msg.body.log_GPOS.flags = (buf.global_pos.baro_valid ? 1 : 0) | (buf.global_pos.global_valid ? 2 : 0); + LOGBUFFER_WRITE_AND_COUNT(GPOS); + } + + /* --- GLOBAL POSITION SETPOINT --- */ + if (copy_if_updated(ORB_ID(position_setpoint_triplet), subs.triplet_sub, &buf.triplet)) { + log_msg.msg_type = LOG_GPSP_MSG; + log_msg.body.log_GPSP.nav_state = buf.triplet.nav_state; + log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7d); + log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7d); + log_msg.body.log_GPSP.alt = buf.triplet.current.alt; + log_msg.body.log_GPSP.yaw = buf.triplet.current.yaw; + log_msg.body.log_GPSP.type = buf.triplet.current.type; + log_msg.body.log_GPSP.loiter_radius = buf.triplet.current.loiter_radius; + log_msg.body.log_GPSP.loiter_direction = buf.triplet.current.loiter_direction; + log_msg.body.log_GPSP.pitch_min = buf.triplet.current.pitch_min; + LOGBUFFER_WRITE_AND_COUNT(GPSP); + } + + /* --- VICON POSITION --- */ + if (copy_if_updated(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos)) { + // TODO not implemented yet + } + + /* --- 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.quality = buf.flow.quality; + log_msg.body.log_FLOW.sensor_id = buf.flow.sensor_id; + LOGBUFFER_WRITE_AND_COUNT(FLOW); + } + + /* --- RC CHANNELS --- */ + if (copy_if_updated(ORB_ID(rc_channels), subs.rc_sub, &buf.rc)) { + log_msg.msg_type = LOG_RC_MSG; + /* Copy only the first 8 channels of 14 */ + memcpy(log_msg.body.log_RC.channel, buf.rc.chan, sizeof(log_msg.body.log_RC.channel)); + log_msg.body.log_RC.channel_count = buf.rc.chan_count; + LOGBUFFER_WRITE_AND_COUNT(RC); + } + + /* --- AIRSPEED --- */ + if (copy_if_updated(ORB_ID(airspeed), subs.airspeed_sub, &buf.airspeed)) { + log_msg.msg_type = LOG_AIRS_MSG; + log_msg.body.log_AIRS.indicated_airspeed = buf.airspeed.indicated_airspeed_m_s; + log_msg.body.log_AIRS.true_airspeed = buf.airspeed.true_airspeed_m_s; + LOGBUFFER_WRITE_AND_COUNT(AIRS); + } + + /* --- ESCs --- */ + if (copy_if_updated(ORB_ID(esc_status), subs.esc_sub, &buf.esc)) { + for (uint8_t i = 0; i < buf.esc.esc_count; i++) { + log_msg.msg_type = LOG_ESC_MSG; + log_msg.body.log_ESC.counter = buf.esc.counter; + log_msg.body.log_ESC.esc_count = buf.esc.esc_count; + log_msg.body.log_ESC.esc_connectiontype = buf.esc.esc_connectiontype; + log_msg.body.log_ESC.esc_num = i; + log_msg.body.log_ESC.esc_address = buf.esc.esc[i].esc_address; + log_msg.body.log_ESC.esc_version = buf.esc.esc[i].esc_version; + log_msg.body.log_ESC.esc_voltage = buf.esc.esc[i].esc_voltage; + log_msg.body.log_ESC.esc_current = buf.esc.esc[i].esc_current; + log_msg.body.log_ESC.esc_rpm = buf.esc.esc[i].esc_rpm; + log_msg.body.log_ESC.esc_temperature = buf.esc.esc[i].esc_temperature; + log_msg.body.log_ESC.esc_setpoint = buf.esc.esc[i].esc_setpoint; + log_msg.body.log_ESC.esc_setpoint_raw = buf.esc.esc[i].esc_setpoint_raw; + LOGBUFFER_WRITE_AND_COUNT(ESC); + } + } + + /* --- GLOBAL VELOCITY SETPOINT --- */ + if (copy_if_updated(ORB_ID(vehicle_global_velocity_setpoint), subs.global_vel_sp_sub, &buf.global_vel_sp)) { + log_msg.msg_type = LOG_GVSP_MSG; + log_msg.body.log_GVSP.vx = buf.global_vel_sp.vx; + log_msg.body.log_GVSP.vy = buf.global_vel_sp.vy; + log_msg.body.log_GVSP.vz = buf.global_vel_sp.vz; + LOGBUFFER_WRITE_AND_COUNT(GVSP); + } + + /* --- BATTERY --- */ + if (copy_if_updated(ORB_ID(battery_status), subs.battery_sub, &buf.battery)) { + log_msg.msg_type = LOG_BATT_MSG; + log_msg.body.log_BATT.voltage = buf.battery.voltage_v; + log_msg.body.log_BATT.voltage_filtered = buf.battery.voltage_filtered_v; + log_msg.body.log_BATT.current = buf.battery.current_a; + log_msg.body.log_BATT.discharged = buf.battery.discharged_mah; + LOGBUFFER_WRITE_AND_COUNT(BATT); + } + + /* --- TELEMETRY --- */ + if (copy_if_updated(ORB_ID(telemetry_status), subs.telemetry_sub, &buf.telemetry)) { + log_msg.msg_type = LOG_TELE_MSG; + log_msg.body.log_TELE.rssi = buf.telemetry.rssi; + log_msg.body.log_TELE.remote_rssi = buf.telemetry.remote_rssi; + log_msg.body.log_TELE.noise = buf.telemetry.noise; + log_msg.body.log_TELE.remote_noise = buf.telemetry.remote_noise; + log_msg.body.log_TELE.rxerrors = buf.telemetry.rxerrors; + log_msg.body.log_TELE.fixed = buf.telemetry.fixed; + log_msg.body.log_TELE.txbuf = buf.telemetry.txbuf; + LOGBUFFER_WRITE_AND_COUNT(TELE); + } + + /* signal the other thread new data, but not yet unlock */ + if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) { + /* only request write if several packets can be written at once */ + pthread_cond_signal(&logbuffer_cond); + } + + /* unlock, now the writer thread may run */ + pthread_mutex_unlock(&logbuffer_mutex); + + usleep(sleep_delay); } if (logging_enabled) @@ -1461,8 +1291,6 @@ int file_copy(const char *file_old, const char *file_new) void handle_command(struct vehicle_command_s *cmd) { - /* result of the command */ - uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; int param; /* request to set different system mode */ From 80595c177a65e9ba91a35ffdee49d518df657508 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 7 Mar 2014 11:18:31 +0400 Subject: [PATCH 138/381] sdlog2: code style fixed --- src/modules/sdlog2/logbuffer.c | 3 ++- src/modules/sdlog2/sdlog2.c | 24 ++++++++++++++++++++---- src/modules/sdlog2/sdlog2_messages.h | 14 +++++++------- 3 files changed, 29 insertions(+), 12 deletions(-) diff --git a/src/modules/sdlog2/logbuffer.c b/src/modules/sdlog2/logbuffer.c index 6a29d7e5c9..2da67d8a92 100644 --- a/src/modules/sdlog2/logbuffer.c +++ b/src/modules/sdlog2/logbuffer.c @@ -74,8 +74,9 @@ bool logbuffer_write(struct logbuffer_s *lb, void *ptr, int size) // bytes available to write int available = lb->read_ptr - lb->write_ptr - 1; - if (available < 0) + if (available < 0) { available += lb->size; + } if (size > available) { // buffer overflow diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index fa25d74f85..5acd2b615a 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -222,8 +222,9 @@ static int open_log_file(void); static void sdlog2_usage(const char *reason) { - if (reason) + if (reason) { fprintf(stderr, "%s\n", reason); + } errx(1, "usage: sdlog2 {start|stop|status} [-r ] [-b ] -e -a -t\n" "\t-r\tLog rate in Hz, 0 means unlimited rate\n" @@ -243,8 +244,9 @@ sdlog2_usage(const char *reason) */ int sdlog2_main(int argc, char *argv[]) { - if (argc < 2) + if (argc < 2) { sdlog2_usage("missing command"); + } if (!strcmp(argv[1], "start")) { @@ -403,22 +405,29 @@ static void *logwriter_thread(void *arg) int log_fd = open_log_file(); - if (log_fd < 0) + if (log_fd < 0) { return NULL; + } struct logbuffer_s *logbuf = (struct logbuffer_s *)arg; /* write log messages formats, version and parameters */ log_bytes_written += write_formats(log_fd); + log_bytes_written += write_version(log_fd); + log_bytes_written += write_parameters(log_fd); + fsync(log_fd); int poll_count = 0; void *read_ptr; + int n = 0; + bool should_wait = false; + bool is_part = false; while (true) { @@ -673,6 +682,7 @@ int sdlog2_thread_main(int argc, char *argv[]) if (r <= 0) { r = 1; } + sleep_delay = 1000000 / r; } break; @@ -745,9 +755,11 @@ int sdlog2_thread_main(int argc, char *argv[]) } struct vehicle_status_s buf_status; + struct vehicle_gps_position_s buf_gps_pos; memset(&buf_status, 0, sizeof(buf_status)); + memset(&buf_gps_pos, 0, sizeof(buf_gps_pos)); /* warning! using union here to save memory, elements should be used separately! */ @@ -892,6 +904,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- VEHICLE STATUS - LOG MANAGEMENT --- */ bool status_updated = copy_if_updated(ORB_ID(vehicle_status), subs.status_sub, &buf_status); + if (status_updated) { if (log_when_armed) { handle_status(&buf_status); @@ -900,6 +913,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- GPS POSITION - LOG MANAGEMENT --- */ bool gps_pos_updated = copy_if_updated(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos); + if (gps_pos_updated && log_name_timestamp) { gps_time = buf_gps_pos.time_gps_usec; } @@ -1068,6 +1082,7 @@ int sdlog2_thread_main(int argc, char *argv[]) if (buf.local_pos.dist_bottom_valid) { dist_bottom_present = true; } + if (dist_bottom_present) { log_msg.msg_type = LOG_DIST_MSG; log_msg.body.log_DIST.bottom = buf.local_pos.dist_bottom; @@ -1215,8 +1230,9 @@ int sdlog2_thread_main(int argc, char *argv[]) usleep(sleep_delay); } - if (logging_enabled) + if (logging_enabled) { sdlog2_stop_log(); + } pthread_mutex_destroy(&logbuffer_mutex); pthread_cond_destroy(&logbuffer_cond); diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 16bfc355d6..e27518aa0a 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -267,13 +267,13 @@ struct log_DIST_s { /* --- TELE - TELEMETRY STATUS --- */ #define LOG_TELE_MSG 22 struct log_TELE_s { - uint8_t rssi; - uint8_t remote_rssi; - uint8_t noise; - uint8_t remote_noise; - uint16_t rxerrors; - uint16_t fixed; - uint8_t txbuf; + uint8_t rssi; + uint8_t remote_rssi; + uint8_t noise; + uint8_t remote_noise; + uint16_t rxerrors; + uint16_t fixed; + uint8_t txbuf; }; /********** SYSTEM MESSAGES, ID > 0x80 **********/ From 2ee8214244efbe09981e71ea194573daa1c2f5bc Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 7 Mar 2014 11:27:24 +0400 Subject: [PATCH 139/381] sdlog2: move some global variables to local scope, set default log rate to 50 Hz --- src/modules/sdlog2/sdlog2.c | 15 ++++++--------- 1 file changed, 6 insertions(+), 9 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 5acd2b615a..d784ecc4f4 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -134,12 +134,6 @@ static uint64_t gps_time = 0; /* current state of logging */ static bool logging_enabled = false; -/* enable logging on start (-e option) */ -static bool log_on_start = false; -/* enable logging when armed (-a option) */ -static bool log_when_armed = false; -/* delay = 1 / rate (rate defined by -r option) */ -static useconds_t sleep_delay = 0; /* use date/time for naming directories and files (-t option) */ static bool log_name_timestamp = false; @@ -660,11 +654,14 @@ int sdlog2_thread_main(int argc, char *argv[]) warnx("failed to open MAVLink log stream, start mavlink app first"); } - useconds_t sleep_delay = 10000; // default log rate: 100 Hz + /* delay = 1 / rate (rate defined by -r option), default log rate: 50 Hz */ + useconds_t sleep_delay = 20000; int log_buffer_size = LOG_BUFFER_SIZE_DEFAULT; logging_enabled = false; - log_on_start = false; - log_when_armed = false; + /* enable logging on start (-e option) */ + bool log_on_start = false; + /* enable logging when armed (-a option) */ + bool log_when_armed = false; log_name_timestamp = false; flag_system_armed = false; From c11e1ee0ab8579826ed73b1f596e0535156bbce4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 7 Mar 2014 23:14:39 +0400 Subject: [PATCH 140/381] sdlog2: lseek() workaround removed --- src/modules/sdlog2/sdlog2.c | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index d784ecc4f4..200d70e93f 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -454,7 +454,6 @@ static void *logwriter_thread(void *arg) n = available; } - lseek(log_fd, 0, SEEK_CUR); n = write(log_fd, read_ptr, n); should_wait = (n == available) && !is_part; From 2ee8cf1cf08095693b168238a311b1caf8df1701 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 7 Mar 2014 23:28:38 +0400 Subject: [PATCH 141/381] sdlog2: bug fixed, sleep when idle too --- src/modules/sdlog2/sdlog2.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 200d70e93f..58890322be 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -893,6 +893,8 @@ int sdlog2_thread_main(int argc, char *argv[]) } while (!main_thread_should_exit) { + usleep(sleep_delay); + /* --- VEHICLE COMMAND - LOG MANAGEMENT --- */ if (copy_if_updated(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd)) { handle_command(&buf.cmd); @@ -1222,8 +1224,6 @@ int sdlog2_thread_main(int argc, char *argv[]) /* unlock, now the writer thread may run */ pthread_mutex_unlock(&logbuffer_mutex); - - usleep(sleep_delay); } if (logging_enabled) { From a478dac621301a993fb8ed13b86abd61d93bfec0 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 7 Mar 2014 23:29:05 +0400 Subject: [PATCH 142/381] sdlog2: max write chunk increased to 1024 --- src/modules/sdlog2/sdlog2.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 58890322be..d213383763 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -110,7 +110,7 @@ static bool logwriter_should_exit = false; /**< Logwriter thread exit flag */ static const int MAX_NO_LOGFOLDER = 999; /**< Maximum number of log dirs */ static const int MAX_NO_LOGFILE = 999; /**< Maximum number of log files */ static const int LOG_BUFFER_SIZE_DEFAULT = 8192; -static const int MAX_WRITE_CHUNK = 512; +static const int MAX_WRITE_CHUNK = 1024; static const int MIN_BYTES_TO_WRITE = 512; static const char *log_root = "/fs/microsd/log"; From aca6806b82b8ee201260a9e383879b6d821dba1a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 7 Mar 2014 23:36:07 +0400 Subject: [PATCH 143/381] Revert "sdlog2: max write chunk increased to 1024" This reverts commit a478dac621301a993fb8ed13b86abd61d93bfec0. --- src/modules/sdlog2/sdlog2.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index d213383763..58890322be 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -110,7 +110,7 @@ static bool logwriter_should_exit = false; /**< Logwriter thread exit flag */ static const int MAX_NO_LOGFOLDER = 999; /**< Maximum number of log dirs */ static const int MAX_NO_LOGFILE = 999; /**< Maximum number of log files */ static const int LOG_BUFFER_SIZE_DEFAULT = 8192; -static const int MAX_WRITE_CHUNK = 1024; +static const int MAX_WRITE_CHUNK = 512; static const int MIN_BYTES_TO_WRITE = 512; static const char *log_root = "/fs/microsd/log"; From 39b1f7244ce06ff89492ad6843c5fc74a30dfe67 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 8 Mar 2014 11:37:49 +0400 Subject: [PATCH 144/381] mavlink: RC_CHANNELS_RAW added to default streams set --- src/modules/mavlink/mavlink_main.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index c10c95dbdb..788fe5732d 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1731,6 +1731,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("GPS_RAW_INT", 1.0f * rate_mult); configure_stream("GLOBAL_POSITION_INT", 3.0f * rate_mult); configure_stream("LOCAL_POSITION_NED", 3.0f * rate_mult); + configure_stream("RC_CHANNELS_RAW", 1.0f * rate_mult); break; case MODE_HIL: @@ -1743,6 +1744,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("GPS_RAW_INT", 1.0f * rate_mult); configure_stream("GLOBAL_POSITION_INT", 1.0f * rate_mult); configure_stream("LOCAL_POSITION_NED", 1.0f * rate_mult); + configure_stream("RC_CHANNELS_RAW", 1.0f * rate_mult); configure_stream("HIL_CONTROLS", 15.0f * rate_mult); break; From cb8bd1a3ad21fec45af7a1f1d53fe7b891c59c2b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 8 Mar 2014 21:05:13 +0400 Subject: [PATCH 145/381] dumpfile command and fetch_log.py tool implemented to fetch logs via nsh console on USB --- Tools/fetch_log.py | 133 ++++++++++++++++++++++++++ makefiles/config_px4fmu-v1_default.mk | 1 + makefiles/config_px4fmu-v2_default.mk | 1 + src/systemcmds/dumpfile/dumpfile.c | 116 ++++++++++++++++++++++ src/systemcmds/dumpfile/module.mk | 41 ++++++++ 5 files changed, 292 insertions(+) create mode 100644 Tools/fetch_log.py create mode 100644 src/systemcmds/dumpfile/dumpfile.c create mode 100644 src/systemcmds/dumpfile/module.mk diff --git a/Tools/fetch_log.py b/Tools/fetch_log.py new file mode 100644 index 0000000000..edcc6557c2 --- /dev/null +++ b/Tools/fetch_log.py @@ -0,0 +1,133 @@ +#!/usr/bin/env python +############################################################################ +# +# 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. +# +############################################################################ + +# +# Log fetcher +# +# Print list of logs: +# python fetch_log.py +# +# Fetch log: +# python fetch_log.py sess001/log001.bin +# + +import serial, time, sys, os + +def wait_for_string(ser, s, timeout=1.0, debug=False): + t0 = time.time() + buf = [] + res = [] + n = 0 + while (True): + c = ser.read() + if debug: + sys.stderr.write(c) + buf.append(c) + if len(buf) > len(s): + res.append(buf.pop(0)) + n += 1 + if n % 10000 == 0: + sys.stderr.write(str(n) + "\n") + if "".join(buf) == s: + break + if timeout > 0.0 and time.time() - t0 > timeout: + raise Exception("Timeout while waiting for: " + s) + return "".join(res) + +def exec_cmd(ser, cmd, timeout): + ser.write(cmd + "\n") + ser.flush() + wait_for_string(ser, cmd + "\r\n", timeout) + return wait_for_string(ser, "nsh> \x1b[K", timeout) + +def ls_dir(ser, dir, timeout=1.0): + res = [] + for line in exec_cmd(ser, "ls -l " + dir, timeout).splitlines()[1:]: + res.append((line[20:], int(line[11:19].strip()), line[1] == "d")) + return res + +def list_logs(ser): + logs_dir = "/fs/microsd/log" + res = [] + for d in ls_dir(ser, logs_dir): + if d[2]: + sess_dir = d[0][:-1] + for f in ls_dir(ser, logs_dir + "/" + sess_dir): + log_file = f[0] + log_size = f[1] + res.append(sess_dir + "/" + log_file + "\t" + str(log_size)) + return "\n".join(res) + +def fetch_log(ser, fn, timeout): + cmd = "dumpfile " + fn + ser.write(cmd + "\n") + ser.flush() + wait_for_string(ser, cmd + "\r\n", timeout, True) + res = wait_for_string(ser, "\n", timeout, True) + data = [] + if res.startswith("OK"): + size = int(res.split()[1]) + n = 0 + print "Reading data:" + while (n < size): + buf = ser.read(min(size - n, 8192)) + data.append(buf) + n += len(buf) + sys.stdout.write(".") + sys.stdout.flush() + print + else: + raise Exception("Error reading log") + wait_for_string(ser, "nsh> \x1b[K", timeout) + return "".join(data) + +def main(): + dev = "/dev/tty.usbmodem1" + ser = serial.Serial(dev, "115200", timeout=0.2) + if len(sys.argv) < 2: + print list_logs(ser) + else: + log_file = sys.argv[1] + data = fetch_log(ser, "/fs/microsd/log/" + log_file, 1.0) + try: + os.mkdir(log_file.split("/")[0]) + except: + pass + fout = open(log_file, "wb") + fout.write(data) + fout.close() + ser.close() + +if __name__ == "__main__": + main() diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index aff614cbb3..651c2ac381 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -56,6 +56,7 @@ MODULES += systemcmds/tests MODULES += systemcmds/config MODULES += systemcmds/nshterm MODULES += systemcmds/hw_ver +MODULES += systemcmds/dumpfile # # General system control diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 2ad2f63a95..889abc4d9a 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -63,6 +63,7 @@ MODULES += systemcmds/config MODULES += systemcmds/nshterm MODULES += systemcmds/mtd MODULES += systemcmds/hw_ver +MODULES += systemcmds/dumpfile # # General system control diff --git a/src/systemcmds/dumpfile/dumpfile.c b/src/systemcmds/dumpfile/dumpfile.c new file mode 100644 index 0000000000..c188143429 --- /dev/null +++ b/src/systemcmds/dumpfile/dumpfile.c @@ -0,0 +1,116 @@ +/**************************************************************************** + * + * 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 dumpfile.c + * + * Dump file utility. Prints file size and contents in binary mode (don't replace LF with CR LF) to stdout. + * + * @author Anton Babushkin + */ + +#include +#include +#include +#include +#include +#include +#include + +#include + +__EXPORT int dumpfile_main(int argc, char *argv[]); + +int +dumpfile_main(int argc, char *argv[]) +{ + if (argc < 2) { + errx(1, "usage: dumpfile "); + } + + /* open input file */ + FILE *f; + f = fopen(argv[1], "r"); + + if (f == NULL) { + printf("ERROR\n"); + exit(1); + } + + /* get file size */ + fseek(f, 0L, SEEK_END); + int size = ftell(f); + fseek(f, 0L, SEEK_SET); + + printf("OK %d\n", size); + + /* configure stdout */ + int out = fileno(stdout); + + struct termios tc; + struct termios tc_old; + tcgetattr(out, &tc); + + /* save old terminal attributes to restore it later on exit */ + memcpy(&tc_old, &tc, sizeof(tc)); + + /* don't add CR on each LF*/ + tc.c_oflag &= ~ONLCR; + + if (tcsetattr(out, TCSANOW, &tc) < 0) { + warnx("ERROR setting stdout attributes"); + exit(1); + } + + char buf[512]; + int nread; + + /* dump file */ + while ((nread = fread(buf, 1, sizeof(buf), f)) > 0) { + if (write(out, buf, nread) <= 0) { + warnx("error dumping file"); + break; + } + } + + fsync(out); + fclose(f); + + /* restore old terminal attributes */ + if (tcsetattr(out, TCSANOW, &tc_old) < 0) { + warnx("ERROR restoring stdout attributes"); + exit(1); + } + + return OK; +} diff --git a/src/systemcmds/dumpfile/module.mk b/src/systemcmds/dumpfile/module.mk new file mode 100644 index 0000000000..36461f4772 --- /dev/null +++ b/src/systemcmds/dumpfile/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 = dumpfile +SRCS = dumpfile.c + +MAXOPTIMIZATION = -Os From 5c86eb6dd0be72cb3455a82020457d13515cb527 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 8 Mar 2014 18:48:24 +0100 Subject: [PATCH 146/381] Fix code style of mb12xx driver, no functional changes --- src/drivers/mb12xx/mb12xx.cpp | 237 +++++++++++++++++++--------------- 1 file changed, 134 insertions(+), 103 deletions(-) diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index c910e28908..7693df295c 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -37,7 +37,7 @@ * * Driver for the Maxbotix sonar range finders connected via I2C. */ - + #include #include @@ -84,7 +84,7 @@ /* Device limits */ #define MB12XX_MIN_DISTANCE (0.20f) #define MB12XX_MAX_DISTANCE (7.65f) - + #define MB12XX_CONVERSION_INTERVAL 60000 /* 60ms */ /* oddly, ERROR is not defined for c++ */ @@ -102,17 +102,17 @@ class MB12XX : public device::I2C public: MB12XX(int bus = MB12XX_BUS, int address = MB12XX_BASEADDR); virtual ~MB12XX(); - + 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(); @@ -124,13 +124,13 @@ private: bool _sensor_ok; int _measure_ticks; bool _collect_phase; - + 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. @@ -139,7 +139,7 @@ private: * @return True if the device is present. */ int probe_address(uint8_t address); - + /** * Initialise the automatic measurement state machine and start it. * @@ -147,12 +147,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(); - + /** * 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 MB12XX_MIN_DISTANCE @@ -162,7 +162,7 @@ private: 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. @@ -177,8 +177,8 @@ private: * @param arg Instance pointer for the driver that is polling. */ static void cycle_trampoline(void *arg); - - + + }; /* @@ -201,7 +201,7 @@ MB12XX::MB12XX(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)); } @@ -212,8 +212,9 @@ MB12XX::~MB12XX() stop(); /* free any existing reports */ - if (_reports != nullptr) + if (_reports != nullptr) { delete _reports; + } } int @@ -222,22 +223,25 @@ MB12XX::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(range_finder_report)); - if (_reports == nullptr) + if (_reports == nullptr) { goto out; + } /* get a publish handle on the range finder topic */ struct range_finder_report zero_report; memset(&zero_report, 0, sizeof(zero_report)); _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &zero_report); - if (_range_finder_topic < 0) + 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 */ @@ -256,13 +260,13 @@ void MB12XX::set_minimum_distance(float min) { _min_distance = min; -} +} void MB12XX::set_maximum_distance(float max) { _max_distance = max; -} +} float MB12XX::get_minimum_distance() @@ -284,20 +288,20 @@ MB12XX::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? */ @@ -307,13 +311,14 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg) _measure_ticks = USEC2TICK(MB12XX_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); @@ -322,15 +327,17 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg) unsigned ticks = USEC2TICK(1000000 / arg); /* check against maximum rate */ - if (ticks < USEC2TICK(MB12XX_CONVERSION_INTERVAL)) + if (ticks < USEC2TICK(MB12XX_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; } @@ -338,45 +345,49 @@ MB12XX::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; - - irqstate_t flags = irqsave(); - if (!_reports->resize(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 -ENOMEM; + + return OK; } - 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; + + 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); @@ -391,8 +402,9 @@ MB12XX::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) { @@ -453,14 +465,14 @@ MB12XX::measure() uint8_t cmd = MB12XX_TAKE_RANGE_REG; ret = transfer(&cmd, 1, nullptr, 0); - if (OK != ret) - { + if (OK != ret) { perf_count(_comms_errors); log("i2c::transfer returned %d", ret); return ret; } + ret = OK; - + return ret; } @@ -468,32 +480,31 @@ int MB12XX::collect() { int ret = -EIO; - + /* read from the sensor */ uint8_t val[2] = {0, 0}; - + perf_begin(_sample_perf); - + ret = transfer(nullptr, 0, &val[0], 2); - - if (ret < 0) - { + + 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 * 1.0f)/ 100.0f; /* cm to m */ + float si_units = (distance * 1.0f) / 100.0f; /* cm 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.error_count = perf_event_count(_comms_errors); report.distance = si_units; report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; - + /* publish it */ orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); @@ -519,17 +530,19 @@ MB12XX::start() /* schedule a cycle to start things */ work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, 1); - + /* notify about state change */ struct subsystem_info_s info = { true, true, true, - SUBSYSTEM_TYPE_RANGEFINDER}; + 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); } @@ -583,8 +596,9 @@ MB12XX::cycle() } /* measurement phase */ - if (OK != measure()) + if (OK != measure()) { log("measure error"); + } /* next phase is collection */ _collect_phase = true; @@ -635,33 +649,37 @@ start() { int fd; - if (g_dev != nullptr) + if (g_dev != nullptr) { errx(1, "already started"); + } /* create the driver */ g_dev = new MB12XX(MB12XX_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(RANGE_FINDER_DEVICE_PATH, O_RDONLY); - if (fd < 0) + if (fd < 0) { goto fail; + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { goto fail; + } exit(0); fail: - if (g_dev != nullptr) - { + if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; } @@ -674,15 +692,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); } @@ -700,22 +717,25 @@ test() int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "%s open failed (try 'mb12xx start' if the driver is not running", RANGE_FINDER_DEVICE_PATH); + } /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); - if (sz != 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)) + 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++) { @@ -726,14 +746,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("measurement: %0.3f", (double)report.distance); @@ -751,14 +773,17 @@ reset() { int fd = open(RANGE_FINDER_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 +794,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 +812,37 @@ mb12xx_main(int argc, char *argv[]) /* * Start/load the driver. */ - if (!strcmp(argv[1], "start")) + if (!strcmp(argv[1], "start")) { mb12xx::start(); - - /* - * Stop the driver - */ - if (!strcmp(argv[1], "stop")) - mb12xx::stop(); + } + + /* + * Stop the driver + */ + if (!strcmp(argv[1], "stop")) { + mb12xx::stop(); + } /* * Test the driver/device. */ - if (!strcmp(argv[1], "test")) + if (!strcmp(argv[1], "test")) { mb12xx::test(); + } /* * Reset the driver. */ - if (!strcmp(argv[1], "reset")) + if (!strcmp(argv[1], "reset")) { mb12xx::reset(); + } /* * Print driver information. */ - if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) + if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) { mb12xx::info(); + } errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); } From 008efff42ea0fec64173941bd8a746ec0e7ecfa4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 8 Mar 2014 20:00:06 +0100 Subject: [PATCH 147/381] Added support for driver rangefinder --- makefiles/config_px4fmu-v2_default.mk | 1 + src/drivers/sf0x/module.mk | 40 ++ src/drivers/sf0x/sf0x.cpp | 829 ++++++++++++++++++++++++++ 3 files changed, 870 insertions(+) create mode 100644 src/drivers/sf0x/module.mk create mode 100644 src/drivers/sf0x/sf0x.cpp diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 2ad2f63a95..aac5beeb16 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -27,6 +27,7 @@ MODULES += drivers/l3gd20 MODULES += drivers/hmc5883 MODULES += drivers/ms5611 MODULES += drivers/mb12xx +MODULES += drivers/sf0x MODULES += drivers/gps MODULES += drivers/hil MODULES += drivers/hott/hott_telemetry diff --git a/src/drivers/sf0x/module.mk b/src/drivers/sf0x/module.mk new file mode 100644 index 0000000000..dc93a90e78 --- /dev/null +++ b/src/drivers/sf0x/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. +# +############################################################################ + +# +# Makefile to build the Lightware laser range finder driver. +# + +MODULE_COMMAND = sf0x + +SRCS = sf0x.cpp diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp new file mode 100644 index 0000000000..40be85b27b --- /dev/null +++ b/src/drivers/sf0x/sf0x.cpp @@ -0,0 +1,829 @@ +/**************************************************************************** + * + * 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.cpp + * @author Lorenz Meier + * + * Driver for the Lightware SF0x laser rangefinder series + */ + +#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 */ + +/* 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 + +#define SF0X_CONVERSION_INTERVAL 84000 +#define SF0X_TAKE_RANGE_REG 'D' +#define SF02F_MIN_DISTANCE 0.0f +#define SF02F_MAX_DISTANCE 40.0f + +class SF0X : public device::CDev +{ +public: + SF0X(const char* port="/dev/ttyS1"); + virtual ~SF0X(); + + 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 _fd; + + orb_advert_t _range_finder_topic; + + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; + perf_counter_t _buffer_overflows; + + /** + * 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 SF0X_MIN_DISTANCE + * and SF0X_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); + + +}; + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int sf0x_main(int argc, char *argv[]); + +SF0X::SF0X(const char *port) : + CDev("SF0X", RANGE_FINDER_DEVICE_PATH), + _min_distance(SF02F_MIN_DISTANCE), + _max_distance(SF02F_MAX_DISTANCE), + _reports(nullptr), + _sensor_ok(false), + _measure_ticks(0), + _collect_phase(false), + _range_finder_topic(-1), + _sample_perf(perf_alloc(PC_ELAPSED, "sf0x_read")), + _comms_errors(perf_alloc(PC_COUNT, "sf0x_comms_errors")), + _buffer_overflows(perf_alloc(PC_COUNT, "sf0x_buffer_overflows")) +{ + /* open fd */ + _fd = ::open(port, O_RDWR | O_NOCTTY | O_NONBLOCK); + + // enable debug() calls + _debug_enabled = true; + + // work_cancel in the dtor will explode if we don't do this... + memset(&_work, 0, sizeof(_work)); +} + +SF0X::~SF0X() +{ + /* make sure we are truly inactive */ + stop(); + + /* free any existing reports */ + if (_reports != nullptr) { + delete _reports; + } +} + +int +SF0X::init() +{ + int ret = ERROR; + + /* allocate basic report buffers */ + _reports = new RingBuffer(2, sizeof(range_finder_report)); + + if (_reports == nullptr) { + goto out; + } + + /* get a publish handle on the range finder topic */ + struct range_finder_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &zero_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 +SF0X::probe() +{ + return measure(); +} + +void +SF0X::set_minimum_distance(float min) +{ + _min_distance = min; +} + +void +SF0X::set_maximum_distance(float max) +{ + _max_distance = max; +} + +float +SF0X::get_minimum_distance() +{ + return _min_distance; +} + +float +SF0X::get_maximum_distance() +{ + return _max_distance; +} + +int +SF0X::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(SF0X_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(SF0X_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 CDev::ioctl(filp, cmd, arg); + } +} + +ssize_t +SF0X::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(SF0X_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 +SF0X::measure() +{ + int ret; + + /* + * Send the command to begin a measurement. + */ + char cmd = SF0X_TAKE_RANGE_REG; + ret = ::write(_fd, &cmd, 1); + + if (OK != sizeof(cmd)) { + perf_count(_comms_errors); + log("serial transfer returned %d", ret); + return ret; + } + + ret = OK; + + return ret; +} + +int +SF0X::collect() +{ + int ret = -EIO; + + /* read from the sensor */ + float val; + + perf_begin(_sample_perf); + + char buf[16]; + ret = ::read(_fd, buf, sizeof(buf)); + printf("ret: %d, val: %s", ret, buf); + + if (ret < 0) { + log("error reading from sensor: %d", ret); + perf_count(_comms_errors); + perf_end(_sample_perf); + return ret; + } + + float si_units = 0.0f; + 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 = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; + + /* publish it */ + 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 +SF0X::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)&SF0X::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 +SF0X::stop() +{ + work_cancel(HPWORK, &_work); +} + +void +SF0X::cycle_trampoline(void *arg) +{ + SF0X *dev = (SF0X *)arg; + + dev->cycle(); +} + +void +SF0X::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(SF0X_CONVERSION_INTERVAL)) { + + /* schedule a fresh cycle call when we are ready to measure again */ + work_queue(HPWORK, + &_work, + (worker_t)&SF0X::cycle_trampoline, + this, + _measure_ticks - USEC2TICK(SF0X_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)&SF0X::cycle_trampoline, + this, + USEC2TICK(SF0X_CONVERSION_INTERVAL)); +} + +void +SF0X::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 sf0x +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +const int ERROR = -1; + +SF0X *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 SF0X(); + + 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(RANGE_FINDER_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(RANGE_FINDER_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + err(1, "%s open failed (try 'sf0x start' if the driver is not running", RANGE_FINDER_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 5x and report each value */ + for (unsigned i = 0; i < 5; 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("measurement: %0.3f", (double)report.distance); + warnx("time: %lld", report.timestamp); + } + + errx(0, "PASS"); +} + +/** + * Reset the driver. + */ +void +reset() +{ + int fd = open(RANGE_FINDER_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 +sf0x_main(int argc, char *argv[]) +{ + /* + * Start/load the driver. + */ + if (!strcmp(argv[1], "start")) { + sf0x::start(); + } + + /* + * Stop the driver + */ + if (!strcmp(argv[1], "stop")) { + sf0x::stop(); + } + + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) { + sf0x::test(); + } + + /* + * Reset the driver. + */ + if (!strcmp(argv[1], "reset")) { + sf0x::reset(); + } + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) { + sf0x::info(); + } + + errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); +} From 7cc33b9ca6c0facd6e2f38f0bc515e9769e13073 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 8 Mar 2014 21:06:29 +0100 Subject: [PATCH 148/381] Complete output parsing, pending testing --- src/drivers/sf0x/sf0x.cpp | 25 +++++++++++++------------ 1 file changed, 13 insertions(+), 12 deletions(-) diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 40be85b27b..d3043a26bf 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -33,7 +33,8 @@ /** * @file sf0x.cpp - * @author Lorenz Meier + * @author Lorenz Meier + * @author Greg Hulands * * Driver for the Lightware SF0x laser rangefinder series */ @@ -459,25 +460,26 @@ SF0X::measure() int SF0X::collect() { - int ret = -EIO; - - /* read from the sensor */ - float val; + int ret; perf_begin(_sample_perf); char buf[16]; + /* read from the sensor (uart buffer) */ ret = ::read(_fd, buf, sizeof(buf)); - printf("ret: %d, val: %s", ret, buf); - if (ret < 0) { + if (ret < 1) { log("error reading from sensor: %d", ret); perf_count(_comms_errors); perf_end(_sample_perf); return ret; } - float si_units = 0.0f; + printf("ret: %d, val (str): %s\n", ret, buf); + char* end; + float si_units = strtod(buf,&end); + printf("val (float): %8.4f\n", si_units); + struct range_finder_report report; /* this should be fairly close to the end of the measurement, so the best approximation of the time */ @@ -538,7 +540,7 @@ SF0X::stop() void SF0X::cycle_trampoline(void *arg) { - SF0X *dev = (SF0X *)arg; + SF0X *dev = static_cast(arg); dev->cycle(); } @@ -598,7 +600,7 @@ SF0X::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); + printf("poll interval: %d ticks\n", _measure_ticks); _reports->print_info("report queue"); } @@ -694,7 +696,6 @@ test() { struct range_finder_report report; ssize_t sz; - int ret; int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); @@ -725,7 +726,7 @@ test() /* wait for data to be ready */ fds.fd = fd; fds.events = POLLIN; - ret = poll(&fds, 1, 2000); + int ret = poll(&fds, 1, 2000); if (ret != 1) { errx(1, "timed out waiting for sensor data"); From 171af566f716a99b078eaa4ef0f90405091e8d19 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 8 Mar 2014 21:45:22 +0100 Subject: [PATCH 149/381] Allow to set the UART via start argument, cleanups --- src/drivers/sf0x/sf0x.cpp | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index d3043a26bf..f25856689b 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -87,11 +87,12 @@ static const int ERROR = -1; #define SF0X_TAKE_RANGE_REG 'D' #define SF02F_MIN_DISTANCE 0.0f #define SF02F_MAX_DISTANCE 40.0f +#define SF0X_DEFAULT_PORT "/dev/ttyS2" class SF0X : public device::CDev { public: - SF0X(const char* port="/dev/ttyS1"); + SF0X(const char* port=SF0X_DEFAULT_PORT); virtual ~SF0X(); virtual int init(); @@ -628,7 +629,7 @@ void info(); * Start the driver. */ void -start() +start(const char* port) { int fd; @@ -637,7 +638,7 @@ start() } /* create the driver */ - g_dev = new SF0X(); + g_dev = new SF0X(port); if (g_dev == nullptr) { goto fail; @@ -795,7 +796,11 @@ sf0x_main(int argc, char *argv[]) * Start/load the driver. */ if (!strcmp(argv[1], "start")) { - sf0x::start(); + if (argc > 2) { + sf0x::start(argv[2]); + } else { + sf0x::start(SF0X_DEFAULT_PORT); + } } /* From d69d895f0240a0dda5fda2f8f046179b069439a4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 9 Mar 2014 00:46:13 +0100 Subject: [PATCH 150/381] Add missing CDev init step --- src/drivers/sf0x/sf0x.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index f25856689b..5dd1f59dea 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -209,6 +209,10 @@ SF0X::init() { int ret = ERROR; + /* do regular cdev init */ + if (CDev::init() != OK) + goto out; + /* allocate basic report buffers */ _reports = new RingBuffer(2, sizeof(range_finder_report)); @@ -447,7 +451,7 @@ SF0X::measure() char cmd = SF0X_TAKE_RANGE_REG; ret = ::write(_fd, &cmd, 1); - if (OK != sizeof(cmd)) { + if (ret != sizeof(cmd)) { perf_count(_comms_errors); log("serial transfer returned %d", ret); return ret; From d2905b8d8eba3a3b1e4258343e7a384071c55073 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 9 Mar 2014 11:18:02 +0100 Subject: [PATCH 151/381] SF02/F operational, but not cleaned up / optimized - good enough for first logging --- src/drivers/sf0x/sf0x.cpp | 133 +++++++++++++++++++++++++++++--------- 1 file changed, 104 insertions(+), 29 deletions(-) diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 5dd1f59dea..7b4b49813d 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -53,6 +53,7 @@ #include #include #include +#include #include #include @@ -83,8 +84,8 @@ static const int ERROR = -1; # error This requires CONFIG_SCHED_WORKQUEUE. #endif -#define SF0X_CONVERSION_INTERVAL 84000 -#define SF0X_TAKE_RANGE_REG 'D' +#define SF0X_CONVERSION_INTERVAL 90000 +#define SF0X_TAKE_RANGE_REG 'd' #define SF02F_MIN_DISTANCE 0.0f #define SF02F_MAX_DISTANCE 40.0f #define SF0X_DEFAULT_PORT "/dev/ttyS2" @@ -186,8 +187,39 @@ SF0X::SF0X(const char *port) : /* open fd */ _fd = ::open(port, O_RDWR | O_NOCTTY | O_NONBLOCK); - // enable debug() calls - _debug_enabled = true; + if (_fd < 0) { + warnx("FAIL: laser fd"); + } + + struct termios uart_config; + + int termios_state; + + /* fill the struct for the new configuration */ + tcgetattr(_fd, &uart_config); + + /* clear ONLCR flag (which appends a CR for every LF) */ + uart_config.c_oflag &= ~ONLCR; + /* no parity, one stop bit */ + uart_config.c_cflag &= ~(CSTOPB | PARENB); + + unsigned speed = B9600; + + /* set baud rate */ + if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) { + warnx("ERR CFG: %d ISPD", termios_state); + } + + if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) { + warnx("ERR CFG: %d OSPD\n", termios_state); + } + + if ((termios_state = tcsetattr(_fd, TCSANOW, &uart_config)) < 0) { + warnx("ERR baud %d ATTR", termios_state); + } + + // disable debug() calls + _debug_enabled = false; // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); @@ -208,6 +240,7 @@ int SF0X::init() { int ret = ERROR; + unsigned i = 0; /* do regular cdev init */ if (CDev::init() != OK) @@ -217,6 +250,7 @@ SF0X::init() _reports = new RingBuffer(2, sizeof(range_finder_report)); if (_reports == nullptr) { + warnx("mem err"); goto out; } @@ -226,12 +260,33 @@ SF0X::init() _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &zero_report); if (_range_finder_topic < 0) { - debug("failed to create sensor_range_finder object. Did you start uOrb?"); + warnx("advert err"); } - ret = OK; - /* sensor is ok, but we don't really know if it is within range */ - _sensor_ok = true; + /* attempt to get a measurement 5 times */ + while (ret != OK && i < 5) { + + if (measure()) { + ret = ERROR; + _sensor_ok = false; + } + + usleep(100000); + if (collect()) { + ret = ERROR; + _sensor_ok = false; + } else { + ret = OK; + /* sensor is ok, but we don't really know if it is within range */ + _sensor_ok = true; + } + + i++; + } + + /* close the fd */ + ::close(_fd); + _fd = -1; out: return ret; } @@ -453,7 +508,7 @@ SF0X::measure() if (ret != sizeof(cmd)) { perf_count(_comms_errors); - log("serial transfer returned %d", ret); + log("write fail %d", ret); return ret; } @@ -473,17 +528,30 @@ SF0X::collect() /* read from the sensor (uart buffer) */ ret = ::read(_fd, buf, sizeof(buf)); - if (ret < 1) { - log("error reading from sensor: %d", ret); + if (ret < 3) { + log("read err: %d", ret); perf_count(_comms_errors); perf_end(_sample_perf); return ret; } - printf("ret: %d, val (str): %s\n", ret, buf); + if (buf[ret - 2] != '\r' || buf[ret - 1] != '\n') { + /* incomplete read */ + return -1; + } + char* end; - float si_units = strtod(buf,&end); - printf("val (float): %8.4f\n", si_units); + float si_units; + bool valid; + + if (buf[0] == '-' && buf[1] == '-' && buf[2] == '.') { + si_units = -1.0f; + valid = false; + } else { + si_units = strtod(buf,&end); + valid = true; + } + debug("val (float): %8.4f, raw: %s\n", si_units, buf); struct range_finder_report report; @@ -491,7 +559,7 @@ SF0X::collect() report.timestamp = hrt_absolute_time(); report.error_count = perf_event_count(_comms_errors); report.distance = si_units; - report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; + report.valid = valid && (si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0); /* publish it */ orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); @@ -519,21 +587,21 @@ SF0X::start() /* schedule a cycle to start things */ work_queue(HPWORK, &_work, (worker_t)&SF0X::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; + // /* 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); + // if (pub > 0) { + // orb_publish(ORB_ID(subsystem_info), pub, &info); - } else { - pub = orb_advertise(ORB_ID(subsystem_info), &info); - } + // } else { + // pub = orb_advertise(ORB_ID(subsystem_info), &info); + // } } void @@ -553,6 +621,12 @@ SF0X::cycle_trampoline(void *arg) void SF0X::cycle() { + /* fds initialized? */ + if (_fd < 0) { + /* open fd */ + _fd = ::open(SF0X_DEFAULT_PORT, O_RDWR | O_NOCTTY | O_NONBLOCK); + } + /* collection phase? */ if (_collect_phase) { @@ -653,9 +727,10 @@ start(const char* port) } /* set the poll rate to default, starts automatic data collection */ - fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); + fd = open(RANGE_FINDER_DEVICE_PATH, 0); if (fd < 0) { + warnx("device open fail"); goto fail; } From 8267bdf4a5aa857db0f970da750d782f1d8ec369 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 9 Mar 2014 15:50:41 +0100 Subject: [PATCH 152/381] fw_att_control: airspeed is now used correctly --- src/modules/fw_att_control/fw_att_control_main.cpp | 14 +++++--------- 1 file changed, 5 insertions(+), 9 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 17b1028f9a..c59a396c27 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -230,7 +230,7 @@ private: /** * Check for airspeed updates. */ - bool vehicle_airspeed_poll(); + void vehicle_airspeed_poll(); /** * Check for accel updates. @@ -452,7 +452,7 @@ FixedwingAttitudeControl::vehicle_manual_poll() } } -bool +void FixedwingAttitudeControl::vehicle_airspeed_poll() { /* check if there is a new position */ @@ -462,10 +462,7 @@ FixedwingAttitudeControl::vehicle_airspeed_poll() if (airspeed_updated) { orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); // warnx("airspeed poll: ind: %.4f, true: %.4f", _airspeed.indicated_airspeed_m_s, _airspeed.true_airspeed_m_s); - return true; } - - return false; } void @@ -539,7 +536,7 @@ FixedwingAttitudeControl::task_main() parameters_update(); /* get an initial update for all sensor and status data */ - (void)vehicle_airspeed_poll(); + vehicle_airspeed_poll(); vehicle_setpoint_poll(); vehicle_accel_poll(); vehicle_control_mode_poll(); @@ -596,7 +593,7 @@ FixedwingAttitudeControl::task_main() /* load local copies */ orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); - _airspeed_valid = vehicle_airspeed_poll(); + vehicle_airspeed_poll(); vehicle_setpoint_poll(); @@ -636,8 +633,7 @@ FixedwingAttitudeControl::task_main() float airspeed; /* if airspeed is smaller than min, the sensor is not giving good readings */ - if (!_airspeed_valid || - (_airspeed.indicated_airspeed_m_s < 0.5f * _parameters.airspeed_min) || + if ((_airspeed.indicated_airspeed_m_s < 0.5f * _parameters.airspeed_min) || !isfinite(_airspeed.indicated_airspeed_m_s)) { airspeed = _parameters.airspeed_trim; From 1c488a95da934817999a0a40ccb87e07bf5e0307 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 10 Mar 2014 18:46:22 +0100 Subject: [PATCH 153/381] Added laser range finder logging, needs testing --- src/drivers/sf0x/sf0x.cpp | 32 +++++++++++++++++++++++++------- 1 file changed, 25 insertions(+), 7 deletions(-) diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 7b4b49813d..685b96646c 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -118,6 +118,9 @@ private: int _measure_ticks; bool _collect_phase; int _fd; + char _linebuf[10]; + unsigned _linebuf_index; + hrt_abstime _last_read; orb_advert_t _range_finder_topic; @@ -179,6 +182,9 @@ SF0X::SF0X(const char *port) : _sensor_ok(false), _measure_ticks(0), _collect_phase(false), + _fd(-1), + _linebuf_index(0), + _last_read(0), _range_finder_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "sf0x_read")), _comms_errors(perf_alloc(PC_COUNT, "sf0x_comms_errors")), @@ -187,6 +193,10 @@ SF0X::SF0X(const char *port) : /* open fd */ _fd = ::open(port, O_RDWR | O_NOCTTY | O_NONBLOCK); + /* tell it to stop auto-triggering */ + char stop_auto = ' '; + ::write(_fd, &stop_auto, 1); + if (_fd < 0) { warnx("FAIL: laser fd"); } @@ -219,7 +229,7 @@ SF0X::SF0X(const char *port) : } // disable debug() calls - _debug_enabled = false; + _debug_enabled = true; // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); @@ -524,18 +534,26 @@ SF0X::collect() perf_begin(_sample_perf); - char buf[16]; - /* read from the sensor (uart buffer) */ - ret = ::read(_fd, buf, sizeof(buf)); + /* clear buffer if last read was too long ago */ + if (hrt_elapsed(&_last_read) > (SF0X_CONVERSION_INTERVAL * 2)) { + _linebuf_index = 0; + } - if (ret < 3) { + /* read from the sensor (uart buffer) */ + ret = ::read(_fd, _linebuf[_linebuf_index], sizeof(_linebuf) - _linebuf_index); + + if (ret < 1) { log("read err: %d", ret); perf_count(_comms_errors); perf_end(_sample_perf); return ret; } - if (buf[ret - 2] != '\r' || buf[ret - 1] != '\n') { + _linebuf_index += ret; + + _last_read = hrt_absolute_time(); + + if (_linebuf[_linebuf_index - 2] != '\r' || _linebuf[_linebuf_index - 1] != '\n') { /* incomplete read */ return -1; } @@ -544,7 +562,7 @@ SF0X::collect() float si_units; bool valid; - if (buf[0] == '-' && buf[1] == '-' && buf[2] == '.') { + if (_linebuf[0] == '-' && _linebuf[1] == '-' && _linebuf[2] == '.') { si_units = -1.0f; valid = false; } else { From 3f3abebf688c97f87fd6d6016aac5d234d76893f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 10 Mar 2014 18:47:31 +0100 Subject: [PATCH 154/381] Add range finder to logging --- src/modules/sdlog2/sdlog2.c | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 1365d9e705..53f07651fe 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -58,6 +58,8 @@ #include #include +#include + #include #include #include @@ -759,6 +761,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_global_velocity_setpoint_s global_vel_sp; struct battery_status_s battery; struct telemetry_status_s telemetry; + struct range_finder_report range_finder; } buf; memset(&buf, 0, sizeof(buf)); @@ -785,6 +788,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int global_vel_sp_sub; int battery_sub; int telemetry_sub; + int range_finder_sub; } subs; /* log message buffer: header + body */ @@ -955,6 +959,12 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; + /* --- RANGE FINDER --- */ + subs.range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder)); + fds[fdsc_count].fd = subs.range_finder_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + /* WARNING: If you get the error message below, * then the number of registered messages (fdsc) * differs from the number of messages in the above list. @@ -1370,6 +1380,16 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(TELE); } + /* --- BOTTOM DISTANCE --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(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); + } + /* 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 */ From 30a57421f8027cb9400bea71cb3b9c2b4f5094ec Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 10 Mar 2014 18:48:07 +0100 Subject: [PATCH 155/381] Dropped default gains --- ROMFS/px4fmu_common/init.d/10016_3dr_iris | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris index d691a6f2e6..f11aa704eb 100644 --- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -10,11 +10,11 @@ sh /etc/init.d/rc.mc_defaults if [ $DO_AUTOCONFIG == yes ] then # TODO tune roll/pitch separately - param set MC_ROLL_P 9.0 + param set MC_ROLL_P 7.0 param set MC_ROLLRATE_P 0.13 param set MC_ROLLRATE_I 0.0 param set MC_ROLLRATE_D 0.004 - param set MC_PITCH_P 9.0 + param set MC_PITCH_P 7.0 param set MC_PITCHRATE_P 0.13 param set MC_PITCHRATE_I 0.0 param set MC_PITCHRATE_D 0.004 From b3839afbbc904fe0009610dae57a4626b3306fb6 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 11 Mar 2014 11:12:39 +0400 Subject: [PATCH 156/381] mavlink: channel ID allocation fixed --- src/modules/mavlink/mavlink_main.cpp | 49 ++++++++++++++++++++++-- src/modules/mavlink/mavlink_main.h | 6 ++- src/modules/mavlink/mavlink_receiver.cpp | 4 +- 3 files changed, 53 insertions(+), 6 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 788fe5732d..4bc1055d1e 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -159,8 +159,9 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length static void usage(void); -Mavlink::Mavlink() : +Mavlink::Mavlink(int instance_id) : next(nullptr), + _instance_id(instance_id), _device_name(DEFAULT_DEVICE_NAME), _task_should_exit(false), _mavlink_fd(-1), @@ -179,6 +180,40 @@ Mavlink::Mavlink() : { _wpm = &_wpm_s; fops.ioctl = (int (*)(file *, int, long unsigned int))&mavlink_dev_ioctl; + + /* set channel according to instance id */ + switch (_instance_id) { + case 0: + _channel = MAVLINK_COMM_0; + break; + case 1: + _channel = MAVLINK_COMM_1; + break; + case 2: + _channel = MAVLINK_COMM_2; + break; + case 3: + _channel = MAVLINK_COMM_3; + break; +#ifdef MAVLINK_COMM_4 + case 4: + _channel = MAVLINK_COMM_4; + break; +#endif +#ifdef MAVLINK_COMM_5 + case 5: + _channel = MAVLINK_COMM_5; + break; +#endif +#ifdef MAVLINK_COMM_6 + case 6: + _channel = MAVLINK_COMM_6; + break; +#endif + default: + errx(1, "instance ID is out of range"); + break; + } } Mavlink::~Mavlink() @@ -226,7 +261,9 @@ Mavlink::instance_count() Mavlink * Mavlink::new_instance() { - Mavlink *inst = new Mavlink(); + int id = Mavlink::instance_count(); + + Mavlink *inst = new Mavlink(id); Mavlink *next = ::_mavlink_instances; /* create the first instance at _head */ @@ -353,6 +390,12 @@ Mavlink::get_uart_fd() return _uart_fd; } +int +Mavlink::get_instance_id() +{ + return _instance_id; +} + mavlink_channel_t Mavlink::get_channel() { @@ -1881,7 +1924,7 @@ int Mavlink::start(int argc, char *argv[]) { // Instantiate thread - char buf[32]; + char buf[24]; sprintf(buf, "mavlink_if%d", Mavlink::instance_count()); task_spawn_cmd(buf, diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index b52c12796d..c606da5045 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -105,7 +105,7 @@ public: /** * Constructor */ - Mavlink(); + Mavlink(int instance_id); /** * Destructor, also kills the mavlinks task. @@ -182,6 +182,8 @@ public: MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic); + int get_instance_id(); + mavlink_channel_t get_channel(); bool _task_should_exit; /**< if true, mavlink task should exit */ @@ -190,6 +192,8 @@ protected: Mavlink *next; private: + int _instance_id; + int _mavlink_fd; bool _task_running; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index ef1a747da0..c9fcc4cd84 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -877,8 +877,8 @@ MavlinkReceiver::receive_thread(void *arg) mavlink_message_t msg; /* set thread name */ - char thread_name[18]; - sprintf(thread_name, "mavlink_uart_rcv_%d", _mavlink->get_channel()); + char thread_name[24]; + sprintf(thread_name, "mavlink_rcv_if%d", _mavlink->get_instance_id()); prctl(PR_SET_NAME, thread_name, getpid()); _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); From 290b07920c94df09415ccc1c44850c57d0750fc3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 11 Mar 2014 10:52:26 +0100 Subject: [PATCH 157/381] Fixed missing reset of poll rate after test exit in ultrasound driver --- src/drivers/mb12xx/mb12xx.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index 7693df295c..5adb8cf698 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -200,7 +200,7 @@ MB12XX::MB12XX(int bus, int address) : _buffer_overflows(perf_alloc(PC_COUNT, "mb12xx_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)); @@ -762,6 +762,11 @@ test() 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"); } From 7851472ae9f19341a1a8905eed63c2c907f16385 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 11 Mar 2014 10:52:53 +0100 Subject: [PATCH 158/381] Laser range finder ready to roll, logging tested --- src/drivers/sf0x/sf0x.cpp | 68 ++++++++++++++++++++++++++++++--------- 1 file changed, 52 insertions(+), 16 deletions(-) diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 685b96646c..dace02cf8d 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -84,7 +84,7 @@ static const int ERROR = -1; # error This requires CONFIG_SCHED_WORKQUEUE. #endif -#define SF0X_CONVERSION_INTERVAL 90000 +#define SF0X_CONVERSION_INTERVAL 83334 #define SF0X_TAKE_RANGE_REG 'd' #define SF02F_MIN_DISTANCE 0.0f #define SF02F_MAX_DISTANCE 40.0f @@ -193,14 +193,16 @@ SF0X::SF0X(const char *port) : /* open fd */ _fd = ::open(port, O_RDWR | O_NOCTTY | O_NONBLOCK); - /* tell it to stop auto-triggering */ - char stop_auto = ' '; - ::write(_fd, &stop_auto, 1); - if (_fd < 0) { 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; @@ -229,7 +231,7 @@ SF0X::SF0X(const char *port) : } // disable 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)); @@ -535,41 +537,58 @@ SF0X::collect() perf_begin(_sample_perf); /* clear buffer if last read was too long ago */ - if (hrt_elapsed(&_last_read) > (SF0X_CONVERSION_INTERVAL * 2)) { + uint64_t read_elapsed = hrt_elapsed_time(&_last_read); + if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) { _linebuf_index = 0; } /* read from the sensor (uart buffer) */ - ret = ::read(_fd, _linebuf[_linebuf_index], sizeof(_linebuf) - _linebuf_index); + ret = ::read(_fd, &_linebuf[_linebuf_index], sizeof(_linebuf) - _linebuf_index); - if (ret < 1) { - log("read err: %d", ret); + if (ret < 0) { + _linebuf[sizeof(_linebuf) - 1] = '\0'; + debug("read err: %d lbi: %d buf: %s", ret, (int)_linebuf_index, _linebuf); perf_count(_comms_errors); perf_end(_sample_perf); - return ret; + + /* only throw an error if we time out */ + if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) { + return ret; + } else { + return -EAGAIN; + } } _linebuf_index += ret; + if (_linebuf_index >= sizeof(_linebuf)) { + _linebuf_index = 0; + } _last_read = hrt_absolute_time(); if (_linebuf[_linebuf_index - 2] != '\r' || _linebuf[_linebuf_index - 1] != '\n') { - /* incomplete read */ - return -1; + /* incomplete read, reschedule ourselves */ + return -EAGAIN; } char* end; float si_units; bool valid; + /* enforce line ending */ + _linebuf[sizeof(_linebuf) - 1] = '\0'; + if (_linebuf[0] == '-' && _linebuf[1] == '-' && _linebuf[2] == '.') { si_units = -1.0f; valid = false; } else { - si_units = strtod(buf,&end); + si_units = strtod(_linebuf,&end); valid = true; } - debug("val (float): %8.4f, raw: %s\n", si_units, buf); + debug("val (float): %8.4f, raw: %s\n", si_units, _linebuf); + + /* done with this chunk, resetting */ + _linebuf_index = 0; struct range_finder_report report; @@ -649,7 +668,19 @@ SF0X::cycle() if (_collect_phase) { /* perform collection */ - if (OK != collect()) { + int collect_ret = collect(); + + if (collect_ret == -EAGAIN) { + /* reschedule to grab the missing bits, time to transmit 10 bytes @9600 bps */ + work_queue(HPWORK, + &_work, + (worker_t)&SF0X::cycle_trampoline, + this, + USEC2TICK(1100)); + return; + } + + if (OK != collect_ret) { log("collection error"); /* restart the measurement state machine */ start(); @@ -842,6 +873,11 @@ test() 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"); } From b7662537df656f8546acecc9cca6ab2ac40714bf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 11 Mar 2014 10:54:48 +0100 Subject: [PATCH 159/381] Fix code style of laser rangefinder --- src/drivers/sf0x/sf0x.cpp | 21 +++++++++++++++------ 1 file changed, 15 insertions(+), 6 deletions(-) diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index dace02cf8d..70cd1ab1e5 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -93,7 +93,7 @@ static const int ERROR = -1; class SF0X : public device::CDev { public: - SF0X(const char* port=SF0X_DEFAULT_PORT); + SF0X(const char *port = SF0X_DEFAULT_PORT); virtual ~SF0X(); virtual int init(); @@ -255,8 +255,9 @@ SF0X::init() unsigned i = 0; /* do regular cdev init */ - if (CDev::init() != OK) + if (CDev::init() != OK) { goto out; + } /* allocate basic report buffers */ _reports = new RingBuffer(2, sizeof(range_finder_report)); @@ -284,9 +285,11 @@ SF0X::init() } usleep(100000); + if (collect()) { ret = ERROR; _sensor_ok = false; + } else { ret = OK; /* sensor is ok, but we don't really know if it is within range */ @@ -538,6 +541,7 @@ SF0X::collect() /* clear buffer if last read was too long ago */ uint64_t read_elapsed = hrt_elapsed_time(&_last_read); + if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) { _linebuf_index = 0; } @@ -550,16 +554,18 @@ SF0X::collect() debug("read err: %d lbi: %d buf: %s", ret, (int)_linebuf_index, _linebuf); perf_count(_comms_errors); perf_end(_sample_perf); - + /* only throw an error if we time out */ if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) { return ret; + } else { return -EAGAIN; } } _linebuf_index += ret; + if (_linebuf_index >= sizeof(_linebuf)) { _linebuf_index = 0; } @@ -571,7 +577,7 @@ SF0X::collect() return -EAGAIN; } - char* end; + char *end; float si_units; bool valid; @@ -581,10 +587,12 @@ SF0X::collect() if (_linebuf[0] == '-' && _linebuf[1] == '-' && _linebuf[2] == '.') { si_units = -1.0f; valid = false; + } else { - si_units = strtod(_linebuf,&end); + si_units = strtod(_linebuf, &end); valid = true; } + debug("val (float): %8.4f, raw: %s\n", si_units, _linebuf); /* done with this chunk, resetting */ @@ -756,7 +764,7 @@ void info(); * Start the driver. */ void -start(const char* port) +start(const char *port) { int fd; @@ -931,6 +939,7 @@ sf0x_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { if (argc > 2) { sf0x::start(argv[2]); + } else { sf0x::start(SF0X_DEFAULT_PORT); } From 2db140bcabaafcccc1ed2ce070d7a24b4133eb57 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 11 Mar 2014 10:59:11 +0100 Subject: [PATCH 160/381] fw_att_control: delete the unused flag --- src/modules/fw_att_control/fw_att_control_main.cpp | 8 +------- 1 file changed, 1 insertion(+), 7 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 c59a396c27..ec91f4d8ca 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -136,7 +136,6 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ bool _setpoint_valid; /**< flag if the position control setpoint is valid */ - bool _airspeed_valid; /**< flag if the airspeed measurement is valid */ struct { float tconst; @@ -293,8 +292,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), /* states */ - _setpoint_valid(false), - _airspeed_valid(false) + _setpoint_valid(false) { /* safely initialize structs */ _att = {0}; @@ -753,10 +751,6 @@ FixedwingAttitudeControl::task_main() warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", roll_sp, pitch_sp); } - // warnx("aspd: %s: %6.2f, aspd scaling: %6.2f, controls: %5.2f %5.2f %5.2f %5.2f", (_airspeed_valid) ? "valid" : "unknown", - // airspeed, airspeed_scaling, _actuators.control[0], _actuators.control[1], - // _actuators.control[2], _actuators.control[3]); - /* * Lazily publish the rate setpoint (for analysis, the actuators are published below) * only once available From 2cd29b7c8ce7fc26af6f1a3957e243a18bc09d5b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 11 Mar 2014 11:07:48 +0100 Subject: [PATCH 161/381] fw_att_control: proper struct initialization --- .../fw_att_control/fw_att_control_main.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 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 ec91f4d8ca..fa0f5962f6 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -295,15 +295,15 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _setpoint_valid(false) { /* safely initialize structs */ - _att = {0}; - _accel = {0}; - _att_sp = {0}; - _manual = {0}; - _airspeed = {0}; - _vcontrol_mode = {0}; - _actuators = {0}; - _actuators_airframe = {0}; - _global_pos = {0}; + _att = {}; + _accel = {}; + _att_sp = {}; + _manual = {}; + _airspeed = {}; + _vcontrol_mode = {}; + _actuators = {}; + _actuators_airframe = {}; + _global_pos = {}; _parameter_handles.tconst = param_find("FW_ATT_TC"); From 2f06f15240a9d03a282212538283e53e9bcc4b9f Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 11 Mar 2014 11:09:49 +0100 Subject: [PATCH 162/381] fw_att_control: whitespace only --- .../fw_att_control/fw_att_control_main.cpp | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 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 fa0f5962f6..deafd7f86f 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -108,14 +108,14 @@ private: bool _task_should_exit; /**< if true, sensor task should exit */ int _control_task; /**< task handle for sensor task */ - int _att_sub; /**< vehicle attitude subscription */ - int _accel_sub; /**< accelerometer subscription */ + int _att_sub; /**< vehicle attitude subscription */ + int _accel_sub; /**< accelerometer subscription */ int _att_sp_sub; /**< vehicle attitude setpoint */ int _attitude_sub; /**< raw rc channels data subscription */ int _airspeed_sub; /**< airspeed subscription */ int _vcontrol_mode_sub; /**< vehicle status subscription */ - int _params_sub; /**< notification of parameter updates */ - int _manual_sub; /**< notification of manual control updates */ + int _params_sub; /**< notification of parameter updates */ + int _manual_sub; /**< notification of manual control updates */ int _global_pos_sub; /**< global position subscription */ orb_advert_t _rate_sp_pub; /**< rate setpoint publication */ @@ -123,15 +123,15 @@ private: orb_advert_t _actuators_0_pub; /**< actuator control group 0 setpoint */ orb_advert_t _actuators_1_pub; /**< actuator control group 1 setpoint (Airframe) */ - struct vehicle_attitude_s _att; /**< vehicle attitude */ - struct accel_report _accel; /**< body frame accelerations */ + struct vehicle_attitude_s _att; /**< vehicle attitude */ + struct accel_report _accel; /**< body frame accelerations */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ struct manual_control_setpoint_s _manual; /**< r/c channel data */ - struct airspeed_s _airspeed; /**< airspeed */ - struct vehicle_control_mode_s _vcontrol_mode; /**< vehicle control mode */ + struct airspeed_s _airspeed; /**< airspeed */ + struct vehicle_control_mode_s _vcontrol_mode; /**< vehicle control mode */ struct actuator_controls_s _actuators; /**< actuator control inputs */ - struct actuator_controls_s _actuators_airframe; /**< actuator control inputs */ - struct vehicle_global_position_s _global_pos; /**< global position */ + struct actuator_controls_s _actuators_airframe; /**< actuator control inputs */ + struct vehicle_global_position_s _global_pos; /**< global position */ perf_counter_t _loop_perf; /**< loop performance counter */ From 0ad38a73231877ec142946cb4c380c74a1633e63 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 11 Mar 2014 15:34:27 +0400 Subject: [PATCH 163/381] mavlink: task stop fixes, WIP --- src/modules/mavlink/mavlink_main.cpp | 67 +++++++++++----------------- src/modules/mavlink/mavlink_main.h | 2 +- 2 files changed, 28 insertions(+), 41 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 4bc1055d1e..2dfc67fd4d 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -159,9 +159,8 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length static void usage(void); -Mavlink::Mavlink(int instance_id) : +Mavlink::Mavlink() : next(nullptr), - _instance_id(instance_id), _device_name(DEFAULT_DEVICE_NAME), _task_should_exit(false), _mavlink_fd(-1), @@ -181,6 +180,8 @@ Mavlink::Mavlink(int instance_id) : _wpm = &_wpm_s; fops.ioctl = (int (*)(file *, int, long unsigned int))&mavlink_dev_ioctl; + _instance_id = Mavlink::instance_count(); + /* set channel according to instance id */ switch (_instance_id) { case 0: @@ -214,10 +215,15 @@ Mavlink::Mavlink(int instance_id) : errx(1, "instance ID is out of range"); break; } + + LL_APPEND(_mavlink_instances, this); } Mavlink::~Mavlink() { + warnx("DESTRUCTOR"); + usleep(1000000); + if (_task_running) { /* task wakes up every 10ms or so at the longest */ _task_should_exit = true; @@ -237,6 +243,7 @@ Mavlink::~Mavlink() } } while (_task_running); } + LL_DELETE(_mavlink_instances, this); } void @@ -258,31 +265,6 @@ Mavlink::instance_count() return inst_index; } -Mavlink * -Mavlink::new_instance() -{ - int id = Mavlink::instance_count(); - - Mavlink *inst = new Mavlink(id); - Mavlink *next = ::_mavlink_instances; - - /* create the first instance at _head */ - if (::_mavlink_instances == nullptr) { - ::_mavlink_instances = inst; - /* afterwards follow the next and append the instance */ - - } else { - while (next->next != nullptr) { - next = next->next; - } - - /* now parent has a null pointer, fill it */ - next->next = inst; - } - - return inst; -} - Mavlink * Mavlink::get_instance(unsigned instance) { @@ -342,13 +324,8 @@ Mavlink::destroy_all_instances() return ERROR; } } - - delete inst_to_del; } - /* reset head */ - ::_mavlink_instances = nullptr; - printf("\n"); warnx("all instances stopped"); return OK; @@ -1659,7 +1636,7 @@ Mavlink::task_main(int argc, char *argv[]) if (err_flag) { usage(); - exit(1); + return ERROR; } if (_datarate == 0) { @@ -1672,7 +1649,8 @@ Mavlink::task_main(int argc, char *argv[]) } if (Mavlink::instance_exists(_device_name, this)) { - errx(1, "mavlink instance for %s already running", _device_name); + warnx("mavlink instance for %s already running", _device_name); + return ERROR; } /* inform about mode */ @@ -1727,7 +1705,8 @@ Mavlink::task_main(int argc, char *argv[]) _uart_fd = mavlink_open_uart(_baudrate, _device_name, &uart_config_original, &usb_uart); if (_uart_fd < 0) { - err(1, "could not open %s", _device_name); + warn("could not open %s", _device_name); + return ERROR; } /* create the device node that's used for sending text log messages, etc. */ @@ -1906,18 +1885,23 @@ Mavlink::task_main(int argc, char *argv[]) mavlink_logbuffer_destroy(&_logbuffer); warnx("exiting"); - _task_running = false; - _exit(0); + + return OK; } int Mavlink::start_helper(int argc, char *argv[]) { /* create the instance in task context */ - Mavlink *instance = Mavlink::new_instance(); + Mavlink *instance = new Mavlink(); /* this will actually only return once MAVLink exits */ - return instance->task_main(argc, argv); + int res = instance->task_main(argc, argv); + + /* delete instance on main thread end */ + delete instance; + + return res; } int @@ -2002,13 +1986,14 @@ Mavlink::stream(int argc, char *argv[]) static void usage() { - errx(1, "usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate] [-r rate] [-m mode] [-s stream] [-v]"); + warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate] [-r rate] [-m mode] [-s stream] [-v]"); } int mavlink_main(int argc, char *argv[]) { if (argc < 2) { usage(); + exit(1); } if (!strcmp(argv[1], "start")) { @@ -2017,6 +2002,7 @@ int mavlink_main(int argc, char *argv[]) } else if (!strcmp(argv[1], "stop")) { warnx("mavlink stop is deprecated, use stop-all instead"); usage(); + exit(1); } else if (!strcmp(argv[1], "stop-all")) { return Mavlink::destroy_all_instances(); @@ -2029,6 +2015,7 @@ int mavlink_main(int argc, char *argv[]) } else { usage(); + exit(1); } return 0; diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index c606da5045..ffc69bc810 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -105,7 +105,7 @@ public: /** * Constructor */ - Mavlink(int instance_id); + Mavlink(); /** * Destructor, also kills the mavlinks task. From ebab4bfa7ef9ca138ac476943a87b8e3244b9a21 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 11 Mar 2014 13:16:06 +0100 Subject: [PATCH 164/381] Fix for recallstates function --- .../fw_att_pos_estimator/estimator.cpp | 50 +++++++++---------- src/modules/fw_att_pos_estimator/estimator.h | 2 +- 2 files changed, 26 insertions(+), 26 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index cbbb5d7a0c..f578901465 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -1615,34 +1615,34 @@ void StoreStates(uint64_t timestamp_ms) } // Output the state vector stored at the time that best matches that specified by msec -void RecallStates(float (&statesForFusion)[n_states], uint64_t msec) +void RecallStates(float statesForFusion[n_states], uint64_t msec) { - int bestTimeDelta = 200; - unsigned bestStoreIndex = 0; - for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++) - { - // The time delta can also end up as negative number, - // since we might compare future to past or past to future - // therefore cast to int64. - int timeDelta = (int64_t)msec - (int64_t)statetimeStamp[storeIndex]; - if (timeDelta < 0) { - timeDelta = -timeDelta; - } + // int64_t bestTimeDelta = 200; + // unsigned bestStoreIndex = 0; + // for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++) + // { + // // The time delta can also end up as negative number, + // // since we might compare future to past or past to future + // // therefore cast to int64. + // int64_t timeDelta = (int64_t)msec - (int64_t)statetimeStamp[storeIndex]; + // if (timeDelta < 0) { + // timeDelta = -timeDelta; + // } - if (timeDelta < bestTimeDelta) - { - bestStoreIndex = storeIndex; - bestTimeDelta = timeDelta; - } - } - if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error - { - for (uint8_t i=0; i < n_states; i++) statesForFusion[i] = storedStates[i][bestStoreIndex]; - } - else // otherwise output current state - { + // if (timeDelta < bestTimeDelta) + // { + // bestStoreIndex = storeIndex; + // bestTimeDelta = timeDelta; + // } + // } + // if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error + // { + // for (uint8_t i=0; i < n_states; i++) statesForFusion[i] = storedStates[i][bestStoreIndex]; + // } + // else // otherwise output current state + // { for (uint8_t i=0; i < n_states; i++) statesForFusion[i] = states[i]; - } + // } } void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]) diff --git a/src/modules/fw_att_pos_estimator/estimator.h b/src/modules/fw_att_pos_estimator/estimator.h index d68ac1d34e..459515f3b8 100644 --- a/src/modules/fw_att_pos_estimator/estimator.h +++ b/src/modules/fw_att_pos_estimator/estimator.h @@ -147,7 +147,7 @@ void quatNorm(float (&quatOut)[4], const float quatIn[4]); void StoreStates(uint64_t timestamp_ms); // recall stste vector stored at closest time to the one specified by msec -void RecallStates(float (&statesForFusion)[n_states], uint64_t msec); +void RecallStates(float statesForFusion[n_states], uint64_t msec); void quat2Tbn(Mat3f &Tbn, const float (&quat)[4]); From 35b8d00e17cc8161e9d2687af3f9313d010c11ea Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 11 Mar 2014 16:20:37 +0400 Subject: [PATCH 165/381] mavlink: task_main noreturn attribute removed --- src/modules/mavlink/mavlink_main.cpp | 3 --- src/modules/mavlink/mavlink_main.h | 2 +- 2 files changed, 1 insertion(+), 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 2dfc67fd4d..4c79c67b4a 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -221,9 +221,6 @@ Mavlink::Mavlink() : Mavlink::~Mavlink() { - warnx("DESTRUCTOR"); - usleep(1000000); - if (_task_running) { /* task wakes up every 10ms or so at the longest */ _task_should_exit = true; diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index ffc69bc810..ccff6614f8 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -309,6 +309,6 @@ private: /** * Main mavlink task. */ - int task_main(int argc, char *argv[]) __attribute__((noreturn)); + int task_main(int argc, char *argv[]); }; From 09f18408fc5835622a955c80304ce6599ef98090 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 11 Mar 2014 17:21:03 +0400 Subject: [PATCH 166/381] mavlink: msg buffer size fixed --- src/modules/mavlink/mavlink_main.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index ccff6614f8..cc4c896b91 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -209,7 +209,7 @@ private: orb_advert_t _mission_pub; struct mission_s mission; - uint8_t missionlib_msg_buf[sizeof(mavlink_message_t)]; + uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN]; MAVLINK_MODE _mode; uint8_t _mavlink_wpm_comp_id; From 9e41f6af18d3d84413501ce37737d574fd20816d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 11 Mar 2014 19:28:48 +0400 Subject: [PATCH 167/381] mavlink: memory leaks on exit fixed, minor fixes --- src/modules/mavlink/mavlink_main.cpp | 49 ++++++++++++++++++------ src/modules/mavlink/mavlink_receiver.cpp | 17 +++++--- 2 files changed, 50 insertions(+), 16 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 4c79c67b4a..5bdf6c2622 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -221,6 +221,8 @@ Mavlink::Mavlink() : Mavlink::~Mavlink() { + perf_free(_loop_perf); + if (_task_running) { /* task wakes up every 10ms or so at the longest */ _task_should_exit = true; @@ -393,14 +395,12 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) struct mavlink_logmessage msg; strncpy(msg.text, txt, sizeof(msg.text)); - Mavlink *inst = ::_mavlink_instances; - - while (inst != nullptr) { - - mavlink_logbuffer_write(&inst->_logbuffer, &msg); - inst->_total_counter++; - inst = inst->next; - + Mavlink *inst; + LL_FOREACH(_mavlink_instances, inst) { + if (!inst->_task_should_exit) { + mavlink_logbuffer_write(&inst->_logbuffer, &msg); + inst->_total_counter++; + } } return OK; @@ -1557,9 +1557,6 @@ Mavlink::task_main(int argc, char *argv[]) warnx("start"); fflush(stdout); - /* initialize mavlink text message buffering */ - mavlink_logbuffer_init(&_logbuffer, 5); - int ch; _baudrate = 57600; _datarate = 0; @@ -1706,6 +1703,9 @@ Mavlink::task_main(int argc, char *argv[]) return ERROR; } + /* initialize mavlink text message buffering */ + mavlink_logbuffer_init(&_logbuffer, 5); + /* create the device node that's used for sending text log messages, etc. */ register_driver(MAVLINK_LOG_DEVICE, &fops, 0666, NULL); @@ -1867,6 +1867,30 @@ Mavlink::task_main(int argc, char *argv[]) delete _subscribe_to_stream; _subscribe_to_stream = nullptr; + /* delete streams */ + MavlinkStream *stream_to_del = nullptr; + MavlinkStream *stream_next = _streams; + + while (stream_next != nullptr) { + stream_to_del = stream_next; + stream_next = stream_to_del->next; + delete stream_to_del; + } + + _streams = nullptr; + + /* delete subscriptions */ + MavlinkOrbSubscription *sub_to_del = nullptr; + MavlinkOrbSubscription *sub_next = _subscriptions; + + while (sub_next != nullptr) { + sub_to_del = sub_next; + sub_next = sub_to_del->next; + delete sub_to_del; + } + + _subscriptions = nullptr; + warnx("waiting for UART receive thread"); /* wait for threads to complete */ @@ -1878,6 +1902,9 @@ Mavlink::task_main(int argc, char *argv[]) /* close UART */ close(_uart_fd); + /* close mavlink logging device */ + close(_mavlink_fd); + /* destroy log buffer */ mavlink_logbuffer_destroy(&_logbuffer); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index c9fcc4cd84..fa63e06c52 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -83,7 +83,7 @@ __BEGIN_DECLS __END_DECLS -static const float mg2ms2 = 9.8f / 1000.0f; +static const float mg2ms2 = CONSTANTS_ONE_G / 1000.0f; MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _mavlink(parent), @@ -118,6 +118,10 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : memset(&hil_local_pos, 0, sizeof(hil_local_pos)); } +MavlinkReceiver::~MavlinkReceiver() +{ +} + void MavlinkReceiver::handle_message(mavlink_message_t *msg) { @@ -828,9 +832,9 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) memset(&accel, 0, sizeof(accel)); accel.timestamp = timestamp; - accel.x_raw = hil_state.xacc / 9.81f * 1e3f; - accel.y_raw = hil_state.yacc / 9.81f * 1e3f; - accel.z_raw = hil_state.zacc / 9.81f * 1e3f; + accel.x_raw = hil_state.xacc / CONSTANTS_ONE_G * 1e3f; + accel.y_raw = hil_state.yacc / CONSTANTS_ONE_G * 1e3f; + accel.z_raw = hil_state.zacc / CONSTANTS_ONE_G * 1e3f; accel.x = hil_state.xacc; accel.y = hil_state.yacc; accel.z = hil_state.zacc; @@ -925,7 +929,10 @@ void MavlinkReceiver::print_status() void *MavlinkReceiver::start_helper(void *context) { MavlinkReceiver *rcv = new MavlinkReceiver((Mavlink *)context); - return rcv->receive_thread(NULL); + + rcv->receive_thread(NULL); + + delete rcv; } pthread_t From 1fd7b89d3c7c5f9b5c13198c41a015d6c5899260 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 11 Mar 2014 23:03:49 +0400 Subject: [PATCH 168/381] mavlink: use "normal" mode for HIL operation, only add HIL_CONTROLS stream when HIL enabled --- src/modules/mavlink/mavlink_main.cpp | 119 +++++---------------------- src/modules/mavlink/mavlink_main.h | 10 +-- 2 files changed, 26 insertions(+), 103 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 5bdf6c2622..e1c53a5993 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -165,7 +165,7 @@ Mavlink::Mavlink() : _task_should_exit(false), _mavlink_fd(-1), _task_running(false), - _mavlink_hil_enabled(false), + _hil_enabled(false), _main_loop_delay(1000), _subscriptions(nullptr), _streams(nullptr), @@ -548,38 +548,17 @@ Mavlink::set_hil_enabled(bool hil_enabled) { int ret = OK; - /* Enable HIL */ - if (hil_enabled && !_mavlink_hil_enabled) { - - _mavlink_hil_enabled = true; - - /* ramp up some HIL-related subscriptions */ - unsigned hil_rate_interval; - - if (_baudrate < 19200) { - /* 10 Hz */ - hil_rate_interval = 100; - - } else if (_baudrate < 38400) { - /* 10 Hz */ - hil_rate_interval = 100; - - } else if (_baudrate < 115200) { - /* 20 Hz */ - hil_rate_interval = 50; - - } else { - /* 200 Hz */ - hil_rate_interval = 5; - } - -// orb_set_interval(subs.spa_sub, hil_rate_interval); -// set_mavlink_interval_limit(MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, hil_rate_interval); + /* enable HIL */ + if (hil_enabled && !_hil_enabled) { + _hil_enabled = true; + float rate_mult = _datarate / 1000.0f; + configure_stream("HIL_CONTROLS", 15.0f * rate_mult); } - if (!hil_enabled && _mavlink_hil_enabled) { - _mavlink_hil_enabled = false; -// orb_set_interval(subs.spa_sub, 200); + /* disable HIL */ + if (!hil_enabled && _hil_enabled) { + _hil_enabled = false; + configure_stream("HIL_CONTROLS", 0.0f); } else { ret = ERROR; @@ -1445,10 +1424,8 @@ Mavlink::mavlink_missionlib_send_gcs_string(const char *string) if (i > 1) { /* Enforce null termination */ statustext.text[i] = '\0'; - mavlink_message_t msg; - mavlink_msg_statustext_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &statustext); - mavlink_missionlib_send_message(&msg); + mavlink_msg_statustext_send(_channel, statustext.severity, statustext.text); return OK; } else { @@ -1560,9 +1537,7 @@ Mavlink::task_main(int argc, char *argv[]) int ch; _baudrate = 57600; _datarate = 0; - _channel = MAVLINK_COMM_0; - - _mode = MODE_OFFBOARD; + _mode = MAVLINK_MODE_NORMAL; /* work around some stupidity in task_create's argv handling */ argc -= 2; @@ -1603,17 +1578,8 @@ Mavlink::task_main(int argc, char *argv[]) // break; case 'm': - if (strcmp(optarg, "offboard") == 0) { - _mode = MODE_OFFBOARD; - - } else if (strcmp(optarg, "onboard") == 0) { - _mode = MODE_ONBOARD; - - } else if (strcmp(optarg, "hil") == 0) { - _mode = MODE_HIL; - - } else if (strcmp(optarg, "custom") == 0) { - _mode = MODE_CUSTOM; + if (strcmp(optarg, "custom") == 0) { + _mode = MAVLINK_MODE_CUSTOM; } break; @@ -1649,42 +1615,20 @@ Mavlink::task_main(int argc, char *argv[]) /* inform about mode */ switch (_mode) { - case MODE_CUSTOM: + case MAVLINK_MODE_NORMAL: + warnx("mode: NORMAL"); + break; + + case MAVLINK_MODE_CUSTOM: warnx("mode: CUSTOM"); break; - case MODE_OFFBOARD: - warnx("mode: OFFBOARD"); - break; - - case MODE_ONBOARD: - warnx("mode: ONBOARD"); - break; - - case MODE_HIL: - warnx("mode: HIL"); - break; - default: warnx("ERROR: Unknown mode"); break; } - switch (_mode) { - case MODE_OFFBOARD: - case MODE_HIL: - case MODE_CUSTOM: - _mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER; - break; - - case MODE_ONBOARD: - _mavlink_wpm_comp_id = MAV_COMP_ID_CAMERA; - break; - - default: - _mavlink_wpm_comp_id = MAV_COMP_ID_ALL; - break; - } + _mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER; warnx("data rate: %d bytes/s", _datarate); warnx("port: %s, baudrate: %d", _device_name, _baudrate); @@ -1741,7 +1685,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("HEARTBEAT", 1.0f); switch (_mode) { - case MODE_OFFBOARD: + case MAVLINK_MODE_NORMAL: configure_stream("SYS_STATUS", 1.0f); configure_stream("GPS_GLOBAL_ORIGIN", 0.5f); configure_stream("HIGHRES_IMU", 1.0f * rate_mult); @@ -1753,20 +1697,6 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("RC_CHANNELS_RAW", 1.0f * rate_mult); break; - case MODE_HIL: - /* HIL mode normally runs at high data rate, rate_mult ~ 10 */ - configure_stream("SYS_STATUS", 1.0f); - configure_stream("GPS_GLOBAL_ORIGIN", 0.5f); - configure_stream("HIGHRES_IMU", 1.0f * rate_mult); - configure_stream("ATTITUDE", 2.0f * rate_mult); - configure_stream("VFR_HUD", 2.0f * rate_mult); - configure_stream("GPS_RAW_INT", 1.0f * rate_mult); - configure_stream("GLOBAL_POSITION_INT", 1.0f * rate_mult); - configure_stream("LOCAL_POSITION_NED", 1.0f * rate_mult); - configure_stream("RC_CHANNELS_RAW", 1.0f * rate_mult); - configure_stream("HIL_CONTROLS", 15.0f * rate_mult); - break; - default: break; } @@ -1795,12 +1725,7 @@ Mavlink::task_main(int argc, char *argv[]) if (status_sub->update(t)) { /* switch HIL mode if required */ - if (status->hil_state == HIL_STATE_ON) { - set_hil_enabled(true); - - } else if (status->hil_state == HIL_STATE_OFF) { - set_hil_enabled(false); - } + set_hil_enabled(status->hil_state == HIL_STATE_ON); } /* check for requested subscriptions */ diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index cc4c896b91..3a8625c6bf 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -145,16 +145,14 @@ public: const char *_device_name; enum MAVLINK_MODE { - MODE_CUSTOM = 0, - MODE_OFFBOARD, - MODE_ONBOARD, - MODE_HIL + MAVLINK_MODE_NORMAL = 0, + MAVLINK_MODE_CUSTOM }; void set_mode(enum MAVLINK_MODE); enum MAVLINK_MODE get_mode() { return _mode; } - bool get_hil_enabled() { return _mavlink_hil_enabled; }; + bool get_hil_enabled() { return _hil_enabled; }; /** * Handle waypoint related messages. @@ -200,7 +198,7 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ /* states */ - bool _mavlink_hil_enabled; /**< Hardware In the Loop mode */ + bool _hil_enabled; /**< Hardware In the Loop mode */ unsigned _main_loop_delay; /**< mainloop delay, depends on data rate */ From 4cee3614c7bc2e960ac52e59014bc4d08b8da11e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 11 Mar 2014 23:04:23 +0400 Subject: [PATCH 169/381] rc.usb: increase data rate to 10000bytes/s for USB --- ROMFS/px4fmu_common/init.d/rc.usb | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index 0de2d42958..558be42755 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -5,7 +5,7 @@ echo "Starting MAVLink on this USB console" -mavlink start -r 5000 -d /dev/ttyACM0 +mavlink start -r 10000 -d /dev/ttyACM0 # Exit shell to make it available to MAVLink exit From 5f847ff8a3df1ba303129a7a4938305a6901755c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 11 Mar 2014 21:14:54 +0100 Subject: [PATCH 170/381] Remove HIL flag on startup --- 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 eba18ddb19..8cb8f4dc77 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -392,7 +392,7 @@ then if [ $HIL == yes ] then sleep 1 - set MAVLINK_FLAGS "-r 10000 -d /dev/ttyACM0 -m hil" + set MAVLINK_FLAGS "-r 10000 -d /dev/ttyACM0" usleep 5000 else # Normal mode, use baudrate 57600 (default) and data rate 1000 bytes/s From a37e539647df2b3cfd335b33e4d186c4d39316b6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 11 Mar 2014 22:26:12 +0100 Subject: [PATCH 171/381] Correctly initialize mission count --- src/modules/mavlink/mavlink_main.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 5bdf6c2622..6675804523 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -178,6 +178,7 @@ Mavlink::Mavlink() : _loop_perf(perf_alloc(PC_ELAPSED, "mavlink")) { _wpm = &_wpm_s; + mission.count = 0; fops.ioctl = (int (*)(file *, int, long unsigned int))&mavlink_dev_ioctl; _instance_id = Mavlink::instance_count(); From a977ff2b6295bd4bac614a854ecc35e1b9b2d75c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 12 Mar 2014 00:20:27 +0100 Subject: [PATCH 172/381] fw att control: add timestamp dependency in airspeed check --- src/modules/fw_att_control/fw_att_control_main.cpp | 3 ++- 1 file changed, 2 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 deafd7f86f..497d1d15a4 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -632,7 +632,8 @@ FixedwingAttitudeControl::task_main() /* if airspeed is smaller than min, the sensor is not giving good readings */ if ((_airspeed.indicated_airspeed_m_s < 0.5f * _parameters.airspeed_min) || - !isfinite(_airspeed.indicated_airspeed_m_s)) { + !isfinite(_airspeed.indicated_airspeed_m_s) || + (float)hrt_elapsed_time(&_airspeed.timestamp) > 1e6f) { airspeed = _parameters.airspeed_trim; } else { From f66b0ad8ac926a79608cf872c8e49ea7ace92288 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Mar 2014 09:04:30 +0100 Subject: [PATCH 173/381] Added hardware flow control to mavlink app. Auto-disables if nothing can be written to the port --- src/modules/mavlink/mavlink_main.cpp | 46 ++++++++++++++++++++++++++++ src/modules/mavlink/mavlink_main.h | 7 +++++ 2 files changed, 53 insertions(+) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 6675804523..ed3b265f9e 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -104,6 +104,8 @@ static struct file_operations fops; */ extern "C" __EXPORT int mavlink_main(int argc, char *argv[]); +static uint64_t last_write_times[6] = {0}; + /* * Internal function to send the bytes through the right serial port */ @@ -149,10 +151,28 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length } ssize_t desired = (sizeof(uint8_t) * length); + + /* + * Check if the OS buffer is full and disable HW + * flow control if it continues to be full + */ + int buf_free = 0; + + if (ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0 && + hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500*1000UL) { + struct termios uart_config; + (void)tcgetattr(uart, &uart_config); + uart_config.c_cflag &= ~CRTSCTS; + (void)tcsetattr(uart, TCSANOW, &uart_config); + warnx("DISABLING HARDWARE FLOW CONTROL"); + } + ssize_t ret = write(uart, ch, desired); if (ret != desired) { warn("write err"); + } else { + last_write_times[(unsigned)channel] = hrt_absolute_time(); } } @@ -541,9 +561,35 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * return -1; } + /* + * Setup hardware flow control. If the port has no RTS pin this call will fail, + * which is not an issue, but requires a separate call so we can fail silently. + */ + (void)tcgetattr(_uart_fd, &uart_config); + uart_config.c_cflag |= CRTS_IFLOW; + (void)tcsetattr(_uart_fd, TCSANOW, &uart_config); + + /* setup output flow control */ + (void)enable_flow_control(true); + return _uart_fd; } +int +Mavlink::enable_flow_control(bool enabled) +{ + struct termios uart_config; + int ret = tcgetattr(_uart_fd, &uart_config); + if (enabled) { + uart_config.c_cflag |= CRTSCTS; + } else { + uart_config.c_cflag &= ~CRTSCTS; + } + ret = tcsetattr(_uart_fd, TCSANOW, &uart_config); + + return ret; +} + int Mavlink::set_hil_enabled(bool hil_enabled) { diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index cc4c896b91..2a7bd57cfa 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -184,6 +184,13 @@ public: int get_instance_id(); + /** + * Enable / disable hardware flow control. + * + * @param enabled True if hardware flow control should be enabled + */ + int enable_flow_control(bool enabled); + mavlink_channel_t get_channel(); bool _task_should_exit; /**< if true, mavlink task should exit */ From 023c7069c4a340aa1b7c9f05963a066cee368b2a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Mar 2014 09:36:12 +0100 Subject: [PATCH 174/381] Fix FIONWRITE usage --- src/modules/mavlink/mavlink_main.cpp | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index ed3b265f9e..9a6e1130e3 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -158,13 +158,17 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length */ int buf_free = 0; - if (ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0 && - hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500*1000UL) { - struct termios uart_config; - (void)tcgetattr(uart, &uart_config); - uart_config.c_cflag &= ~CRTSCTS; - (void)tcsetattr(uart, TCSANOW, &uart_config); - warnx("DISABLING HARDWARE FLOW CONTROL"); + if (ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) { + + if (buf_free == 0 && last_write_times[(unsigned)channel] != 0 && + hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500*1000UL) { + + struct termios uart_config; + (void)tcgetattr(uart, &uart_config); + uart_config.c_cflag &= ~CRTSCTS; + (void)tcsetattr(uart, TCSANOW, &uart_config); + warnx("DISABLING HARDWARE FLOW CONTROL"); + } } ssize_t ret = write(uart, ch, desired); From 76af0970f5220d3a842c4daa353a45ef51df1082 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Mar 2014 09:46:02 +0100 Subject: [PATCH 175/381] Increased param rate, fixed wrong usage of MAVLink chan. --- src/modules/mavlink/mavlink_main.cpp | 5 ++--- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 6675804523..1044984bfd 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -661,7 +661,7 @@ int Mavlink::mavlink_pm_send_param(param_t param) mavlink_msg_param_value_pack_chan(mavlink_system.sysid, mavlink_system.compid, - MAVLINK_COMM_0, + _channel, &tx_msg, name_buf, val_buf, @@ -1561,7 +1561,6 @@ Mavlink::task_main(int argc, char *argv[]) int ch; _baudrate = 57600; _datarate = 0; - _channel = MAVLINK_COMM_0; _mode = MODE_OFFBOARD; @@ -1776,7 +1775,7 @@ Mavlink::task_main(int argc, char *argv[]) _mavlink_param_queue_index = param_count(); MavlinkRateLimiter slow_rate_limiter(2000000.0f / rate_mult); - MavlinkRateLimiter fast_rate_limiter(100000.0f / rate_mult); + MavlinkRateLimiter fast_rate_limiter(20000.0f / rate_mult); /* set main loop delay depending on data rate to minimize CPU overhead */ _main_loop_delay = MAIN_LOOP_DELAY / rate_mult; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index fa63e06c52..c222a3ddf4 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -912,7 +912,7 @@ MavlinkReceiver::receive_thread(void *arg) _mavlink->mavlink_wpm_message_handler(&msg); /* handle packet with parameter component */ - _mavlink->mavlink_pm_message_handler(MAVLINK_COMM_0, &msg); + _mavlink->mavlink_pm_message_handler(_mavlink->get_channel(), &msg); } } } From 2d2ecbad00c4f21d0c3ded2faa4f9bdb7adefddc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Mar 2014 09:56:26 +0100 Subject: [PATCH 176/381] Fixed more wrong usages of encoding channel --- src/modules/mavlink/mavlink_main.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 1044984bfd..d722bec469 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -856,7 +856,7 @@ void Mavlink::mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8 wpa.target_component = compid; wpa.type = type; - mavlink_msg_mission_ack_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpa); + mavlink_msg_mission_ack_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpa); mavlink_missionlib_send_message(&msg); if (_verbose) { warnx("Sent waypoint ack (%u) to ID %u", wpa.type, wpa.target_system); } @@ -879,7 +879,7 @@ void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq) wpc.seq = seq; - mavlink_msg_mission_current_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpc); + mavlink_msg_mission_current_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpc); mavlink_missionlib_send_message(&msg); } else if (seq == 0 && _wpm->size == 0) { @@ -902,7 +902,7 @@ void Mavlink::mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uin wpc.target_component = compid; wpc.count = mission.count; - mavlink_msg_mission_count_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpc); + mavlink_msg_mission_count_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpc); mavlink_missionlib_send_message(&msg); if (_verbose) { warnx("Sent waypoint count (%u) to ID %u", wpc.count, wpc.target_system); } @@ -933,7 +933,7 @@ void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t wp.target_system = sysid; wp.target_component = compid; wp.seq = seq; - mavlink_msg_mission_item_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wp); + mavlink_msg_mission_item_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wp); mavlink_missionlib_send_message(&msg); if (_verbose) { warnx("Sent waypoint %u to ID %u", wp.seq, wp.target_system); } @@ -953,7 +953,7 @@ void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, u wpr.target_system = sysid; wpr.target_component = compid; wpr.seq = seq; - mavlink_msg_mission_request_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wpr); + mavlink_msg_mission_request_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpr); mavlink_missionlib_send_message(&msg); if (_verbose) { warnx("Sent waypoint request %u to ID %u", wpr.seq, wpr.target_system); } @@ -979,7 +979,7 @@ void Mavlink::mavlink_wpm_send_waypoint_reached(uint16_t seq) wp_reached.seq = seq; - mavlink_msg_mission_item_reached_encode(mavlink_system.sysid, _mavlink_wpm_comp_id, &msg, &wp_reached); + mavlink_msg_mission_item_reached_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wp_reached); mavlink_missionlib_send_message(&msg); if (_verbose) { warnx("Sent waypoint %u reached message", wp_reached.seq); } @@ -1448,7 +1448,7 @@ Mavlink::mavlink_missionlib_send_gcs_string(const char *string) statustext.text[i] = '\0'; mavlink_message_t msg; - mavlink_msg_statustext_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &statustext); + mavlink_msg_statustext_encode_chan(mavlink_system.sysid, mavlink_system.compid, _channel, &msg, &statustext); mavlink_missionlib_send_message(&msg); return OK; From d9d0e60bd574b135d98bdaa48c4b32056f815217 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 12 Mar 2014 10:02:39 +0100 Subject: [PATCH 177/381] fw att ctrl: airspeed check: remove unnecessary cast --- src/modules/fw_att_control/fw_att_control_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 497d1d15a4..bb674d6c98 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -633,7 +633,7 @@ FixedwingAttitudeControl::task_main() /* if airspeed is smaller than min, the sensor is not giving good readings */ if ((_airspeed.indicated_airspeed_m_s < 0.5f * _parameters.airspeed_min) || !isfinite(_airspeed.indicated_airspeed_m_s) || - (float)hrt_elapsed_time(&_airspeed.timestamp) > 1e6f) { + hrt_elapsed_time(&_airspeed.timestamp) > 1e6) { airspeed = _parameters.airspeed_trim; } else { From 7604518db479e7cc9618e19b224c5d5c94013142 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Mar 2014 10:29:39 +0100 Subject: [PATCH 178/381] Fixed HW flow control enable / disable scheme, console output cleanup on start --- src/modules/mavlink/mavlink_main.cpp | 45 ++++++++++++++-------------- 1 file changed, 23 insertions(+), 22 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 7e1af88240..4420e04fde 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -160,23 +160,30 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length if (ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) { - if (buf_free == 0 && last_write_times[(unsigned)channel] != 0 && + if (buf_free == 0) { + + if (last_write_times[(unsigned)channel] != 0 && hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500*1000UL) { - struct termios uart_config; - (void)tcgetattr(uart, &uart_config); - uart_config.c_cflag &= ~CRTSCTS; - (void)tcsetattr(uart, TCSANOW, &uart_config); - warnx("DISABLING HARDWARE FLOW CONTROL"); + struct termios uart_config; + (void)tcgetattr(uart, &uart_config); + uart_config.c_cflag &= ~CRTSCTS; + (void)tcsetattr(uart, TCSANOW, &uart_config); + warnx("DISABLING HARDWARE FLOW CONTROL"); + } + + } else { + + /* apparently there is space left, although we might be + * partially overflooding the buffer already */ + last_write_times[(unsigned)channel] = hrt_absolute_time(); } } ssize_t ret = write(uart, ch, desired); if (ret != desired) { - warn("write err"); - } else { - last_write_times[(unsigned)channel] = hrt_absolute_time(); + // XXX do something here, but change to using FIONWRITE and OS buf size for detection } } @@ -536,7 +543,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * /* Back up the original uart configuration to restore it after exit */ if ((termios_state = tcgetattr(_uart_fd, uart_config_original)) < 0) { - warnx("ERROR get termios config %s: %d\n", uart_name, termios_state); + warnx("ERR GET CONF %s: %d\n", uart_name, termios_state); close(_uart_fd); return -1; } @@ -552,7 +559,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * /* Set baud rate */ 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 SET BAUD %s: %d\n", uart_name, termios_state); close(_uart_fd); return -1; } @@ -560,7 +567,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * } if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) { - warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name); + warnx("ERR SET CONF %s\n", uart_name); close(_uart_fd); return -1; } @@ -574,7 +581,9 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * (void)tcsetattr(_uart_fd, TCSANOW, &uart_config); /* setup output flow control */ - (void)enable_flow_control(true); + if (enable_flow_control(true)) { + warnx("ERR FLOW CTRL EN"); + } return _uart_fd; } @@ -1604,10 +1613,6 @@ Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate) int Mavlink::task_main(int argc, char *argv[]) { - /* inform about start */ - warnx("start"); - fflush(stdout); - int ch; _baudrate = 57600; _datarate = 0; @@ -1736,8 +1741,7 @@ Mavlink::task_main(int argc, char *argv[]) break; } - warnx("data rate: %d bytes/s", _datarate); - warnx("port: %s, baudrate: %d", _device_name, _baudrate); + warnx("data rate: %d bps, port: %s, baud: %d", _datarate, _device_name, (int)_baudrate); /* flush stdout in case MAVLink is about to take it over */ fflush(stdout); @@ -1782,9 +1786,6 @@ Mavlink::task_main(int argc, char *argv[]) struct vehicle_status_s *status = (struct vehicle_status_s *) status_sub->get_data(); - warnx("started"); - mavlink_log_info(_mavlink_fd, "[mavlink] started"); - /* add default streams depending on mode and intervals depending on datarate */ float rate_mult = _datarate / 1000.0f; From 45a18587fc5e96697da586b9bf51ee65c10e3753 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Mar 2014 17:53:25 +0100 Subject: [PATCH 179/381] Fine tuning for flow control disable to stop firing after change is effective --- src/modules/mavlink/mavlink_main.cpp | 35 +++++++++++++++++----------- src/modules/mavlink/mavlink_main.h | 4 ++++ 2 files changed, 26 insertions(+), 13 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 4420e04fde..b699b86a5e 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -112,44 +112,50 @@ static uint64_t last_write_times[6] = {0}; void mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length) { - int uart = -1; + Mavlink *instance; switch (channel) { case MAVLINK_COMM_0: - uart = Mavlink::get_uart_fd(0); + instance = Mavlink::get_instance(0); break; case MAVLINK_COMM_1: - uart = Mavlink::get_uart_fd(1); + instance = Mavlink::get_instance(1); break; case MAVLINK_COMM_2: - uart = Mavlink::get_uart_fd(2); + instance = Mavlink::get_instance(2); break; case MAVLINK_COMM_3: - uart = Mavlink::get_uart_fd(3); + instance = Mavlink::get_instance(3); break; #ifdef MAVLINK_COMM_4 case MAVLINK_COMM_4: - uart = Mavlink::get_uart_fd(4); + instance = Mavlink::get_instance(4); break; #endif #ifdef MAVLINK_COMM_5 case MAVLINK_COMM_5: - uart = Mavlink::get_uart_fd(5); + instance = Mavlink::get_instance(5); break; #endif #ifdef MAVLINK_COMM_6 case MAVLINK_COMM_6: - uart = Mavlink::get_uart_fd(6); + instance = Mavlink::get_instance(6); break; #endif } + /* no valid instance, bail */ + if (!instance) + return; + + int uart = instance->get_uart_fd(); + ssize_t desired = (sizeof(uint8_t) * length); /* @@ -158,18 +164,16 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length */ int buf_free = 0; - if (ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) { + if (instance->get_flow_control_enabled() + && ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) { if (buf_free == 0) { if (last_write_times[(unsigned)channel] != 0 && hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500*1000UL) { - struct termios uart_config; - (void)tcgetattr(uart, &uart_config); - uart_config.c_cflag &= ~CRTSCTS; - (void)tcsetattr(uart, TCSANOW, &uart_config); warnx("DISABLING HARDWARE FLOW CONTROL"); + instance->enable_flow_control(false); } } else { @@ -204,6 +208,7 @@ Mavlink::Mavlink() : _mavlink_param_queue_index(0), _subscribe_to_stream(nullptr), _subscribe_to_stream_rate(0.0f), + _flow_control_enabled(true), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mavlink")) @@ -600,6 +605,10 @@ Mavlink::enable_flow_control(bool enabled) } ret = tcsetattr(_uart_fd, TCSANOW, &uart_config); + if (!ret) { + _flow_control_enabled = enabled; + } + return ret; } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 2a7bd57cfa..94b2c9dbc4 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -156,6 +156,8 @@ public: bool get_hil_enabled() { return _mavlink_hil_enabled; }; + bool get_flow_control_enabled() { return _flow_control_enabled; } + /** * Handle waypoint related messages. */ @@ -248,6 +250,8 @@ private: char *_subscribe_to_stream; float _subscribe_to_stream_rate; + bool _flow_control_enabled; + /** * Send one parameter. * From 6f550775410b0045bce5dafd0d4fd3f03988f8bf Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 13 Mar 2014 00:24:28 +0400 Subject: [PATCH 180/381] gpio_led: bugs fixed --- src/modules/gpio_led/gpio_led.c | 41 ++++++++++++++------------------- 1 file changed, 17 insertions(+), 24 deletions(-) diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index f5f3dea761..93a29a851c 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -91,8 +91,7 @@ int gpio_led_main(int argc, char *argv[]) #endif #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 errx(1, "usage: gpio_led {start|stop} [-p ]\n" - "\t-p\tUse pin:\n" - "\t\tn\tAUX OUT pin number (default: 1)\n" + "\t-p \tUse specified AUX OUT pin number (default: 1)" ); #endif @@ -108,6 +107,14 @@ int gpio_led_main(int argc, char *argv[]) /* by default use GPIO_EXT_1 on FMUv1 and GPIO_SERVO_1 on FMUv2 */ int pin = 1; + /* pin name to display */ +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + char *pin_name = "PX4FMU GPIO_EXT1"; +#endif +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + char pin_name[] = "AUX OUT 1"; +#endif + if (argc > 2) { if (!strcmp(argv[2], "-p")) { #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 @@ -115,26 +122,32 @@ int gpio_led_main(int argc, char *argv[]) if (!strcmp(argv[3], "1")) { use_io = false; pin = GPIO_EXT_1; + pin_name = "PX4FMU GPIO_EXT1"; } else if (!strcmp(argv[3], "2")) { use_io = false; pin = GPIO_EXT_2; + pin_name = "PX4FMU GPIO_EXT2"; } else if (!strcmp(argv[3], "a1")) { use_io = true; pin = PX4IO_P_SETUP_RELAYS_ACC1; + pin_name = "PX4IO ACC1"; } else if (!strcmp(argv[3], "a2")) { use_io = true; pin = PX4IO_P_SETUP_RELAYS_ACC2; + pin_name = "PX4IO ACC2"; } else if (!strcmp(argv[3], "r1")) { use_io = true; pin = PX4IO_P_SETUP_RELAYS_POWER1; + pin_name = "PX4IO RELAY1"; } else if (!strcmp(argv[3], "r2")) { use_io = true; pin = PX4IO_P_SETUP_RELAYS_POWER2; + pin_name = "PX4IO RELAY2"; } else { errx(1, "unsupported pin: %s", argv[3]); @@ -146,7 +159,8 @@ int gpio_led_main(int argc, char *argv[]) if (n >= 1 && n <= 6) { use_io = false; - pin = n; + pin = 1 << (n - 1); + snprintf(pin_name, sizeof(pin_name), "AUX OUT %d", n); } else { errx(1, "unsupported pin: %s", argv[3]); @@ -166,27 +180,6 @@ int gpio_led_main(int argc, char *argv[]) } else { gpio_led_started = true; - char pin_name[24]; - -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 - - if (use_io) { - if (pin & (PX4IO_P_SETUP_RELAYS_ACC1 | PX4IO_P_SETUP_RELAYS_ACC2)) { - sprintf(pin_name, "PX4IO ACC%i", (pin >> 3)); - - } else { - sprintf(pin_name, "PX4IO RELAY%i", pin); - } - - } else { - sprintf(pin_name, "PX4FMU GPIO_EXT%i", pin); - } - -#endif -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 - sprintf(pin_name, "AUX OUT %i", pin); -#endif - warnx("start, using pin: %s", pin_name); } From bc36b540a5608f17d7c14bc87fb2bb7ca76459b2 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 13 Mar 2014 00:25:00 +0400 Subject: [PATCH 181/381] gpio_led: code style fixed --- src/modules/gpio_led/gpio_led.c | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index 93a29a851c..6dfd22fdff 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -253,8 +253,9 @@ void gpio_led_cycle(FAR void *arg) bool updated; orb_check(priv->vehicle_status_sub, &updated); - if (updated) + if (updated) { orb_copy(ORB_ID(vehicle_status), priv->vehicle_status_sub, &priv->status); + } /* select pattern for current status */ int pattern = 0; @@ -294,8 +295,9 @@ void gpio_led_cycle(FAR void *arg) priv->counter++; - if (priv->counter > 5) + if (priv->counter > 5) { priv->counter = 0; + } /* repeat cycle at 5 Hz */ if (gpio_led_started) { From b6ea38b91a3d161ce01ab3683b0097f45c4e4de3 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 13 Mar 2014 13:32:24 +0100 Subject: [PATCH 182/381] sensors app: add timestamp in airspeed message --- src/modules/sensors/sensors.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index b176d7417a..6e2df89ed0 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1030,6 +1030,7 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) raw.differential_pressure_pa = _diff_pres.differential_pressure_pa; raw.differential_pressure_counter++; + _airspeed.timestamp = hrt_absolute_time(); _airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_pa); _airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_pa + raw.baro_pres_mbar * 1e2f, raw.baro_pres_mbar * 1e2f, raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG); From 5745233f803d57f3ad3cb0e95bbc99ed2392728c Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Thu, 13 Mar 2014 14:36:02 +0100 Subject: [PATCH 183/381] Remove non-existent parameter from 4008_ardrone startup script. --- ROMFS/px4fmu_common/init.d/4008_ardrone | 1 - 1 file changed, 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/4008_ardrone b/ROMFS/px4fmu_common/init.d/4008_ardrone index 0f98f7b6c2..045a4ce00b 100644 --- a/ROMFS/px4fmu_common/init.d/4008_ardrone +++ b/ROMFS/px4fmu_common/init.d/4008_ardrone @@ -23,7 +23,6 @@ then param set MC_PITCHRATE_I 0.0 param set MC_PITCHRATE_D 0.0 param set MC_YAW_P 1.0 - param set MC_YAW_D 0.1 param set MC_YAWRATE_P 0.15 param set MC_YAWRATE_I 0.0 param set MC_YAWRATE_D 0.0 From 7a97eb83f629ec718a6e31ccb5d184786d0a5377 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 13 Mar 2014 15:25:43 +0100 Subject: [PATCH 184/381] ardrone startup: set battery V scale parameter in autoconfig to the value given in sensors_params.c --- ROMFS/px4fmu_common/init.d/4008_ardrone | 1 + 1 file changed, 1 insertion(+) diff --git a/ROMFS/px4fmu_common/init.d/4008_ardrone b/ROMFS/px4fmu_common/init.d/4008_ardrone index 045a4ce00b..3a17b34aba 100644 --- a/ROMFS/px4fmu_common/init.d/4008_ardrone +++ b/ROMFS/px4fmu_common/init.d/4008_ardrone @@ -27,6 +27,7 @@ then param set MC_YAWRATE_I 0.0 param set MC_YAWRATE_D 0.0 param set MC_YAW_FF 0.15 + param set BAT_V_SCALING 0.00838095238 fi set OUTPUT_MODE ardrone From bfe8d735604511da44c63e74efa1cba5eb694064 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 13 Mar 2014 15:41:53 +0100 Subject: [PATCH 185/381] rcS: introduce flag that allows disabling startup of the default apps in order to start a different set of apps in a autostart script --- ROMFS/px4fmu_common/init.d/rcS | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 57299d7724..55e2a886e1 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -118,6 +118,7 @@ then set MKBLCTRL_MODE none set FMU_MODE pwm set MAV_TYPE none + set LOAD_DEFAULT_APPS yes # # Set DO_AUTOCONFIG flag to use it in AUTOSTART scripts @@ -465,7 +466,10 @@ then sh /etc/init.d/rc.interface # Start standard fixedwing apps - sh /etc/init.d/rc.fw_apps + if [ LOAD_DEFAULT_APPS == yes ] + then + sh /etc/init.d/rc.fw_apps + fi fi # @@ -521,7 +525,10 @@ then sh /etc/init.d/rc.interface # Start standard multicopter apps - sh /etc/init.d/rc.mc_apps + if [ LOAD_DEFAULT_APPS == yes ] + then + sh /etc/init.d/rc.mc_apps + fi fi # From 11f12b4dfed0a5209e34a3d7cc1e473c0649aca6 Mon Sep 17 00:00:00 2001 From: dominiho Date: Thu, 13 Mar 2014 15:43:52 +0100 Subject: [PATCH 186/381] added i2c px4flow driver --- src/drivers/drv_px4flow.h | 84 ++++ src/drivers/px4flow/module.mk | 40 ++ src/drivers/px4flow/px4flow.cpp | 806 ++++++++++++++++++++++++++++++++ 3 files changed, 930 insertions(+) create mode 100644 src/drivers/drv_px4flow.h create mode 100644 src/drivers/px4flow/module.mk create mode 100644 src/drivers/px4flow/px4flow.cpp diff --git a/src/drivers/drv_px4flow.h b/src/drivers/drv_px4flow.h new file mode 100644 index 0000000000..bef02d54ef --- /dev/null +++ b/src/drivers/drv_px4flow.h @@ -0,0 +1,84 @@ +/**************************************************************************** + * + * 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 Rangefinder driver interface. + */ + +#ifndef _DRV_PX4FLOW_H +#define _DRV_PX4FLOW_H + +#include +#include + +#include "drv_sensor.h" +#include "drv_orb_dev.h" + +#define PX4FLOW_DEVICE_PATH "/dev/px4flow" + +/** + * Optical flow in NED body frame in SI units. + * + * @see http://en.wikipedia.org/wiki/International_System_of_Units + */ +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, 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 */ + uint8_t sensor_id; /**< id of the sensor emitting the flow value */ + +}; + +/* + * ObjDev tag for px4flow data. + */ +ORB_DECLARE(optical_flow); + +/* + * ioctl() definitions + * + * px4flow drivers also implement the generic sensor driver + * interfaces from drv_sensor.h + */ + +#define _PX4FLOWIOCBASE (0x7700) +#define __PX4FLOWIOC(_n) (_IOC(_PX4FLOWIOCBASE, _n)) + + +#endif /* _DRV_PX4FLOW_H */ diff --git a/src/drivers/px4flow/module.mk b/src/drivers/px4flow/module.mk new file mode 100644 index 0000000000..d3062e4579 --- /dev/null +++ b/src/drivers/px4flow/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 PX4FLOW driver. +# + +MODULE_COMMAND = px4flow + +SRCS = px4flow.cpp diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp new file mode 100644 index 0000000000..50089282ab --- /dev/null +++ b/src/drivers/px4flow/px4flow.cpp @@ -0,0 +1,806 @@ +/**************************************************************************** + * + * 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 px4flow.cpp + * @author Dominik Honegger + * + * Driver for the PX4FLOW module 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 + +#include + +/* Configuration Constants */ +#define PX4FLOW_BUS PX4_I2C_BUS_EXPANSION +#define I2C_FLOW_ADDRESS 0x45 //* 7-bit address. 8-bit address is 0x8A +//range 0x42 - 0x49 + +/* PX4FLOW Registers addresses */ +#define PX4FLOW_REG 0x00 /* Measure Register */ + +#define PX4FLOW_CONVERSION_INTERVAL 8000 /* 8ms 125Hz + +/* 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 + +//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 +{ +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(); + +private: + + work_s _work; + RingBuffer *_reports; + 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. + * + * @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(); + + /** + * 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); + + +}; + +/* + * Driver 'main' command. + */ +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")) +{ + // enable debug() calls + _debug_enabled = true; + + // work_cancel in the dtor will explode if we don't do this... + memset(&_work, 0, sizeof(_work)); +} + +PX4FLOW::~PX4FLOW() +{ + /* make sure we are truly inactive */ + stop(); + + /* free any existing reports */ + if (_reports != nullptr) + delete _reports; +} + +int +PX4FLOW::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(px4flow_report)); + + if (_reports == nullptr) + goto out; + + /* get a publish handle on the px4flow topic */ + struct px4flow_report 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; +} + +int +PX4FLOW::probe() +{ + return measure(); +} + +int +PX4FLOW::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(PX4FLOW_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(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) + 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; + + default: + /* give it to the superclass */ + return I2C::ioctl(filp, cmd, 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); + 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(PX4FLOW_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 +PX4FLOW::measure() +{ + int ret; + + /* + * Send the command to begin a measurement. + */ + uint8_t cmd = PX4FLOW_REG; + ret = transfer(&cmd, 1, nullptr, 0); + + if (OK != ret) + { + perf_count(_comms_errors); + log("i2c::transfer returned %d", ret); + printf("i2c::transfer flow returned %d"); + return ret; + } + ret = OK; + + return ret; +} + +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); + 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]; + + struct px4flow_report 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(); + + + /* publish it */ + orb_publish(ORB_ID(optical_flow), _px4flow_topic, &report); + + /* post a report to the ring */ + 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 +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); + + /* notify about state change */ + struct subsystem_info_s info = { + true, + true, + true, + 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() +{ + work_cancel(HPWORK, &_work); +} + +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; + } + + /* next phase is measurement */ + _collect_phase = false; + + /* + * Is there a collect->measure gap? + */ + if (_measure_ticks > USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)) { + + /* 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)); + + 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)&PX4FLOW::cycle_trampoline, + this, + USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)); +} + +void +PX4FLOW::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 px4flow +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +const int ERROR = -1; + +PX4FLOW *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 PX4FLOW(PX4FLOW_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(PX4FLOW_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + goto fail; + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX) < 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 px4flow_report report; + ssize_t sz; + int ret; + + 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); + + /* do a simple demand read */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) + // 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("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 5x and report each value */ + for (unsigned i = 0; i < 5; 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("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); + + + } + + errx(0, "PASS"); +} + +/** + * Reset the driver. + */ +void +reset() +{ + int fd = open(PX4FLOW_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 +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(); + + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) + px4flow::test(); + + /* + * Reset the driver. + */ + if (!strcmp(argv[1], "reset")) + px4flow::reset(); + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) + px4flow::info(); + + errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); +} From 7d48941c02fdb84df2f11731735caaa3b8a2e698 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 13 Mar 2014 18:47:31 +0100 Subject: [PATCH 187/381] set correct MAV_TYPE in ardrone startup script --- ROMFS/px4fmu_common/init.d/4008_ardrone | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/4008_ardrone b/ROMFS/px4fmu_common/init.d/4008_ardrone index 3a17b34aba..14786f2106 100644 --- a/ROMFS/px4fmu_common/init.d/4008_ardrone +++ b/ROMFS/px4fmu_common/init.d/4008_ardrone @@ -33,3 +33,5 @@ fi set OUTPUT_MODE ardrone set USE_IO no set MIXER skip +# set MAV_TYPE because no specific mixer is set +set MAV_TYPE 2 From 0784ef918925a546e0e4294571859150f0789561 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 14 Mar 2014 01:13:22 +0100 Subject: [PATCH 188/381] add low pass filter to meas airspeed driver including logging of filtered value --- src/drivers/meas_airspeed/meas_airspeed.cpp | 9 ++++++++- src/modules/sdlog2/sdlog2.c | 1 + src/modules/sdlog2/sdlog2_messages.h | 3 ++- src/modules/sensors/sensors.cpp | 1 + src/modules/uORB/topics/differential_pressure.h | 5 +++-- src/modules/uORB/topics/sensor_combined.h | 5 +++-- 6 files changed, 18 insertions(+), 6 deletions(-) diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 06d89abf77..39ee1e8822 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -79,6 +79,8 @@ #include #include +#include + #include #include @@ -99,6 +101,8 @@ #define ADDR_READ_MR 0x00 /* write to this address to start conversion */ /* Measurement rate is 100Hz */ +#define MEAS_RATE 100.0f +#define MEAS_DRIVER_FILTER_FREQ 3.0f #define CONVERSION_INTERVAL (1000000 / 100) /* microseconds */ class MEASAirspeed : public Airspeed @@ -116,6 +120,7 @@ protected: virtual int measure(); virtual int collect(); + math::LowPassFilter2p _filter; }; /* @@ -124,7 +129,8 @@ protected: extern "C" __EXPORT int meas_airspeed_main(int argc, char *argv[]); MEASAirspeed::MEASAirspeed(int bus, int address, const char* path) : Airspeed(bus, address, - CONVERSION_INTERVAL, path) + CONVERSION_INTERVAL, path), + _filter(MEAS_RATE, MEAS_DRIVER_FILTER_FREQ) { } @@ -208,6 +214,7 @@ MEASAirspeed::collect() 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.voltage = 0; report.max_differential_pressure_pa = _max_differential_pressure_pa; diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 1365d9e705..a128f46c77 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1142,6 +1142,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_SENS.baro_alt = buf.sensor.baro_alt_meter; log_msg.body.log_SENS.baro_temp = buf.sensor.baro_temp_celcius; log_msg.body.log_SENS.diff_pres = buf.sensor.differential_pressure_pa; + log_msg.body.log_SENS.diff_pres_filtered = buf.sensor.differential_pressure_filtered_pa; LOGBUFFER_WRITE_AND_COUNT(SENS); } } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 16bfc355d6..8a8c3c84cc 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -92,6 +92,7 @@ struct log_SENS_s { float baro_alt; float baro_temp; float diff_pres; + float diff_pres_filtered; }; /* --- LPOS - LOCAL POSITION --- */ @@ -306,7 +307,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(ATT, "fffffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"), LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), - LOG_FORMAT(SENS, "ffff", "BaroPres,BaroAlt,BaroTemp,DiffPres"), + LOG_FORMAT(SENS, "ffff", "BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"), LOG_FORMAT(LPOS, "ffffffLLfBBB", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt,XYFlags,ZFlags,Landed"), LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index b176d7417a..4ee04d462f 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1028,6 +1028,7 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &_diff_pres); raw.differential_pressure_pa = _diff_pres.differential_pressure_pa; + raw.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa; raw.differential_pressure_counter++; _airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_pa); diff --git a/src/modules/uORB/topics/differential_pressure.h b/src/modules/uORB/topics/differential_pressure.h index 5d94d42883..3592c023c5 100644 --- a/src/modules/uORB/topics/differential_pressure.h +++ b/src/modules/uORB/topics/differential_pressure.h @@ -54,8 +54,9 @@ struct differential_pressure_s { uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ uint64_t error_count; - float differential_pressure_pa; /**< Differential pressure reading */ - float max_differential_pressure_pa; /**< Maximum differential pressure reading */ + float differential_pressure_pa; /**< Differential pressure reading */ + float differential_pressure_filtered_pa; /**< Low pass filtered differential pressure reading */ + float max_differential_pressure_pa; /**< Maximum differential pressure reading */ float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */ float temperature; /**< Temperature provided by sensor */ diff --git a/src/modules/uORB/topics/sensor_combined.h b/src/modules/uORB/topics/sensor_combined.h index ad164555e2..c84fd5db71 100644 --- a/src/modules/uORB/topics/sensor_combined.h +++ b/src/modules/uORB/topics/sensor_combined.h @@ -103,8 +103,9 @@ struct sensor_combined_s { float mcu_temp_celcius; /**< Internal temperature measurement of MCU */ uint32_t baro_counter; /**< Number of raw baro measurements taken */ - float differential_pressure_pa; /**< Airspeed sensor differential pressure */ - uint32_t differential_pressure_counter; /**< Number of raw differential pressure measurements taken */ + float differential_pressure_pa; /**< Airspeed sensor differential pressure */ + float differential_pressure_filtered_pa; /**< Low pass filtered airspeed sensor differential pressure reading */ + uint32_t differential_pressure_counter; /**< Number of raw differential pressure measurements taken */ }; /** From 6b79f533388ab78ddbbb8cc316b38c6e7403e46a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 14 Mar 2014 14:40:54 +0100 Subject: [PATCH 189/381] mavlink stream: do not use getopt as it leads to problems with the global optarg variable --- src/modules/mavlink/mavlink_main.cpp | 37 +++++++++++++--------------- 1 file changed, 17 insertions(+), 20 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index d722bec469..f1a1f95683 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1538,7 +1538,7 @@ Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate) /* copy stream name */ unsigned n = strlen(stream_name) + 1; char *s = new char[n]; - memcpy(s, stream_name, n); + strcpy(s, stream_name); /* set subscription task */ _subscribe_to_stream_rate = rate; @@ -1959,36 +1959,33 @@ Mavlink::stream(int argc, char *argv[]) const char *stream_name = nullptr; int ch; - argc -= 1; - argv += 1; + argc -= 2; + argv += 2; /* don't exit from getopt loop to leave getopt global variables in consistent state, * set error flag instead */ bool err_flag = false; - while ((ch = getopt(argc, argv, "r:d:s:")) != EOF) { - switch (ch) { - case 'r': - rate = strtod(optarg, nullptr); + int i = 0; + while (i < argc) { + if (0 == strcmp(argv[i], "-r") && i < argc - 1 ) { + rate = strtod(argv[i+1], nullptr); if (rate < 0.0f) { err_flag = true; } - - break; - - case 'd': - device_name = optarg; - break; - - case 's': - stream_name = optarg; - break; - - default: + i++; + } else if (0 == strcmp(argv[i], "-d") && i < argc - 1 ) { + device_name = argv[i+1]; + i++; + } else if (0 == strcmp(argv[i], "-s") && i < argc - 1 ) { + stream_name = argv[i+1]; + i++; + } else { err_flag = true; - break; } + + i++; } if (!err_flag && rate >= 0.0 && stream_name != nullptr) { From d2a74dff673e2d3f49fc98dea601be753bd18416 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 15 Mar 2014 13:11:46 +0100 Subject: [PATCH 190/381] fix logging of filtered diff pressure signal --- src/modules/sdlog2/sdlog2_messages.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 27bbce354d..fe500ad5f5 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -307,7 +307,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(ATT, "fffffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"), LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), - LOG_FORMAT(SENS, "ffff", "BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"), + LOG_FORMAT(SENS, "fffff", "BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"), LOG_FORMAT(LPOS, "ffffffLLfBBB", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt,XYFlags,ZFlags,Landed"), LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), From be349b989a7a716a4e8d5ff4143f79124d7abc76 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 15 Mar 2014 13:24:17 +0100 Subject: [PATCH 191/381] sensors: use filtered diff pressure to calculate airspeed --- src/modules/sensors/sensors.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 4ee04d462f..d62177c85d 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1031,8 +1031,8 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) raw.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa; raw.differential_pressure_counter++; - _airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_pa); - _airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_pa + raw.baro_pres_mbar * 1e2f, + _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, raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG); /* announce the airspeed if needed, just publish else */ From f4c91b19e47f8d8e0ee890dc605ee675a3592bad Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 15 Mar 2014 13:46:09 +0100 Subject: [PATCH 192/381] set phantom autoconfig parameters as provided by Andreas Antener --- ROMFS/px4fmu_common/init.d/3031_phantom | 31 ++++++++++++++++++++++++- 1 file changed, 30 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/3031_phantom b/ROMFS/px4fmu_common/init.d/3031_phantom index a3004d1e13..6cbd236430 100644 --- a/ROMFS/px4fmu_common/init.d/3031_phantom +++ b/ROMFS/px4fmu_common/init.d/3031_phantom @@ -2,9 +2,38 @@ # # Phantom FPV Flying Wing # -# Simon Wilks +# Simon Wilks , Thomas Gubler # sh /etc/init.d/rc.fw_defaults +if [ $DO_AUTOCONFIG == yes ] +then + param set FW_AIRSPD_MIN 13 + param set FW_AIRSPD_TRIM 18 + param set FW_AIRSPD_MAX 40 + param set FW_ATT_TC 0.3 + param set FW_L1_DAMPING 0.75 + param set FW_L1_PERIOD 15 + param set FW_PR_FF 0.2 + param set FW_PR_I 0.005 + param set FW_PR_IMAX 0.2 + 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 0 + param set FW_RR_FF 0.5 + param set FW_RR_I 0.02 + param set FW_RR_IMAX 0.2 + param set FW_RR_P 0.08 + param set FW_R_LIM 70 + param set FW_R_RMAX 0 + param set FW_T_HRATE_P 0.01 + param set FW_T_RLL2THR 15 + param set FW_T_SRATE_P 0.01 + param set FW_T_TIME_CONST 5 +fi + set MIXER FMU_Q From 75c168f13fea4c413dc7168a8d5696380c3e9ed5 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 15 Mar 2014 15:28:37 +0100 Subject: [PATCH 193/381] add missing $ in rcS script --- ROMFS/px4fmu_common/init.d/rcS | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 55e2a886e1..7c3524fef0 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -466,7 +466,7 @@ then sh /etc/init.d/rc.interface # Start standard fixedwing apps - if [ LOAD_DEFAULT_APPS == yes ] + if [ $LOAD_DEFAULT_APPS == yes ] then sh /etc/init.d/rc.fw_apps fi @@ -525,7 +525,7 @@ then sh /etc/init.d/rc.interface # Start standard multicopter apps - if [ LOAD_DEFAULT_APPS == yes ] + if [ $LOAD_DEFAULT_APPS == yes ] then sh /etc/init.d/rc.mc_apps fi From 717e1bd374e7710ce579e91c45852bbba906eba8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Mar 2014 13:47:26 +0100 Subject: [PATCH 194/381] Removed stupid sensor counter, replaced it with much more useful timestamps --- .../attitude_estimator_ekf_main.cpp | 10 ++---- .../attitude_estimator_so3_main.cpp | 10 ++---- .../position_estimator_inav_main.c | 16 +++++----- src/modules/sdlog2/sdlog2.c | 31 ++++++++++--------- src/modules/sensors/sensors.cpp | 10 +++--- src/modules/uORB/topics/sensor_combined.h | 17 +++++----- 6 files changed, 43 insertions(+), 51 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 00bd23f838..11ae2a2683 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -277,7 +277,6 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds // XXX write this out to perf regs /* keep track of sensor updates */ - uint32_t sensor_last_count[3] = {0, 0, 0}; uint64_t sensor_last_timestamp[3] = {0, 0, 0}; struct attitude_estimator_ekf_params ekf_params; @@ -380,9 +379,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds uint8_t update_vect[3] = {0, 0, 0}; /* Fill in gyro measurements */ - if (sensor_last_count[0] != raw.gyro_counter) { + if (sensor_last_timestamp[0] != raw.timestamp) { update_vect[0] = 1; - sensor_last_count[0] = raw.gyro_counter; sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]); sensor_last_timestamp[0] = raw.timestamp; } @@ -392,9 +390,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds z_k[2] = raw.gyro_rad_s[2] - gyro_offsets[2]; /* update accelerometer measurements */ - if (sensor_last_count[1] != raw.accelerometer_counter) { + if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) { update_vect[1] = 1; - sensor_last_count[1] = raw.accelerometer_counter; sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); sensor_last_timestamp[1] = raw.timestamp; } @@ -445,9 +442,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds z_k[5] = raw.accelerometer_m_s2[2] - acc(2); /* update magnetometer measurements */ - if (sensor_last_count[2] != raw.magnetometer_counter) { + if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) { update_vect[2] = 1; - sensor_last_count[2] = raw.magnetometer_counter; sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); sensor_last_timestamp[2] = raw.timestamp; } diff --git a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp index e79726613d..7150218ffc 100755 --- a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp +++ b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp @@ -445,7 +445,6 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[]) // XXX write this out to perf regs /* keep track of sensor updates */ - uint32_t sensor_last_count[3] = {0, 0, 0}; uint64_t sensor_last_timestamp[3] = {0, 0, 0}; struct attitude_estimator_so3_params so3_comp_params; @@ -526,9 +525,8 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[]) uint8_t update_vect[3] = {0, 0, 0}; /* Fill in gyro measurements */ - if (sensor_last_count[0] != raw.gyro_counter) { + if (sensor_last_timestamp[0] != raw.timestamp) { update_vect[0] = 1; - sensor_last_count[0] = raw.gyro_counter; sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]); sensor_last_timestamp[0] = raw.timestamp; } @@ -538,9 +536,8 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[]) gyro[2] = raw.gyro_rad_s[2] - gyro_offsets[2]; /* update accelerometer measurements */ - if (sensor_last_count[1] != raw.accelerometer_counter) { + if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) { update_vect[1] = 1; - sensor_last_count[1] = raw.accelerometer_counter; sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); sensor_last_timestamp[1] = raw.timestamp; } @@ -550,9 +547,8 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[]) acc[2] = raw.accelerometer_m_s2[2]; /* update magnetometer measurements */ - if (sensor_last_count[2] != raw.magnetometer_counter) { + if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) { update_vect[2] = 1; - sensor_last_count[2] = raw.magnetometer_counter; sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); sensor_last_timestamp[2] = raw.timestamp; } 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 a14354138b..368fa6ee25 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -200,8 +200,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) bool landed = true; hrt_abstime landed_time = 0; - uint32_t accel_counter = 0; - uint32_t baro_counter = 0; + hrt_abstime accel_timestamp = 0; + hrt_abstime baro_timestamp = 0; bool ref_inited = false; hrt_abstime ref_init_start = 0; @@ -310,8 +310,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (fds_init[0].revents & POLLIN) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); - if (wait_baro && sensor.baro_counter != baro_counter) { - baro_counter = sensor.baro_counter; + if (wait_baro && sensor.baro_timestamp != baro_timestamp) { + baro_timestamp = sensor.baro_timestamp; /* mean calculation over several measurements */ if (baro_init_cnt < baro_init_num) { @@ -384,7 +384,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); - if (sensor.accelerometer_counter != accel_counter) { + if (sensor.accelerometer_timestamp != accel_timestamp) { if (att.R_valid) { /* correct accel bias */ sensor.accelerometer_m_s2[0] -= acc_bias[0]; @@ -408,13 +408,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) memset(corr_acc, 0, sizeof(corr_acc)); } - accel_counter = sensor.accelerometer_counter; + accel_timestamp = sensor.accelerometer_timestamp; accel_updates++; } - if (sensor.baro_counter != baro_counter) { + if (sensor.baro_timestamp != baro_timestamp) { corr_baro = baro_offset - sensor.baro_alt_meter - z_est[0]; - baro_counter = sensor.baro_counter; + baro_timestamp = sensor.baro_timestamp; baro_updates++; } } diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 2514bafeee..ee3ec72166 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -887,11 +887,11 @@ int sdlog2_thread_main(int argc, char *argv[]) pthread_cond_init(&logbuffer_cond, NULL); /* track changes in sensor_combined topic */ - uint16_t gyro_counter = 0; - uint16_t accelerometer_counter = 0; - uint16_t magnetometer_counter = 0; - uint16_t baro_counter = 0; - uint16_t differential_pressure_counter = 0; + hrt_abstime gyro_timestamp = 0; + hrt_abstime accelerometer_timestamp = 0; + hrt_abstime magnetometer_timestamp = 0; + hrt_abstime barometer_timestamp = 0; + hrt_abstime differential_pressure_timestamp = 0; /* track changes in distance status */ bool dist_bottom_present = false; @@ -976,28 +976,28 @@ int sdlog2_thread_main(int argc, char *argv[]) bool write_IMU = false; bool write_SENS = false; - if (buf.sensor.gyro_counter != gyro_counter) { - gyro_counter = buf.sensor.gyro_counter; + if (buf.sensor.timestamp != gyro_timestamp) { + gyro_timestamp = buf.sensor.timestamp; write_IMU = true; } - if (buf.sensor.accelerometer_counter != accelerometer_counter) { - accelerometer_counter = buf.sensor.accelerometer_counter; + if (buf.sensor.accelerometer_timestamp != accelerometer_timestamp) { + accelerometer_timestamp = buf.sensor.accelerometer_timestamp; write_IMU = true; } - if (buf.sensor.magnetometer_counter != magnetometer_counter) { - magnetometer_counter = buf.sensor.magnetometer_counter; + if (buf.sensor.magnetometer_timestamp != magnetometer_timestamp) { + magnetometer_timestamp = buf.sensor.magnetometer_timestamp; write_IMU = true; } - if (buf.sensor.baro_counter != baro_counter) { - baro_counter = buf.sensor.baro_counter; + if (buf.sensor.baro_timestamp != barometer_timestamp) { + barometer_timestamp = buf.sensor.baro_timestamp; write_SENS = true; } - if (buf.sensor.differential_pressure_counter != differential_pressure_counter) { - differential_pressure_counter = buf.sensor.differential_pressure_counter; + if (buf.sensor.differential_pressure_timestamp != differential_pressure_timestamp) { + differential_pressure_timestamp = buf.sensor.differential_pressure_timestamp; write_SENS = true; } @@ -1023,6 +1023,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_SENS.diff_pres = buf.sensor.differential_pressure_pa; LOGBUFFER_WRITE_AND_COUNT(SENS); } + } /* --- ATTITUDE --- */ diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index b176d7417a..76dc262f13 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -940,7 +940,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) raw.accelerometer_raw[1] = accel_report.y_raw; raw.accelerometer_raw[2] = accel_report.z_raw; - raw.accelerometer_counter++; + raw.accelerometer_timestamp = accel_report.timestamp; } } @@ -966,7 +966,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) raw.gyro_raw[1] = gyro_report.y_raw; raw.gyro_raw[2] = gyro_report.z_raw; - raw.gyro_counter++; + raw.timestamp = gyro_report.timestamp; } } @@ -996,7 +996,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw) raw.magnetometer_raw[1] = mag_report.y_raw; raw.magnetometer_raw[2] = mag_report.z_raw; - raw.magnetometer_counter++; + raw.magnetometer_timestamp = mag_report.timestamp; } } @@ -1014,7 +1014,7 @@ Sensors::baro_poll(struct sensor_combined_s &raw) raw.baro_alt_meter = _barometer.altitude; // Altitude in meters raw.baro_temp_celcius = _barometer.temperature; // Temperature in degrees celcius - raw.baro_counter++; + raw.baro_timestamp = _barometer.timestamp; } } @@ -1028,7 +1028,7 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &_diff_pres); raw.differential_pressure_pa = _diff_pres.differential_pressure_pa; - raw.differential_pressure_counter++; + raw.differential_pressure_timestamp = _diff_pres.timestamp; _airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_pa); _airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_pa + raw.baro_pres_mbar * 1e2f, diff --git a/src/modules/uORB/topics/sensor_combined.h b/src/modules/uORB/topics/sensor_combined.h index ad164555e2..92812efd49 100644 --- a/src/modules/uORB/topics/sensor_combined.h +++ b/src/modules/uORB/topics/sensor_combined.h @@ -77,34 +77,33 @@ struct sensor_combined_s { /* NOTE: Ordering of fields optimized to align to 32 bit / 4 bytes Change with consideration only */ - uint64_t timestamp; /**< Timestamp in microseconds since boot */ + uint64_t timestamp; /**< Timestamp in microseconds since boot, from gyro */ int16_t gyro_raw[3]; /**< Raw sensor values of angular velocity */ - uint16_t gyro_counter; /**< Number of raw measurments taken */ float gyro_rad_s[3]; /**< Angular velocity in radian per seconds */ - + int16_t accelerometer_raw[3]; /**< Raw acceleration in NED body frame */ - uint32_t accelerometer_counter; /**< Number of raw acc measurements taken */ float accelerometer_m_s2[3]; /**< Acceleration in NED body frame, in m/s^2 */ int accelerometer_mode; /**< Accelerometer measurement mode */ float accelerometer_range_m_s2; /**< Accelerometer measurement range in m/s^2 */ + uint64_t accelerometer_timestamp; /**< Accelerometer timestamp */ int16_t magnetometer_raw[3]; /**< Raw magnetic field in NED body frame */ float magnetometer_ga[3]; /**< Magnetic field in NED body frame, in Gauss */ int magnetometer_mode; /**< Magnetometer measurement mode */ float magnetometer_range_ga; /**< ± measurement range in Gauss */ float magnetometer_cuttoff_freq_hz; /**< Internal analog low pass frequency of sensor */ - uint32_t magnetometer_counter; /**< Number of raw mag measurements taken */ - + uint64_t magnetometer_timestamp; /**< Magnetometer timestamp */ + float baro_pres_mbar; /**< Barometric pressure, already temp. comp. */ float baro_alt_meter; /**< Altitude, already temp. comp. */ float baro_temp_celcius; /**< Temperature in degrees celsius */ float adc_voltage_v[4]; /**< ADC voltages of ADC Chan 10/11/12/13 or -1 */ float mcu_temp_celcius; /**< Internal temperature measurement of MCU */ - uint32_t baro_counter; /**< Number of raw baro measurements taken */ + uint64_t baro_timestamp; /**< Barometer timestamp */ - float differential_pressure_pa; /**< Airspeed sensor differential pressure */ - uint32_t differential_pressure_counter; /**< Number of raw differential pressure measurements taken */ + float differential_pressure_pa; /**< Airspeed sensor differential pressure */ + uint64_t differential_pressure_timestamp; /**< Last measurement timestamp */ }; /** From 3874bca2084bb88dcd739b309bd4a7929db3b417 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Mar 2014 13:48:33 +0100 Subject: [PATCH 195/381] mavlink: Only send messages when we have updates for them. --- src/modules/mavlink/mavlink_messages.cpp | 462 +++++++++--------- .../mavlink/mavlink_orb_subscription.cpp | 21 +- .../mavlink/mavlink_orb_subscription.h | 9 + src/modules/mavlink/mavlink_receiver.cpp | 9 +- 4 files changed, 276 insertions(+), 225 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 4d83afe825..7d388f88da 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -216,8 +216,8 @@ protected: void send(const hrt_abstime t) { - status_sub->update(t); - pos_sp_triplet_sub->update(t); + (void)status_sub->update(t); + (void)pos_sp_triplet_sub->update(t); uint8_t mavlink_state = 0; uint8_t mavlink_base_mode = 0; @@ -261,22 +261,23 @@ protected: void send(const hrt_abstime t) { - status_sub->update(t); + if (status_sub->update(t)) { - mavlink_msg_sys_status_send(_channel, - status->onboard_control_sensors_present, - status->onboard_control_sensors_enabled, - status->onboard_control_sensors_health, - status->load * 1000.0f, - status->battery_voltage * 1000.0f, - status->battery_current * 1000.0f, - status->battery_remaining, - status->drop_rate_comm, - status->errors_comm, - status->errors_count1, - status->errors_count2, - status->errors_count3, - status->errors_count4); + mavlink_msg_sys_status_send(_channel, + status->onboard_control_sensors_present, + status->onboard_control_sensors_enabled, + status->onboard_control_sensors_health, + status->load * 1000.0f, + status->battery_voltage * 1000.0f, + status->battery_current * 1000.0f, + status->battery_remaining, + status->drop_rate_comm, + status->errors_comm, + status->errors_count1, + status->errors_count2, + status->errors_count3, + status->errors_count4); + } } }; @@ -284,7 +285,7 @@ protected: class MavlinkStreamHighresIMU : public MavlinkStream { public: - MavlinkStreamHighresIMU() : MavlinkStream(), accel_counter(0), gyro_counter(0), mag_counter(0), baro_counter(0) + MavlinkStreamHighresIMU() : MavlinkStream(), accel_timestamp(0), gyro_timestamp(0), mag_timestamp(0), baro_timestamp(0) { } @@ -302,10 +303,10 @@ private: MavlinkOrbSubscription *sensor_sub; struct sensor_combined_s *sensor; - uint32_t accel_counter; - uint32_t gyro_counter; - uint32_t mag_counter; - uint32_t baro_counter; + uint64_t accel_timestamp; + uint64_t gyro_timestamp; + uint64_t mag_timestamp; + uint64_t baro_timestamp; protected: void subscribe(Mavlink *mavlink) @@ -316,42 +317,43 @@ protected: void send(const hrt_abstime t) { - sensor_sub->update(t); + if (sensor_sub->update(t)) { - uint16_t fields_updated = 0; + uint16_t fields_updated = 0; - if (accel_counter != sensor->accelerometer_counter) { - /* mark first three dimensions as changed */ - fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); - accel_counter = sensor->accelerometer_counter; + if (accel_timestamp != sensor->accelerometer_timestamp) { + /* mark first three dimensions as changed */ + fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); + accel_timestamp = sensor->accelerometer_timestamp; + } + + if (gyro_timestamp != sensor->timestamp) { + /* mark second group dimensions as changed */ + fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); + gyro_timestamp = sensor->timestamp; + } + + if (mag_timestamp != sensor->magnetometer_timestamp) { + /* mark third group dimensions as changed */ + fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); + mag_timestamp = sensor->magnetometer_timestamp; + } + + if (baro_timestamp != sensor->baro_timestamp) { + /* mark last group dimensions as changed */ + fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); + baro_timestamp = sensor->baro_timestamp; + } + + mavlink_msg_highres_imu_send(_channel, + sensor->timestamp, + sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2], + sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2], + sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2], + sensor->baro_pres_mbar, sensor->differential_pressure_pa, + sensor->baro_alt_meter, sensor->baro_temp_celcius, + fields_updated); } - - if (gyro_counter != sensor->gyro_counter) { - /* mark second group dimensions as changed */ - fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); - gyro_counter = sensor->gyro_counter; - } - - if (mag_counter != sensor->magnetometer_counter) { - /* mark third group dimensions as changed */ - fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); - mag_counter = sensor->magnetometer_counter; - } - - if (baro_counter != sensor->baro_counter) { - /* mark last group dimensions as changed */ - fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); - baro_counter = sensor->baro_counter; - } - - mavlink_msg_highres_imu_send(_channel, - sensor->timestamp, - sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2], - sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2], - sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2], - sensor->baro_pres_mbar, sensor->differential_pressure_pa, - sensor->baro_alt_meter, sensor->baro_temp_celcius, - fields_updated); } }; @@ -382,12 +384,13 @@ protected: void send(const hrt_abstime t) { - att_sub->update(t); + if (att_sub->update(t)) { - mavlink_msg_attitude_send(_channel, - att->timestamp / 1000, - att->roll, att->pitch, att->yaw, - att->rollspeed, att->pitchspeed, att->yawspeed); + mavlink_msg_attitude_send(_channel, + att->timestamp / 1000, + att->roll, att->pitch, att->yaw, + att->rollspeed, att->pitchspeed, att->yawspeed); + } } }; @@ -418,17 +421,18 @@ protected: void send(const hrt_abstime t) { - att_sub->update(t); + if (att_sub->update(t)) { - mavlink_msg_attitude_quaternion_send(_channel, - att->timestamp / 1000, - att->q[0], - att->q[1], - att->q[2], - att->q[3], - att->rollspeed, - att->pitchspeed, - att->yawspeed); + mavlink_msg_attitude_quaternion_send(_channel, + att->timestamp / 1000, + att->q[0], + att->q[1], + att->q[2], + att->q[3], + att->rollspeed, + att->pitchspeed, + att->yawspeed); + } } }; @@ -483,23 +487,26 @@ protected: void send(const hrt_abstime t) { - att_sub->update(t); - pos_sub->update(t); - armed_sub->update(t); - act_sub->update(t); - airspeed_sub->update(t); + bool updated = att_sub->update(t); + updated |= pos_sub->update(t); + updated |= armed_sub->update(t); + updated |= act_sub->update(t); + updated |= airspeed_sub->update(t); - float groundspeed = sqrtf(pos->vel_n * pos->vel_n + pos->vel_e * pos->vel_e); - uint16_t heading = _wrap_2pi(att->yaw) * M_RAD_TO_DEG_F; - float throttle = armed->armed ? act->control[3] * 100.0f : 0.0f; + if (updated) { - mavlink_msg_vfr_hud_send(_channel, - airspeed->true_airspeed_m_s, - groundspeed, - heading, - throttle, - pos->alt, - -pos->vel_d); + float groundspeed = sqrtf(pos->vel_n * pos->vel_n + pos->vel_e * pos->vel_e); + uint16_t heading = _wrap_2pi(att->yaw) * M_RAD_TO_DEG_F; + float throttle = armed->armed ? act->control[3] * 100.0f : 0.0f; + + mavlink_msg_vfr_hud_send(_channel, + airspeed->true_airspeed_m_s, + groundspeed, + heading, + throttle, + pos->alt, + -pos->vel_d); + } } }; @@ -530,19 +537,20 @@ protected: void send(const hrt_abstime t) { - gps_sub->update(t); + if (gps_sub->update(t)) { - mavlink_msg_gps_raw_int_send(_channel, - gps->timestamp_position, - gps->fix_type, - gps->lat, - gps->lon, - gps->alt, - cm_uint16_from_m_float(gps->eph_m), - cm_uint16_from_m_float(gps->epv_m), - gps->vel_m_s * 100.0f, - _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f, - gps->satellites_visible); + mavlink_msg_gps_raw_int_send(_channel, + gps->timestamp_position, + gps->fix_type, + gps->lat, + gps->lon, + gps->alt, + cm_uint16_from_m_float(gps->eph_m), + cm_uint16_from_m_float(gps->epv_m), + gps->vel_m_s * 100.0f, + _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f, + gps->satellites_visible); + } } }; @@ -579,10 +587,11 @@ protected: void send(const hrt_abstime t) { - pos_sub->update(t); - home_sub->update(t); + bool updated = pos_sub->update(t); + updated |= home_sub->update(t); - mavlink_msg_global_position_int_send(_channel, + if (updated) { + mavlink_msg_global_position_int_send(_channel, pos->timestamp / 1000, pos->lat * 1e7, pos->lon * 1e7, @@ -592,6 +601,7 @@ protected: pos->vel_e * 100.0f, pos->vel_d * 100.0f, _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f); + } } }; @@ -622,16 +632,17 @@ protected: void send(const hrt_abstime t) { - pos_sub->update(t); + if (pos_sub->update(t)) { - mavlink_msg_local_position_ned_send(_channel, - pos->timestamp / 1000, - pos->x, - pos->y, - pos->z, - pos->vx, - pos->vy, - pos->vz); + mavlink_msg_local_position_ned_send(_channel, + pos->timestamp / 1000, + pos->x, + pos->y, + pos->z, + pos->vx, + pos->vy, + pos->vz); + } } }; @@ -662,12 +673,17 @@ protected: void send(const hrt_abstime t) { - home_sub->update(t); - mavlink_msg_gps_global_origin_send(_channel, - (int32_t)(home->lat * 1e7), - (int32_t)(home->lon * 1e7), - (int32_t)(home->alt) * 1000.0f); + /* we're sending the GPS home periodically to ensure the + * the GCS does pick it up at one point */ + if (home_sub->is_published()) { + home_sub->update(t); + + mavlink_msg_gps_global_origin_send(_channel, + (int32_t)(home->lat * 1e7), + (int32_t)(home->lon * 1e7), + (int32_t)(home->alt) * 1000.0f); + } } }; @@ -713,19 +729,20 @@ protected: void send(const hrt_abstime t) { - act_sub->update(t); + if (act_sub->update(t)) { - mavlink_msg_servo_output_raw_send(_channel, - act->timestamp / 1000, - _n, - act->output[0], - act->output[1], - act->output[2], - act->output[3], - act->output[4], - act->output[5], - act->output[6], - act->output[7]); + mavlink_msg_servo_output_raw_send(_channel, + act->timestamp / 1000, + _n, + act->output[0], + act->output[1], + act->output[2], + act->output[3], + act->output[4], + act->output[5], + act->output[6], + act->output[7]); + } } }; @@ -768,57 +785,60 @@ protected: void send(const hrt_abstime t) { - status_sub->update(t); - pos_sp_triplet_sub->update(t); - act_sub->update(t); + bool updated = status_sub->update(t); + updated |= pos_sp_triplet_sub->update(t); + updated |= act_sub->update(t); - /* translate the current syste state to mavlink state and mode */ - uint8_t mavlink_state; - uint8_t mavlink_base_mode; - uint32_t mavlink_custom_mode; - get_mavlink_mode_state(status, pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); + if (updated) { - /* set number of valid outputs depending on vehicle type */ - unsigned n; + /* translate the current syste state to mavlink state and mode */ + uint8_t mavlink_state; + uint8_t mavlink_base_mode; + uint32_t mavlink_custom_mode; + get_mavlink_mode_state(status, pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); - switch (mavlink_system.type) { - case MAV_TYPE_QUADROTOR: - n = 4; - break; + /* set number of valid outputs depending on vehicle type */ + unsigned n; - case MAV_TYPE_HEXAROTOR: - n = 6; - break; + switch (mavlink_system.type) { + case MAV_TYPE_QUADROTOR: + n = 4; + break; - default: - n = 8; - break; - } + case MAV_TYPE_HEXAROTOR: + n = 6; + break; - /* scale / assign outputs depending on system type */ - float out[8]; + default: + n = 8; + break; + } - for (unsigned i = 0; i < 8; i++) { - if (i < n) { - if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { - /* scale fake PWM out 900..1200 us to 0..1*/ - out[i] = (act->output[i] - 900.0f) / 1200.0f; + /* scale / assign outputs depending on system type */ + float out[8]; + + for (unsigned i = 0; i < 8; i++) { + if (i < n) { + if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { + /* scale fake PWM out 900..1200 us to 0..1*/ + out[i] = (act->output[i] - 900.0f) / 1200.0f; + + } else { + /* send 0 when disarmed */ + out[i] = 0.0f; + } } else { - /* send 0 when disarmed */ - out[i] = 0.0f; + out[i] = -1.0f; } - - } else { - out[i] = -1.0f; } - } - mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], - mavlink_base_mode, - 0); + mavlink_msg_hil_controls_send(_channel, + hrt_absolute_time(), + out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], + mavlink_base_mode, + 0); + } } }; @@ -849,14 +869,15 @@ protected: void send(const hrt_abstime t) { - pos_sp_triplet_sub->update(t); + if (pos_sp_triplet_sub->update(t)) { - mavlink_msg_global_position_setpoint_int_send(_channel, - MAV_FRAME_GLOBAL, - (int32_t)(pos_sp_triplet->current.lat * 1e7), - (int32_t)(pos_sp_triplet->current.lon * 1e7), - (int32_t)(pos_sp_triplet->current.alt * 1000), - (int16_t)(pos_sp_triplet->current.yaw * M_RAD_TO_DEG_F * 100.0f)); + mavlink_msg_global_position_setpoint_int_send(_channel, + MAV_FRAME_GLOBAL, + (int32_t)(pos_sp_triplet->current.lat * 1e7), + (int32_t)(pos_sp_triplet->current.lon * 1e7), + (int32_t)(pos_sp_triplet->current.alt * 1000), + (int16_t)(pos_sp_triplet->current.yaw * M_RAD_TO_DEG_F * 100.0f)); + } } }; @@ -925,14 +946,15 @@ protected: void send(const hrt_abstime t) { - att_sp_sub->update(t); + if (att_sp_sub->update(t)) { - mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel, - att_sp->timestamp / 1000, - att_sp->roll_body, - att_sp->pitch_body, - att_sp->yaw_body, - att_sp->thrust); + mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel, + att_sp->timestamp / 1000, + att_sp->roll_body, + att_sp->pitch_body, + att_sp->yaw_body, + att_sp->thrust); + } } }; @@ -963,14 +985,15 @@ protected: void send(const hrt_abstime t) { - att_rates_sp_sub->update(t); + if (att_rates_sp_sub->update(t)) { - mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel, - att_rates_sp->timestamp / 1000, - att_rates_sp->roll, - att_rates_sp->pitch, - att_rates_sp->yaw, - att_rates_sp->thrust); + mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel, + att_rates_sp->timestamp / 1000, + att_rates_sp->roll, + att_rates_sp->pitch, + att_rates_sp->yaw, + att_rates_sp->thrust); + } } }; @@ -1001,24 +1024,25 @@ protected: void send(const hrt_abstime t) { - rc_sub->update(t); + if (rc_sub->update(t)) { - const unsigned port_width = 8; + const unsigned port_width = 8; - for (unsigned i = 0; (i * port_width) < rc->channel_count; i++) { - /* Channels are sent in MAVLink main loop at a fixed interval */ - mavlink_msg_rc_channels_raw_send(_channel, - rc->timestamp_publication / 1000, - i, - (rc->channel_count > (i * port_width) + 0) ? rc->values[(i * port_width) + 0] : UINT16_MAX, - (rc->channel_count > (i * port_width) + 1) ? rc->values[(i * port_width) + 1] : UINT16_MAX, - (rc->channel_count > (i * port_width) + 2) ? rc->values[(i * port_width) + 2] : UINT16_MAX, - (rc->channel_count > (i * port_width) + 3) ? rc->values[(i * port_width) + 3] : UINT16_MAX, - (rc->channel_count > (i * port_width) + 4) ? rc->values[(i * port_width) + 4] : UINT16_MAX, - (rc->channel_count > (i * port_width) + 5) ? rc->values[(i * port_width) + 5] : UINT16_MAX, - (rc->channel_count > (i * port_width) + 6) ? rc->values[(i * port_width) + 6] : UINT16_MAX, - (rc->channel_count > (i * port_width) + 7) ? rc->values[(i * port_width) + 7] : UINT16_MAX, - rc->rssi); + for (unsigned i = 0; (i * port_width) < rc->channel_count; i++) { + /* Channels are sent in MAVLink main loop at a fixed interval */ + mavlink_msg_rc_channels_raw_send(_channel, + rc->timestamp_publication / 1000, + i, + (rc->channel_count > (i * port_width) + 0) ? rc->values[(i * port_width) + 0] : UINT16_MAX, + (rc->channel_count > (i * port_width) + 1) ? rc->values[(i * port_width) + 1] : UINT16_MAX, + (rc->channel_count > (i * port_width) + 2) ? rc->values[(i * port_width) + 2] : UINT16_MAX, + (rc->channel_count > (i * port_width) + 3) ? rc->values[(i * port_width) + 3] : UINT16_MAX, + (rc->channel_count > (i * port_width) + 4) ? rc->values[(i * port_width) + 4] : UINT16_MAX, + (rc->channel_count > (i * port_width) + 5) ? rc->values[(i * port_width) + 5] : UINT16_MAX, + (rc->channel_count > (i * port_width) + 6) ? rc->values[(i * port_width) + 6] : UINT16_MAX, + (rc->channel_count > (i * port_width) + 7) ? rc->values[(i * port_width) + 7] : UINT16_MAX, + rc->rssi); + } } } }; @@ -1050,15 +1074,16 @@ protected: void send(const hrt_abstime t) { - manual_sub->update(t); + if (manual_sub->update(t)) { - mavlink_msg_manual_control_send(_channel, - mavlink_system.sysid, - manual->roll * 1000, - manual->pitch * 1000, - manual->yaw * 1000, - manual->throttle * 1000, - 0); + mavlink_msg_manual_control_send(_channel, + mavlink_system.sysid, + manual->roll * 1000, + manual->pitch * 1000, + manual->yaw * 1000, + manual->throttle * 1000, + 0); + } } }; @@ -1089,15 +1114,16 @@ protected: void send(const hrt_abstime t) { - flow_sub->update(t); + if (flow_sub->update(t)) { - mavlink_msg_optical_flow_send(_channel, - flow->timestamp, - flow->sensor_id, - flow->flow_raw_x, flow->flow_raw_y, - flow->flow_comp_x_m, flow->flow_comp_y_m, - flow->quality, - flow->ground_distance_m); + mavlink_msg_optical_flow_send(_channel, + flow->timestamp, + flow->sensor_id, + flow->flow_raw_x, flow->flow_raw_y, + flow->flow_comp_x_m, flow->flow_comp_y_m, + flow->quality, + flow->ground_distance_m); + } } }; diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp index 6279e5366a..9963184683 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.cpp +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -46,11 +46,15 @@ #include "mavlink_orb_subscription.h" -MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) : _topic(topic), _last_check(0), next(nullptr) +MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) : + _fd(orb_subscribe(_topic)), + _published(false), + _topic(topic), + _last_check(0), + next(nullptr) { _data = malloc(topic->o_size); memset(_data, 0, topic->o_size); - _fd = orb_subscribe(_topic); } MavlinkOrbSubscription::~MavlinkOrbSubscription() @@ -87,3 +91,16 @@ MavlinkOrbSubscription::update(const hrt_abstime t) return false; } + +bool +MavlinkOrbSubscription::is_published() +{ + bool updated; + orb_check(_fd, &updated); + + if (updated) { + _published = true; + } + + return _published; +} diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h index eacc270349..42d47e96ed 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.h +++ b/src/modules/mavlink/mavlink_orb_subscription.h @@ -54,12 +54,21 @@ public: ~MavlinkOrbSubscription(); bool update(const hrt_abstime t); + + /** + * Check if the topic has been published. + * + * This call will return true if the topic was ever published. + * @param true if the topic has been published at least once. + */ + bool is_published(); void *get_data(); const orb_id_t get_topic(); private: const orb_id_t _topic; int _fd; + bool _published; void *_data; hrt_abstime _last_check; }; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index c222a3ddf4..2ce86396cf 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -586,7 +586,6 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) hil_sensors.gyro_rad_s[0] = imu.xgyro; hil_sensors.gyro_rad_s[1] = imu.ygyro; hil_sensors.gyro_rad_s[2] = imu.zgyro; - hil_sensors.gyro_counter = _hil_counter; hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2; hil_sensors.accelerometer_raw[1] = imu.yacc / mg2ms2; @@ -596,7 +595,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) hil_sensors.accelerometer_m_s2[2] = imu.zacc; hil_sensors.accelerometer_mode = 0; // TODO what is this? hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16 - hil_sensors.accelerometer_counter = _hil_counter; + hil_sensors.accelerometer_timestamp = timestamp; hil_sensors.adc_voltage_v[0] = 0.0f; hil_sensors.adc_voltage_v[1] = 0.0f; @@ -611,15 +610,15 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) hil_sensors.magnetometer_range_ga = 32.7f; // int16 hil_sensors.magnetometer_mode = 0; // TODO what is this hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f; - hil_sensors.magnetometer_counter = _hil_counter; + hil_sensors.magnetometer_timestamp = timestamp; hil_sensors.baro_pres_mbar = imu.abs_pressure; hil_sensors.baro_alt_meter = imu.pressure_alt; hil_sensors.baro_temp_celcius = imu.temperature; - hil_sensors.baro_counter = _hil_counter; + hil_sensors.baro_timestamp = timestamp; hil_sensors.differential_pressure_pa = imu.diff_pressure * 1e2f; //from hPa to Pa - hil_sensors.differential_pressure_counter = _hil_counter; + hil_sensors.differential_pressure_timestamp = timestamp; /* publish combined sensor topic */ if (_sensors_pub > 0) { From df5d702baefae7f51161d170c911cb398f418538 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 16 Mar 2014 17:03:39 +0400 Subject: [PATCH 196/381] mavlink: MavlinkOrbSubscription.update() result fixed --- src/modules/mavlink/mavlink_orb_subscription.cpp | 13 ++++++++----- src/modules/mavlink/mavlink_orb_subscription.h | 13 +++++++------ 2 files changed, 15 insertions(+), 11 deletions(-) diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp index 9963184683..e8f9bb75ba 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.cpp +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -78,12 +78,15 @@ MavlinkOrbSubscription::get_data() bool MavlinkOrbSubscription::update(const hrt_abstime t) { - if (_last_check != t) { - _last_check = t; - bool updated; - orb_check(_fd, &updated); + if (_last_check == t) { + /* already checked right now, return result of the check */ + return _updated; - if (updated) { + } else { + _last_check = t; + orb_check(_fd, &_updated); + + if (_updated) { orb_copy(_topic, _fd, _data); return true; } diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h index 42d47e96ed..8529721c03 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.h +++ b/src/modules/mavlink/mavlink_orb_subscription.h @@ -48,7 +48,7 @@ class MavlinkOrbSubscription { public: - MavlinkOrbSubscription *next; + MavlinkOrbSubscription *next; /*< pointer to next subscription in list */ MavlinkOrbSubscription(const orb_id_t topic); ~MavlinkOrbSubscription(); @@ -66,11 +66,12 @@ public: const orb_id_t get_topic(); private: - const orb_id_t _topic; - int _fd; - bool _published; - void *_data; - hrt_abstime _last_check; + const orb_id_t _topic; /*< topic metadata */ + int _fd; /*< subscription handle */ + bool _published; /*< topic was ever published */ + void *_data; /*< pointer to data buffer */ + hrt_abstime _last_check; /*< time of last check */ + bool _updated; /*< updated on last check */ }; From 762e8f52893afee6e8ffb6a728e5bc88518829c3 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 16 Mar 2014 17:28:34 +0400 Subject: [PATCH 197/381] mavlink_orb_subscription: minor optimization and comment fix --- src/modules/mavlink/mavlink_orb_subscription.cpp | 4 ++++ src/modules/mavlink/mavlink_orb_subscription.h | 2 +- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp index e8f9bb75ba..4de7228324 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.cpp +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -98,6 +98,10 @@ MavlinkOrbSubscription::update(const hrt_abstime t) bool MavlinkOrbSubscription::is_published() { + if (_published) { + return true; + } + bool updated; orb_check(_fd, &updated); diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h index 8529721c03..5c6543e813 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.h +++ b/src/modules/mavlink/mavlink_orb_subscription.h @@ -59,7 +59,7 @@ public: * Check if the topic has been published. * * This call will return true if the topic was ever published. - * @param true if the topic has been published at least once. + * @return true if the topic has been published at least once. */ bool is_published(); void *get_data(); From 15c079921bf2f8256e0fcdd5ab9f31157bc09f86 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Mar 2014 15:12:03 +0100 Subject: [PATCH 198/381] Fixed missing parent ioctl call --- src/drivers/gps/gps.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index a736cbdf6b..785c8beeb0 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -229,10 +229,15 @@ GPS::ioctl(struct file *filp, int cmd, unsigned long arg) int ret = OK; switch (cmd) { - case SENSORIOCRESET: - cmd_reset(); + case SENSORIOCRESET: + cmd_reset(); + break; + } + + default: + /* give it to parent if no one wants it */ + ret = CDev::ioctl(filp, cmd, arg); break; - } unlock(); From 85b7670b44d0af1141fae73593f68d8469846d8d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Mar 2014 15:15:31 +0100 Subject: [PATCH 199/381] compile fix --- src/drivers/gps/gps.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 785c8beeb0..c72f692d73 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -229,15 +229,15 @@ GPS::ioctl(struct file *filp, int cmd, unsigned long arg) int ret = OK; switch (cmd) { - case SENSORIOCRESET: - cmd_reset(); - break; - } + case SENSORIOCRESET: + cmd_reset(); + break; - default: + default: /* give it to parent if no one wants it */ ret = CDev::ioctl(filp, cmd, arg); break; + } unlock(); From c1ee1abf274749900a39279f2fed57ad890c2865 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 16 Mar 2014 14:47:29 +0100 Subject: [PATCH 200/381] introduce TBS caipirinha autostart script --- .../px4fmu_common/init.d/3100_tbs_caipirinha | 42 +++++++++++++++++++ ROMFS/px4fmu_common/init.d/rc.autostart | 5 +++ 2 files changed, 47 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha diff --git a/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha b/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha new file mode 100644 index 0000000000..7dbda54d3a --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha @@ -0,0 +1,42 @@ +#!nsh +# +# TBS Caipirinha Flying Wing +# +# Thomas Gubler +# + +sh /etc/init.d/rc.fw_defaults + +if [ $DO_AUTOCONFIG == yes ] +then + + # TODO: these are the X5 default parameters, update them to the caipi + + param set FW_AIRSPD_MIN 15 + param set FW_AIRSPD_TRIM 20 + param set FW_AIRSPD_MAX 40 + param set FW_ATT_TC 0.3 + param set FW_L1_DAMPING 0.74 + param set FW_L1_PERIOD 15 + param set FW_PR_FF 0.3 + 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 + param set FW_RR_IMAX 0.2 + param set FW_RR_P 0.03 + param set FW_R_LIM 60 + param set FW_R_RMAX 0 + param set FW_T_HRATE_P 0.01 + param set FW_T_RLL2THR 15 + param set FW_T_SRATE_P 0.01 + param set FW_T_TIME_CONST 5 +fi + +set MIXER FMU_Q diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 7aaf7133ef..4ae6a27940 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -97,6 +97,11 @@ then sh /etc/init.d/3034_fx79 fi +if param compare SYS_AUTOSTART 3100 +then + sh /etc/init.d/3100_tbs_caipirinha +fi + # # Quad X # From 8818425e584c9670dfa9a823b3dfd95147c087d4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 16 Mar 2014 18:25:09 +0400 Subject: [PATCH 201/381] commander: fixed message formatting when disabling sensors publishing in HIL mode --- 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 31955d3e5d..bf49403d02 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -350,7 +350,7 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 0); close(sensfd); - printf("Disabling %s\n: %s", direntry->d_name, (!block_ret) ? "OK" : "FAIL"); + printf("Disabling %s: %s\n", direntry->d_name, (block_ret == OK) ? "OK" : "ERROR"); } closedir(d); From bb7962936e15c594821ae10e4510febfec0d6b9f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Mar 2014 18:37:16 +0100 Subject: [PATCH 202/381] CDev: Fixed printf format specifier --- src/drivers/device/cdev.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/device/cdev.cpp b/src/drivers/device/cdev.cpp index b157b3f181..6e2ebb1f7b 100644 --- a/src/drivers/device/cdev.cpp +++ b/src/drivers/device/cdev.cpp @@ -124,7 +124,7 @@ CDev::register_class_devname(const char *class_devname) if (ret == OK) break; } else { char name[32]; - snprintf(name, sizeof(name), "%s%u", class_devname, class_instance); + snprintf(name, sizeof(name), "%s%d", class_devname, class_instance); ret = register_driver(name, &fops, 0666, (void *)this); if (ret == OK) break; } From 772fce9f82749bde8c14199fcc73cfa2235e760e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Mar 2014 18:38:55 +0100 Subject: [PATCH 203/381] commander: Improve HIL publication blocking call handling --- src/modules/commander/state_machine_helper.cpp | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 31955d3e5d..131d6a550d 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -309,10 +309,7 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s bool valid_transition = false; int ret = ERROR; - warnx("Current state: %d, requested state: %d", current_status->hil_state, new_state); - if (current_status->hil_state == new_state) { - warnx("Hil state not changed"); valid_transition = true; } else { @@ -347,10 +344,16 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s while ((direntry = readdir(d)) != NULL) { int sensfd = ::open(direntry->d_name, 0); - int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 0); + + if (sensfd < 0) { + warn("failed opening device"); + return 1; + } + + int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 1); close(sensfd); - printf("Disabling %s\n: %s", direntry->d_name, (!block_ret) ? "OK" : "FAIL"); + printf("Disabling %s: %s\n", direntry->d_name, (block_ret == OK) ? "OK" : "FAIL"); } closedir(d); From 8383603f7691e3bc66534b58654c4d13547632a2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Mar 2014 18:41:03 +0100 Subject: [PATCH 204/381] commander: Linting: Reduce scope of variable --- src/modules/commander/state_machine_helper.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 131d6a550d..5537c4e801 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -337,10 +337,11 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s /* list directory */ DIR *d; - struct dirent *direntry; d = opendir("/dev"); if (d) { + struct dirent *direntry; + while ((direntry = readdir(d)) != NULL) { int sensfd = ::open(direntry->d_name, 0); From 4466dbf0b37f0a80790c346affee1e4ac86bef22 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 16 Mar 2014 18:47:21 +0100 Subject: [PATCH 205/381] add SYS_USE_IO param which allows using standard startup scripts for FMU only mode --- ROMFS/px4fmu_common/init.d/rcS | 11 ++++++++++- src/modules/systemlib/system_params.c | 11 +++++++++++ 2 files changed, 21 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 7c3524fef0..65a66a085c 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -108,7 +108,6 @@ then set HIL no set VEHICLE_TYPE none set MIXER none - set USE_IO yes set OUTPUT_MODE none set PWM_OUTPUTS none set PWM_RATE none @@ -129,6 +128,16 @@ then else set DO_AUTOCONFIG no fi + + # + # Set USE_IO flag + # + if param compare SYS_USE_IO 1 + then + set USE_IO yes + else + set USE_IO no + fi # # Set parameters and env variables for selected AUTOSTART diff --git a/src/modules/systemlib/system_params.c b/src/modules/systemlib/system_params.c index cb35a25413..ec2bc3a9a0 100644 --- a/src/modules/systemlib/system_params.c +++ b/src/modules/systemlib/system_params.c @@ -60,3 +60,14 @@ PARAM_DEFINE_INT32(SYS_AUTOSTART, 0); * @group System */ PARAM_DEFINE_INT32(SYS_AUTOCONFIG, 0); + +/** + * Set usage of IO board + * + * Can be used to use a standard startup script but with a FMU only set-up. Set to 0 to force the FMU only set-up. + * + * @min 0 + * @max 1 + * @group System + */ +PARAM_DEFINE_INT32(SYS_USE_IO, 1); From 6db2191a71ed0ea38e1fd18886371f099c14d593 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Mar 2014 18:50:20 +0100 Subject: [PATCH 206/381] commander: Skip devices we do not want to touch in HIL --- .../commander/state_machine_helper.cpp | 19 +++++++++++++++++-- 1 file changed, 17 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 1e892ed77f..9178bcc5ab 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -351,6 +351,23 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s return 1; } + /* skip serial ports */ + if (!strncmp("tty", direntry-d_name, 3)) { + continue; + } + /* skip mtd devices */ + if (!strncmp("mtd", direntry-d_name, 3)) { + continue; + } + /* skip ram devices */ + if (!strncmp("ram", direntry-d_name, 3)) { + continue; + } + /* skip mavlink */ + if (!strcmp("mavlink", direntry-d_name)) { + continue; + } + int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 1); close(sensfd); @@ -359,8 +376,6 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s closedir(d); - warnx("directory listing ok (FS mounted and readable)"); - } else { /* failed opening dir */ warnx("FAILED LISTING DEVICE ROOT DIRECTORY"); From 2fe9f65c45ba3aaad549b30e3b34568d07ab2b20 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Mar 2014 19:16:18 +0100 Subject: [PATCH 207/381] commander: Fixed compile error and some stupidity in usage of path names --- .../commander/state_machine_helper.cpp | 40 +++++++++++++------ 1 file changed, 28 insertions(+), 12 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 9178bcc5ab..e6f245cf93 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -44,6 +44,7 @@ #include #include #include +#include #include #include @@ -341,37 +342,52 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s if (d) { struct dirent *direntry; + char devname[24]; while ((direntry = readdir(d)) != NULL) { - int sensfd = ::open(direntry->d_name, 0); - - if (sensfd < 0) { - warn("failed opening device"); - return 1; - } - /* skip serial ports */ - if (!strncmp("tty", direntry-d_name, 3)) { + if (!strncmp("tty", direntry->d_name, 3)) { continue; } /* skip mtd devices */ - if (!strncmp("mtd", direntry-d_name, 3)) { + if (!strncmp("mtd", direntry->d_name, 3)) { continue; } /* skip ram devices */ - if (!strncmp("ram", direntry-d_name, 3)) { + if (!strncmp("ram", direntry->d_name, 3)) { + continue; + } + /* skip MMC devices */ + if (!strncmp("mmc", direntry->d_name, 3)) { continue; } /* skip mavlink */ - if (!strcmp("mavlink", direntry-d_name)) { + if (!strcmp("mavlink", direntry->d_name)) { + continue; + } + /* skip console */ + if (!strcmp("console", direntry->d_name)) { + continue; + } + /* skip null */ + if (!strcmp("null", direntry->d_name)) { + continue; + } + + snprintf(devname, sizeof(devname), "/dev/%s", direntry->d_name); + + int sensfd = ::open(devname, 0); + + if (sensfd < 0) { + warn("failed opening device %s", devname); continue; } int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 1); close(sensfd); - printf("Disabling %s: %s\n", direntry->d_name, (block_ret == OK) ? "OK" : "ERROR"); + printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR"); } closedir(d); From 2db7d30400d0240baf4532eeef6cb13403417654 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Mar 2014 19:43:09 +0100 Subject: [PATCH 208/381] Do not work on USB UARTs --- src/modules/mavlink/mavlink_main.cpp | 31 +++++++++++++++++----------- src/modules/mavlink/mavlink_main.h | 1 + 2 files changed, 20 insertions(+), 12 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 11882f0f40..9c2e0178a3 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -201,6 +201,7 @@ Mavlink::Mavlink() : _mavlink_fd(-1), _task_running(false), _hil_enabled(false), + _is_usb_uart(false), _main_loop_delay(1000), _subscriptions(nullptr), _streams(nullptr), @@ -577,17 +578,19 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * return -1; } - /* - * Setup hardware flow control. If the port has no RTS pin this call will fail, - * which is not an issue, but requires a separate call so we can fail silently. - */ - (void)tcgetattr(_uart_fd, &uart_config); - uart_config.c_cflag |= CRTS_IFLOW; - (void)tcsetattr(_uart_fd, TCSANOW, &uart_config); + if (!_is_usb_uart) { + /* + * Setup hardware flow control. If the port has no RTS pin this call will fail, + * which is not an issue, but requires a separate call so we can fail silently. + */ + (void)tcgetattr(_uart_fd, &uart_config); + uart_config.c_cflag |= CRTS_IFLOW; + (void)tcsetattr(_uart_fd, TCSANOW, &uart_config); - /* setup output flow control */ - if (enable_flow_control(true)) { - warnx("ERR FLOW CTRL EN"); + /* setup output flow control */ + if (enable_flow_control(true)) { + warnx("ERR FLOW CTRL EN"); + } } return _uart_fd; @@ -596,6 +599,11 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * int Mavlink::enable_flow_control(bool enabled) { + // We can't do this on USB - skip + if (_is_usb_uart) { + return OK; + } + struct termios uart_config; int ret = tcgetattr(_uart_fd, &uart_config); if (enabled) { @@ -1701,10 +1709,9 @@ Mavlink::task_main(int argc, char *argv[]) fflush(stdout); struct termios uart_config_original; - bool usb_uart; /* default values for arguments */ - _uart_fd = mavlink_open_uart(_baudrate, _device_name, &uart_config_original, &usb_uart); + _uart_fd = mavlink_open_uart(_baudrate, _device_name, &uart_config_original, &_is_usb_uart); if (_uart_fd < 0) { warn("could not open %s", _device_name); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index e6e20eb938..f743c25044 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -208,6 +208,7 @@ private: /* states */ bool _hil_enabled; /**< Hardware In the Loop mode */ + bool _is_usb_uart; /**< Port is USB */ unsigned _main_loop_delay; /**< mainloop delay, depends on data rate */ From 1c0d7988e258510d2a4006b63b266a421ae5152c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Mar 2014 21:38:37 +0100 Subject: [PATCH 209/381] Defaults for Wing Wing --- ROMFS/px4fmu_common/init.d/3033_wingwing | 24 ++++++++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/3033_wingwing b/ROMFS/px4fmu_common/init.d/3033_wingwing index 2af3618d98..22d63fad57 100644 --- a/ROMFS/px4fmu_common/init.d/3033_wingwing +++ b/ROMFS/px4fmu_common/init.d/3033_wingwing @@ -7,4 +7,28 @@ sh /etc/init.d/rc.fw_defaults +if [ $DO_AUTOCONFIG == yes ] +then + param set BAT_N_CELLS 2 + param set FW_AIRSPD_MAX 15 + param set FW_AIRSPD_MIN 7 + param set FW_AIRSPD_TRIM 11 + param set FW_ATT_TC 0.3 + param set FW_L1_DAMPING 0.74 + param set FW_L1_PERIOD 12 + param set FW_LND_ANG 15 + param set FW_LND_FLALT 5 + param set FW_LND_HHDIST 15 + param set FW_LND_HVIRT 13 + param set FW_LND_TLALT 5 + param set FW_THR_LND_MAX 0 + param set FW_P_ROLLFF 2 + param set FW_PR_FF 0.6 + param set FW_PR_IMAX 0.2 + param set FW_PR_P 0.06 + param set FW_RR_FF 0.6 + param set FW_RR_IMA 0.2 + param set FW_RR_P 0.09 +fi + set MIXER FMU_Q From ffd0d1032041601916e2bd2faa395ca0af03659c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 17 Mar 2014 13:25:22 +0100 Subject: [PATCH 210/381] Fix usage of right time stamps --- .../attitude_estimator_ekf/attitude_estimator_ekf_main.cpp | 4 ++-- .../attitude_estimator_so3/attitude_estimator_so3_main.cpp | 4 ++-- 2 files changed, 4 insertions(+), 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 11ae2a2683..10a6cd2c5c 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -393,7 +393,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) { update_vect[1] = 1; sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); - sensor_last_timestamp[1] = raw.timestamp; + sensor_last_timestamp[1] = raw.accelerometer_timestamp; } hrt_abstime vel_t = 0; @@ -445,7 +445,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) { update_vect[2] = 1; sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); - sensor_last_timestamp[2] = raw.timestamp; + sensor_last_timestamp[2] = raw.magnetometer_timestamp; } z_k[6] = raw.magnetometer_ga[0]; diff --git a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp index 7150218ffc..3653defa66 100755 --- a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp +++ b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp @@ -539,7 +539,7 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[]) if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) { update_vect[1] = 1; sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); - sensor_last_timestamp[1] = raw.timestamp; + sensor_last_timestamp[1] = raw.accelerometer_timestamp; } acc[0] = raw.accelerometer_m_s2[0]; @@ -550,7 +550,7 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[]) if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) { update_vect[2] = 1; sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); - sensor_last_timestamp[2] = raw.timestamp; + sensor_last_timestamp[2] = raw.magnetometer_timestamp; } mag[0] = raw.magnetometer_ga[0]; From 8cb5a12cc702ac238f4dc79bf1f1cee3fdee005f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 17 Mar 2014 13:33:23 +0100 Subject: [PATCH 211/381] Remove now unused hil_counter --- src/modules/mavlink/mavlink_receiver.cpp | 3 --- src/modules/mavlink/mavlink_receiver.h | 1 - 2 files changed, 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 2ce86396cf..71ea832fdd 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -108,8 +108,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _telemetry_status_pub(-1), _rc_pub(-1), _manual_pub(-1), - - _hil_counter(0), _hil_frames(0), _old_timestamp(0), _hil_local_proj_inited(0), @@ -647,7 +645,6 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) } /* increment counters */ - _hil_counter++; _hil_frames++; /* print HIL sensors rate */ diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index beaae20587..0a5a1b5c7a 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -139,7 +139,6 @@ private: orb_advert_t _telemetry_status_pub; orb_advert_t _rc_pub; orb_advert_t _manual_pub; - int _hil_counter; int _hil_frames; uint64_t _old_timestamp; bool _hil_local_proj_inited; From dbd467fe1f3779b45090f556fd05b056f068cd34 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 17 Mar 2014 13:33:49 +0100 Subject: [PATCH 212/381] sensors: Remove effect-less writing of timestamp - was anyway correctly written by gyro update --- src/modules/sensors/sensors.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 76dc262f13..0ed26b54cb 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1592,8 +1592,7 @@ Sensors::task_main() /* check parameters for updates */ parameter_update_poll(); - /* store the time closest to all measurements (this is bogus, sensor timestamps should be propagated...) */ - raw.timestamp = hrt_absolute_time(); + /* the timestamp of the raw struct is updated by the gyro_poll() method */ /* copy most recent sensor data */ gyro_poll(raw); From 0581c7f712355a6726e84b302667ddc8cc9f6fa7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 17 Mar 2014 14:24:31 +0100 Subject: [PATCH 213/381] Make PX4IO driver obey HIL as it should --- src/drivers/px4io/px4io.cpp | 21 ++++++++++++--------- 1 file changed, 12 insertions(+), 9 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 8e990aa6f0..82f3ba044a 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1332,12 +1332,15 @@ PX4IO::io_handle_battery(uint16_t vbatt, uint16_t ibatt) battery_status.discharged_mah = _battery_mamphour_total; _battery_last_timestamp = battery_status.timestamp; - /* lazily publish the battery voltage */ - if (_to_battery > 0) { - orb_publish(ORB_ID(battery_status), _to_battery, &battery_status); + /* the announced battery status would conflict with the simulated battery status in HIL */ + if (!(_pub_blocked)) { + /* lazily publish the battery voltage */ + if (_to_battery > 0) { + orb_publish(ORB_ID(battery_status), _to_battery, &battery_status); - } else { - _to_battery = orb_advertise(ORB_ID(battery_status), &battery_status); + } else { + _to_battery = orb_advertise(ORB_ID(battery_status), &battery_status); + } } } @@ -1959,8 +1962,7 @@ PX4IO::print_status() } int -PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) -/* Make it obvious that file * isn't used here */ +PX4IO::ioctl(file * filep, int cmd, unsigned long arg) { int ret = OK; @@ -2372,8 +2374,9 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) break; default: - /* not a recognized value */ - ret = -ENOTTY; + /* see if the parent class can make any use of it */ + ret = CDev::ioctl(filep, cmd, arg); + break; } return ret; From f7bd31b6fd949cf9bac11cdcda0748138d623be8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 17 Mar 2014 14:32:02 +0100 Subject: [PATCH 214/381] Teach RGB led driver to forward unknown IOCTLs --- src/drivers/rgbled/rgbled.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index 4f58891ed3..e390140de3 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -242,6 +242,8 @@ RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg) return OK; default: + /* see if the parent class can make any use of it */ + ret = CDev::ioctl(filep, cmd, arg); break; } From 533e3172dc37aeeb2bc4ccc4ee884239d91079f2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 17 Mar 2014 14:24:31 +0100 Subject: [PATCH 215/381] Make PX4IO driver obey HIL as it should --- src/drivers/px4io/px4io.cpp | 21 ++++++++++++--------- 1 file changed, 12 insertions(+), 9 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 8e990aa6f0..82f3ba044a 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1332,12 +1332,15 @@ PX4IO::io_handle_battery(uint16_t vbatt, uint16_t ibatt) battery_status.discharged_mah = _battery_mamphour_total; _battery_last_timestamp = battery_status.timestamp; - /* lazily publish the battery voltage */ - if (_to_battery > 0) { - orb_publish(ORB_ID(battery_status), _to_battery, &battery_status); + /* the announced battery status would conflict with the simulated battery status in HIL */ + if (!(_pub_blocked)) { + /* lazily publish the battery voltage */ + if (_to_battery > 0) { + orb_publish(ORB_ID(battery_status), _to_battery, &battery_status); - } else { - _to_battery = orb_advertise(ORB_ID(battery_status), &battery_status); + } else { + _to_battery = orb_advertise(ORB_ID(battery_status), &battery_status); + } } } @@ -1959,8 +1962,7 @@ PX4IO::print_status() } int -PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) -/* Make it obvious that file * isn't used here */ +PX4IO::ioctl(file * filep, int cmd, unsigned long arg) { int ret = OK; @@ -2372,8 +2374,9 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) break; default: - /* not a recognized value */ - ret = -ENOTTY; + /* see if the parent class can make any use of it */ + ret = CDev::ioctl(filep, cmd, arg); + break; } return ret; From 9cdb416d3a6ceb1c74b03f3f046f5d904bf7fc45 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 17 Mar 2014 14:32:02 +0100 Subject: [PATCH 216/381] Teach RGB led driver to forward unknown IOCTLs --- src/drivers/rgbled/rgbled.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index 4f58891ed3..e390140de3 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -242,6 +242,8 @@ RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg) return OK; default: + /* see if the parent class can make any use of it */ + ret = CDev::ioctl(filep, cmd, arg); break; } From 75ad1c4a1350d73315f48a1f4b9bdbee2d20fa9e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 17 Mar 2014 14:35:07 +0100 Subject: [PATCH 217/381] Completely and properly populate battery status message in HIL --- 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 71ea832fdd..489d2bdcb7 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -851,7 +851,9 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) hil_battery_status.timestamp = timestamp; hil_battery_status.voltage_v = 11.1f; + hil_battery_status.voltage_filtered_v = 11.1f; hil_battery_status.current_a = 10.0f; + hil_battery_status.discharged_mah = -1.0f; if (_battery_pub > 0) { orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); From 51658ac857390f20d3cc9e2e59a485516c4e0814 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 17 Mar 2014 14:37:32 +0100 Subject: [PATCH 218/381] Compile fix --- src/drivers/rgbled/rgbled.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index e390140de3..13cbfdfa8b 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -243,7 +243,7 @@ RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg) default: /* see if the parent class can make any use of it */ - ret = CDev::ioctl(filep, cmd, arg); + ret = CDev::ioctl(filp, cmd, arg); break; } From 7718bbebee2730b3b6e4b036f71bc39a4b8414e1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 17 Mar 2014 15:20:00 +0100 Subject: [PATCH 219/381] startup scripts: romfs_pruner doesn't know the inch symbols when used in eclipse, therefore replace these --- ROMFS/px4fmu_common/init.d/11001_hexa_cox | 2 +- ROMFS/px4fmu_common/init.d/12001_octo_cox | 2 +- ROMFS/px4fmu_common/init.d/4001_quad_x | 2 +- ROMFS/px4fmu_common/init.d/5001_quad_+ | 2 +- ROMFS/px4fmu_common/init.d/6001_hexa_x | 2 +- ROMFS/px4fmu_common/init.d/7001_hexa_+ | 2 +- ROMFS/px4fmu_common/init.d/8001_octo_x | 2 +- ROMFS/px4fmu_common/init.d/9001_octo_+ | 2 +- 8 files changed, 8 insertions(+), 8 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/11001_hexa_cox b/ROMFS/px4fmu_common/init.d/11001_hexa_cox index b98ab47748..38e65435b6 100644 --- a/ROMFS/px4fmu_common/init.d/11001_hexa_cox +++ b/ROMFS/px4fmu_common/init.d/11001_hexa_cox @@ -2,7 +2,7 @@ # # UNTESTED UNTESTED! # -# Generic 10” Hexa coaxial geometry +# Generic 10" Hexa coaxial geometry # # Lorenz Meier # diff --git a/ROMFS/px4fmu_common/init.d/12001_octo_cox b/ROMFS/px4fmu_common/init.d/12001_octo_cox index 655cb6226e..99f902a6dd 100644 --- a/ROMFS/px4fmu_common/init.d/12001_octo_cox +++ b/ROMFS/px4fmu_common/init.d/12001_octo_cox @@ -1,6 +1,6 @@ #!nsh # -# Generic 10” Octo coaxial geometry +# Generic 10" Octo coaxial geometry # # Lorenz Meier # diff --git a/ROMFS/px4fmu_common/init.d/4001_quad_x b/ROMFS/px4fmu_common/init.d/4001_quad_x index 345b0e3e4e..06c54a41db 100644 --- a/ROMFS/px4fmu_common/init.d/4001_quad_x +++ b/ROMFS/px4fmu_common/init.d/4001_quad_x @@ -1,6 +1,6 @@ #!nsh # -# Generic 10” Quad X geometry +# Generic 10" Quad X geometry # # Lorenz Meier # diff --git a/ROMFS/px4fmu_common/init.d/5001_quad_+ b/ROMFS/px4fmu_common/init.d/5001_quad_+ index 55b31067d1..1fb25e5d88 100644 --- a/ROMFS/px4fmu_common/init.d/5001_quad_+ +++ b/ROMFS/px4fmu_common/init.d/5001_quad_+ @@ -1,6 +1,6 @@ #!nsh # -# Generic 10” Quad + geometry +# Generic 10" Quad + geometry # # Anton Babushkin # diff --git a/ROMFS/px4fmu_common/init.d/6001_hexa_x b/ROMFS/px4fmu_common/init.d/6001_hexa_x index 7714a508cf..34fc6523fe 100644 --- a/ROMFS/px4fmu_common/init.d/6001_hexa_x +++ b/ROMFS/px4fmu_common/init.d/6001_hexa_x @@ -1,6 +1,6 @@ #!nsh # -# Generic 10” Hexa X geometry +# Generic 10" Hexa X geometry # # Anton Babushkin # diff --git a/ROMFS/px4fmu_common/init.d/7001_hexa_+ b/ROMFS/px4fmu_common/init.d/7001_hexa_+ index 60db8c0690..235e376a6e 100644 --- a/ROMFS/px4fmu_common/init.d/7001_hexa_+ +++ b/ROMFS/px4fmu_common/init.d/7001_hexa_+ @@ -1,6 +1,6 @@ #!nsh # -# Generic 10” Hexa + geometry +# Generic 10" Hexa + geometry # # Anton Babushkin # diff --git a/ROMFS/px4fmu_common/init.d/8001_octo_x b/ROMFS/px4fmu_common/init.d/8001_octo_x index 411aee1140..769217dc78 100644 --- a/ROMFS/px4fmu_common/init.d/8001_octo_x +++ b/ROMFS/px4fmu_common/init.d/8001_octo_x @@ -1,6 +1,6 @@ #!nsh # -# Generic 10” Octo X geometry +# Generic 10" Octo X geometry # # Anton Babushkin # diff --git a/ROMFS/px4fmu_common/init.d/9001_octo_+ b/ROMFS/px4fmu_common/init.d/9001_octo_+ index 81132fd3e7..28b074a545 100644 --- a/ROMFS/px4fmu_common/init.d/9001_octo_+ +++ b/ROMFS/px4fmu_common/init.d/9001_octo_+ @@ -1,6 +1,6 @@ #!nsh # -# Generic 10” Octo + geometry +# Generic 10" Octo + geometry # # Anton Babushkin # From 4038d3e04eb28295b020ca935d6e4833611aa7e8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 17 Mar 2014 15:28:08 +0100 Subject: [PATCH 220/381] Write out yaw into position topics --- src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 082c695dc9..def437b351 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -931,6 +931,7 @@ FixedwingEstimator::task_main() _local_pos.xy_global = true; _local_pos.z_global = false; + _local_pos.yaw = _att.yaw; /* lazily publish the local position only once available */ if (_local_pos_pub > 0) { From fd8cae19bdb4ba5e97c810f266f78af1461b51b7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 17 Mar 2014 16:53:30 +0100 Subject: [PATCH 221/381] ADC channel mapping fix in sensors app --- src/modules/sensors/sensors.cpp | 11 +++++++---- src/modules/uORB/topics/sensor_combined.h | 3 ++- 2 files changed, 9 insertions(+), 5 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index b176d7417a..6560dec3d6 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1169,16 +1169,19 @@ Sensors::adc_poll(struct sensor_combined_s &raw) hrt_abstime t = hrt_absolute_time(); /* rate limit to 100 Hz */ if (t - _last_adc >= 10000) { - /* make space for a maximum of eight channels */ - struct adc_msg_s buf_adc[8]; + /* make space for a maximum of ten channels (to ensure reading all channels at once) */ + struct adc_msg_s buf_adc[10]; /* read all channels available */ int ret = read(_fd_adc, &buf_adc, sizeof(buf_adc)); if (ret >= (int)sizeof(buf_adc[0])) { - for (unsigned i = 0; i < sizeof(buf_adc) / sizeof(buf_adc[0]); i++) { + + /* Read add channels we got */ + for (unsigned i = 0; i < ret / sizeof(buf_adc[0]); i++) { /* Save raw voltage values */ - if (i < (sizeof(raw.adc_voltage_v)) / sizeof(raw.adc_voltage_v[0])) { + if (i < (sizeof(raw.adc_voltage_v) / sizeof(raw.adc_voltage_v[0]))) { raw.adc_voltage_v[i] = buf_adc[i].am_data / (4096.0f / 3.3f); + raw.adc_mapping[i] = buf_adc[i].am_channel; } /* look for specific channels and process the raw voltage to measurement data */ diff --git a/src/modules/uORB/topics/sensor_combined.h b/src/modules/uORB/topics/sensor_combined.h index ad164555e2..918389f38b 100644 --- a/src/modules/uORB/topics/sensor_combined.h +++ b/src/modules/uORB/topics/sensor_combined.h @@ -99,7 +99,8 @@ struct sensor_combined_s { float baro_pres_mbar; /**< Barometric pressure, already temp. comp. */ float baro_alt_meter; /**< Altitude, already temp. comp. */ float baro_temp_celcius; /**< Temperature in degrees celsius */ - float adc_voltage_v[4]; /**< ADC voltages of ADC Chan 10/11/12/13 or -1 */ + float adc_voltage_v[9]; /**< ADC voltages of ADC Chan 10/11/12/13 or -1 */ + unsigned adc_mapping[9]; /**< Channel indices of each of these values */ float mcu_temp_celcius; /**< Internal temperature measurement of MCU */ uint32_t baro_counter; /**< Number of raw baro measurements taken */ From 4e6e40e5d850f737b25d657630ac89ad1d54e67c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 17 Mar 2014 16:54:39 +0100 Subject: [PATCH 222/381] Fix up ADC tests command to read all channels at once --- src/systemcmds/tests/test_adc.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/systemcmds/tests/test_adc.c b/src/systemcmds/tests/test_adc.c index 030ac6e23c..7302f63cce 100644 --- a/src/systemcmds/tests/test_adc.c +++ b/src/systemcmds/tests/test_adc.c @@ -66,8 +66,8 @@ int test_adc(int argc, char *argv[]) } for (unsigned i = 0; i < 5; i++) { - /* make space for a maximum of eight channels */ - struct adc_msg_s data[8]; + /* make space for a maximum of ten channels */ + struct adc_msg_s data[10]; /* read all channels available */ ssize_t count = read(fd, data, sizeof(data)); From d940bdf7f64268d46503cf4b0b996bec3036603e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 17 Mar 2014 17:02:10 +0100 Subject: [PATCH 223/381] Consistently making space for a maximum of 12 channels, 10 channels (including reference / temperature) are used / available right now --- src/drivers/stm32/adc/adc.cpp | 2 +- src/modules/sensors/sensors.cpp | 4 ++-- src/modules/uORB/topics/sensor_combined.h | 4 ++-- src/systemcmds/tests/test_adc.c | 4 ++-- 4 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp index 00e46d6b82..0b8a275e6a 100644 --- a/src/drivers/stm32/adc/adc.cpp +++ b/src/drivers/stm32/adc/adc.cpp @@ -341,7 +341,7 @@ test(void) err(1, "can't open ADC device"); for (unsigned i = 0; i < 50; i++) { - adc_msg_s data[10]; + adc_msg_s data[12]; ssize_t count = read(fd, data, sizeof(data)); if (count < 0) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 6560dec3d6..8da5bb2946 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1169,8 +1169,8 @@ Sensors::adc_poll(struct sensor_combined_s &raw) hrt_abstime t = hrt_absolute_time(); /* rate limit to 100 Hz */ if (t - _last_adc >= 10000) { - /* make space for a maximum of ten channels (to ensure reading all channels at once) */ - struct adc_msg_s buf_adc[10]; + /* make space for a maximum of twelve channels (to ensure reading all channels at once) */ + struct adc_msg_s buf_adc[12]; /* read all channels available */ int ret = read(_fd_adc, &buf_adc, sizeof(buf_adc)); diff --git a/src/modules/uORB/topics/sensor_combined.h b/src/modules/uORB/topics/sensor_combined.h index 918389f38b..1171054707 100644 --- a/src/modules/uORB/topics/sensor_combined.h +++ b/src/modules/uORB/topics/sensor_combined.h @@ -99,8 +99,8 @@ struct sensor_combined_s { float baro_pres_mbar; /**< Barometric pressure, already temp. comp. */ float baro_alt_meter; /**< Altitude, already temp. comp. */ float baro_temp_celcius; /**< Temperature in degrees celsius */ - float adc_voltage_v[9]; /**< ADC voltages of ADC Chan 10/11/12/13 or -1 */ - unsigned adc_mapping[9]; /**< Channel indices of each of these values */ + float adc_voltage_v[10]; /**< ADC voltages of ADC Chan 10/11/12/13 or -1 */ + unsigned adc_mapping[10]; /**< Channel indices of each of these values */ float mcu_temp_celcius; /**< Internal temperature measurement of MCU */ uint32_t baro_counter; /**< Number of raw baro measurements taken */ diff --git a/src/systemcmds/tests/test_adc.c b/src/systemcmds/tests/test_adc.c index 7302f63cce..03391b8514 100644 --- a/src/systemcmds/tests/test_adc.c +++ b/src/systemcmds/tests/test_adc.c @@ -66,8 +66,8 @@ int test_adc(int argc, char *argv[]) } for (unsigned i = 0; i < 5; i++) { - /* make space for a maximum of ten channels */ - struct adc_msg_s data[10]; + /* make space for a maximum of twelve channels */ + struct adc_msg_s data[12]; /* read all channels available */ ssize_t count = read(fd, data, sizeof(data)); From c3010e5607b6764db6e9eb1a01463903e95acd4e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 17 Mar 2014 18:42:03 +0100 Subject: [PATCH 224/381] Fixed comment that lied --- .../fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index def437b351..e0c2215b6b 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -1037,8 +1037,8 @@ void print_status() // 7-9: Position - m (North, East, Down) // 10-12: Delta Angle bias - rad (X,Y,Z) // 13-14: Wind Vector - m/sec (North,East) - // 15-17: Earth Magnetic Field Vector - milligauss (North, East, Down) - // 18-20: Body Magnetic Field Vector - milligauss (X,Y,Z) + // 15-17: Earth Magnetic Field Vector - gauss (North, East, Down) + // 18-20: Body Magnetic Field Vector - gauss (X,Y,Z) printf("dtIMU: %8.6f dt: %8.6f IMUmsec: %d\n", dtIMU, dt, (int)IMUmsec); printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)dVelIMU.x, (double)dVelIMU.y, (double)dVelIMU.z, (double)accel.x, (double)accel.y, (double)accel.z); From 37513eaafa80d030ca6f8157f10404608f40fb8a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 17 Mar 2014 18:44:37 +0100 Subject: [PATCH 225/381] Added variance and state contrain calls. Need still in-flight re-init and sub-component health checks. Also need to report / log these events as they occur with enough data to identify root causes. --- .../fw_att_pos_estimator/estimator.cpp | 104 ++++++++++++++++++ src/modules/fw_att_pos_estimator/estimator.h | 6 + 2 files changed, 110 insertions(+) diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index f578901465..10e9298ed1 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -1776,7 +1776,111 @@ void CovarianceInit() P[20][20] = P[18][18]; } +float ConstrainFloat(float val, float min, float max) +{ + return (val > max) ? max : ((val < min) ? min : val); +} +void ConstrainVariances() +{ + // State vector: + // 0-3: quaternions (q0, q1, q2, q3) + // 4-6: Velocity - m/sec (North, East, Down) + // 7-9: Position - m (North, East, Down) + // 10-12: Delta Angle bias - rad (X,Y,Z) + // 13-14: Wind Vector - m/sec (North,East) + // 15-17: Earth Magnetic Field Vector - gauss (North, East, Down) + // 18-20: Body Magnetic Field Vector - gauss (X,Y,Z) + + // Constrain quaternion variances + for (unsigned i = 0; i < 4; i++) { + P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f); + } + + // Constrain velocitie variances + for (unsigned i = 4; i < 7; i++) { + P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e3f); + } + + // Constrain position variances + for (unsigned i = 7; i < 10; i++) { + P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e6f); + } + + // Angle bias variances + for (unsigned i = 10; i < 13; i++) { + P[i][i] = ConstrainFloat(P[i][i], 0.0f, sq(0.175f * dtIMU)); + } + + // Wind velocity variances + for (unsigned i = 13; i < 15; i++) { + P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e3f); + } + + // Earth magnetic field variances + for (unsigned i = 15; i < 18; i++) { + P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f); + } + + // Body magnetic field variances + for (unsigned i = 18; i < 21; i++) { + P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f); + } + +} + +void ConstrainStates() +{ + // State vector: + // 0-3: quaternions (q0, q1, q2, q3) + // 4-6: Velocity - m/sec (North, East, Down) + // 7-9: Position - m (North, East, Down) + // 10-12: Delta Angle bias - rad (X,Y,Z) + // 13-14: Wind Vector - m/sec (North,East) + // 15-17: Earth Magnetic Field Vector - gauss (North, East, Down) + // 18-20: Body Magnetic Field Vector - gauss (X,Y,Z) + + + // Constrain quaternion + for (unsigned i = 0; i < 4; i++) { + states[i] = ConstrainFloat(states[i], -1.0f, 1.0f); + } + + // Constrain velocities to what GPS can do for us + for (unsigned i = 4; i < 7; i++) { + states[i] = ConstrainFloat(states[i], -5.0e2f, 5.0e2f); + } + + // Constrain position to a reasonable vehicle range (in meters) + for (unsigned i = 7; i < 9; i++) { + states[i] = ConstrainFloat(states[i], -1.0e6f, 1.0e6f); + } + + // Constrain altitude + states[9] = ConstrainFloat(states[9], -4.0e4f, 1.0e4f); + + // Angle bias limit - set to 8 degrees / sec + for (unsigned i = 10; i < 13; i++) { + states[i] = ConstrainFloat(states[i], -0.12f * dtIMU, 0.12f * dtIMU); + } + + // Wind velocity limits - assume 120 m/s max velocity + for (unsigned i = 13; i < 15; i++) { + states[i] = ConstrainFloat(states[i], -120.0f, 120.0f); + } + + // Earth magnetic field limits (in Gauss) + for (unsigned i = 15; i < 18; i++) { + states[i] = ConstrainFloat(states[i], -1.0f, 1.0f); + } + + // Body magnetic field variances (in Gauss). + // the max offset should be in this range. + for (unsigned i = 18; i < 21; i++) { + states[i] = ConstrainFloat(states[i], -0.5f, 0.5f); + } + +} void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat) { diff --git a/src/modules/fw_att_pos_estimator/estimator.h b/src/modules/fw_att_pos_estimator/estimator.h index 459515f3b8..43bd697934 100644 --- a/src/modules/fw_att_pos_estimator/estimator.h +++ b/src/modules/fw_att_pos_estimator/estimator.h @@ -169,5 +169,11 @@ void CovarianceInit(); void InitialiseFilter(float (&initvelNED)[3]); +float ConstrainFloat(float val, float min, float max); + +void ConstrainVariances(); + +void ConstrainStates(); + uint32_t millis(); From 8fe3475b41b76ecf07aa6cd1d73196c17b4c8ebe Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 17 Mar 2014 20:12:12 +0100 Subject: [PATCH 226/381] mavlink: add onboard function for camera capture commands --- src/modules/mavlink/mavlink_main.cpp | 13 +++++++ src/modules/mavlink/mavlink_main.h | 3 +- src/modules/mavlink/mavlink_messages.cpp | 44 +++++++++++++++++++++++- 3 files changed, 58 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 9c2e0178a3..2457a7cae8 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1653,6 +1653,8 @@ Mavlink::task_main(int argc, char *argv[]) case 'm': if (strcmp(optarg, "custom") == 0) { _mode = MAVLINK_MODE_CUSTOM; + } else if (strcmp(optarg, "camera") == 0) { + _mode = MAVLINK_MODE_CAMERA; } break; @@ -1696,6 +1698,10 @@ Mavlink::task_main(int argc, char *argv[]) warnx("mode: CUSTOM"); break; + case MAVLINK_MODE_CAMERA: + warnx("mode: CAMERA"); + break; + default: warnx("ERROR: Unknown mode"); break; @@ -1765,6 +1771,13 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("RC_CHANNELS_RAW", 1.0f * rate_mult); break; + case MAVLINK_MODE_CAMERA: + configure_stream("SYS_STATUS", 1.0f); + configure_stream("ATTITUDE", 20.0f); + configure_stream("GLOBAL_POSITION_INT", 20.0f); + configure_stream("CAMERA_CAPTURE", 1.0f); + break; + default: break; } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index f743c25044..5a118a0ad9 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -146,7 +146,8 @@ public: enum MAVLINK_MODE { MAVLINK_MODE_NORMAL = 0, - MAVLINK_MODE_CUSTOM + MAVLINK_MODE_CUSTOM, + MAVLINK_MODE_CAMERA }; void set_mode(enum MAVLINK_MODE); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 7d388f88da..014b53829d 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -230,7 +230,6 @@ protected: mavlink_base_mode, mavlink_custom_mode, mavlink_state); - } }; @@ -1127,6 +1126,48 @@ protected: } }; +class MavlinkStreamCameraCapture : public MavlinkStream +{ +public: + const char *get_name() + { + return "CAMERA_CAPTURE"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamCameraCapture(); + } + +private: + MavlinkOrbSubscription *status_sub; + struct vehicle_status_s *status; + +protected: + void subscribe(Mavlink *mavlink) + { + status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); + status = (struct vehicle_status_s *)status_sub->get_data(); + + + } + + void send(const hrt_abstime t) + { + (void)status_sub->update(t); + + if (status->arming_state == ARMING_STATE_ARMED + || status->arming_state == ARMING_STATE_ARMED_ERROR) { + + /* send camera capture on */ + mavlink_msg_command_long_send(_channel, 42, 30, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); + } else { + /* send camera capture off */ + mavlink_msg_command_long_send(_channel, 42, 30, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0); + } + } +}; + MavlinkStream *streams_list[] = { new MavlinkStreamHeartbeat(), @@ -1151,6 +1192,7 @@ MavlinkStream *streams_list[] = { new MavlinkStreamRCChannelsRaw(), new MavlinkStreamManualControl(), new MavlinkStreamOpticalFlow(), + new MavlinkStreamCameraCapture(), nullptr }; From 03ccee289ba6484a889ed6f79fd744b412bc8537 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 18 Mar 2014 09:19:57 +0100 Subject: [PATCH 227/381] Numerical checks on covariances --- .../fw_att_pos_estimator/estimator.cpp | 192 ++++++++++++++++-- src/modules/fw_att_pos_estimator/estimator.h | 11 + .../fw_att_pos_estimator_main.cpp | 5 +- 3 files changed, 184 insertions(+), 24 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index 10e9298ed1..e21b94c43f 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -1,5 +1,7 @@ #include "estimator.h" +#include + // Global variables float KH[n_states][n_states]; // intermediate result used for covariance updates float KHP[n_states][n_states]; // intermediate result used for covariance updates @@ -67,9 +69,21 @@ bool fuseHgtData = false; // this boolean causes the hgtMea obs to be fused bool fuseMagData = false; // boolean true when magnetometer data is to be fused bool fuseVtasData = false; // boolean true when airspeed data is to be fused -bool onGround = true; // boolean true when the flight vehicle is on the ground (not flying) -bool useAirspeed = true; // boolean true if airspeed data is being used -bool useCompass = true; // boolean true if magnetometer data is being used +bool onGround = true; ///< boolean true when the flight vehicle is on the ground (not flying) +bool staticMode = true; ///< boolean true if no position feedback is fused +bool useAirspeed = true; ///< boolean true if airspeed data is being used +bool useCompass = true; ///< boolean true if magnetometer data is being used + +bool velHealth; +bool posHealth; +bool hgtHealth; +bool velTimeout; +bool posTimeout; +bool hgtTimeout; + +bool numericalProtection = true; + +static unsigned storeIndex = 0; float Vector3f::length(void) const { @@ -889,7 +903,7 @@ void CovariancePrediction(float dt) { for (uint8_t i=7; i<=8; i++) { - for (uint8_t j=0; j<=20; j++) + for (unsigned j = 0; j < n_states; j++) { nextP[i][j] = P[i][j]; nextP[j][i] = P[j][i]; @@ -897,19 +911,39 @@ void CovariancePrediction(float dt) } } - // Force symmetry on the covariance matrix to prevent ill-conditioning - // of the matrix which would cause the filter to blow-up - for (uint8_t i=0; i<=20; i++) P[i][i] = nextP[i][i]; - for (uint8_t i=1; i<=20; i++) - { - for (uint8_t j=0; j<=i-1; j++) - { - P[i][j] = 0.5f*(nextP[i][j] + nextP[j][i]); - P[j][i] = P[i][j]; + if (onGround || staticMode) { + // copy the portion of the variances we want to + // propagate + for (unsigned i = 0; i < 14; i++) { + P[i][i] = nextP[i][i]; + + // force symmetry for observable states + // force zero for non-observable states + for (unsigned i = 1; i < n_states; i++) + { + for (uint8_t j = 0; j < i; j++) + { + if ((i > 12) || (j > 12)) { + P[i][j] = 0.0f; + } else { + P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]); + } + P[j][i] = P[i][j]; + } + } } + + } else { + + // Copy covariance + for (unsigned i = 0; i < n_states; i++) { + P[i][i] = nextP[i][i]; + } + + ForceSymmetry(); } - // + ConstrainVariances(); } void FuseVelposNED() @@ -923,12 +957,6 @@ void FuseVelposNED() static uint32_t velFailTime = 0; static uint32_t posFailTime = 0; static uint32_t hgtFailTime = 0; - bool velHealth; - bool posHealth; - bool hgtHealth; - bool velTimeout; - bool posTimeout; - bool hgtTimeout; // declare variables used to check measurement errors float velInnov[3] = {0.0f,0.0f,0.0f}; @@ -1137,6 +1165,9 @@ void FuseVelposNED() } } + ForceSymmetry(); + ConstrainVariances(); + //printf("velh: %s, posh: %s, hgth: %s\n", ((velHealth) ? "OK" : "FAIL"), ((posHealth) ? "OK" : "FAIL"), ((hgtHealth) ? "OK" : "FAIL")); } @@ -1442,6 +1473,9 @@ void FuseMagnetometer() } } obsIndex = obsIndex + 1; + + ForceSymmetry(); + ConstrainVariances(); } void FuseAirspeed() @@ -1568,6 +1602,9 @@ void FuseAirspeed() } } } + + ForceSymmetry(); + ConstrainVariances(); } void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) @@ -1604,8 +1641,6 @@ float sq(float valIn) // Store states in a history array along with time stamp void StoreStates(uint64_t timestamp_ms) { - /* static to keep the store index */ - static uint8_t storeIndex = 0; for (unsigned i=0; i GPS_FIX_2D); + } } void calcEarthRateNED(Vector3f &omega, float latitude) @@ -1783,6 +1841,10 @@ float ConstrainFloat(float val, float min, float max) void ConstrainVariances() { + if (!numericalProtection) { + return; + } + // State vector: // 0-3: quaternions (q0, q1, q2, q3) // 4-6: Velocity - m/sec (North, East, Down) @@ -1831,6 +1893,10 @@ void ConstrainVariances() void ConstrainStates() { + if (!numericalProtection) { + return; + } + // State vector: // 0-3: quaternions (q0, q1, q2, q3) // 4-6: Velocity - m/sec (North, East, Down) @@ -1882,6 +1948,88 @@ void ConstrainStates() } +void ForceSymmetry() +{ + if (!numericalProtection) { + return; + } + + // Force symmetry on the covariance matrix to prevent ill-conditioning + // of the matrix which would cause the filter to blow-up + for (unsigned i = 1; i < n_states; i++) + { + for (uint8_t j = 0; j < i; j++) + { + P[i][j] = 0.5f * (P[i][j] + P[j][i]); + P[j][i] = P[i][j]; + } + } +} + +bool FilterHealthy() +{ + if (!statesInitialised) { + return false; + } + + // XXX Check state vector for NaNs and ill-conditioning + + // Check if any of the major inputs timed out + if (posTimeout || velTimeout || hgtTimeout) { + return false; + } + + // Nothing fired, return ok. + return true; +} + +/** + * Reset the filter position states + * + * This resets the position to the last GPS measurement + * or to zero in case of static position. + */ +void ResetPosition(void) +{ + if (staticMode) { + states[7] = 0; + states[8] = 0; + } else if (GPSstatus >= GPS_FIX_3D) { + + // reset the states from the GPS measurements + states[7] = posNE[0]; + states[8] = posNE[1]; + } +} + +/** + * Reset the height state. + * + * This resets the height state with the last altitude measurements + */ +void ResetHeight(void) +{ + // write to the state vector + states[9] = -hgtMea; +} + +/** + * Reset the velocity state. + */ +void ResetVelocity(void) +{ + if (staticMode) { + states[4] = 0.0f; + states[5] = 0.0f; + states[6] = 0.0f; + } else if (GPSstatus >= GPS_FIX_3D) { + + states[4] = velNED[0]; // north velocity from last reading + states[5] = velNED[1]; // east velocity from last reading + states[6] = velNED[2]; // down velocity from last reading + } +} + void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat) { float initialRoll, initialPitch; diff --git a/src/modules/fw_att_pos_estimator/estimator.h b/src/modules/fw_att_pos_estimator/estimator.h index 43bd697934..b8b218e396 100644 --- a/src/modules/fw_att_pos_estimator/estimator.h +++ b/src/modules/fw_att_pos_estimator/estimator.h @@ -121,10 +121,19 @@ extern uint8_t GPSstatus; extern float baroHgt; extern bool statesInitialised; +extern bool numericalProtection; const float covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions const float covDelAngMax = 0.02f; // maximum delta angle between covariance predictions +extern bool staticMode; + +enum GPS_FIX { + GPS_FIX_NOFIX = 0, + GPS_FIX_2D = 2, + GPS_FIX_3D = 3 +}; + void UpdateStrapdownEquationsNED(); void CovariancePrediction(float dt); @@ -175,5 +184,7 @@ void ConstrainVariances(); void ConstrainStates(); +void ForceSymmetry(); + uint32_t millis(); diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index e0c2215b6b..df2608f831 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -1050,7 +1050,7 @@ void print_status() printf("states (wind) [14-15]: %8.4f, %8.4f\n", (double)states[13], (double)states[14]); printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)states[15], (double)states[16], (double)states[17]); printf("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f\n", (double)states[18], (double)states[19], (double)states[20]); - printf("states: %s %s %s %s %s %s %s %s %s\n", + printf("states: %s %s %s %s %s %s %s %s %s %s\n", (statesInitialised) ? "INITIALIZED" : "NON_INIT", (onGround) ? "ON_GROUND" : "AIRBORNE", (fuseVelData) ? "FUSE_VEL" : "INH_VEL", @@ -1059,7 +1059,8 @@ void print_status() (fuseMagData) ? "FUSE_MAG" : "INH_MAG", (fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS", (useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD", - (useCompass) ? "USE_COMPASS" : "IGN_COMPASS"); + (useCompass) ? "USE_COMPASS" : "IGN_COMPASS", + (staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE"); } int fw_att_pos_estimator_main(int argc, char *argv[]) From 7cdb7291af52f2b60aa53607e7ec229fecf7497f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 18 Mar 2014 09:21:27 +0100 Subject: [PATCH 228/381] Protect against divergence --- src/modules/fw_att_pos_estimator/estimator.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index e21b94c43f..fa81d4dfa0 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -312,6 +312,8 @@ void UpdateStrapdownEquationsNED() states[8] = states[8] + 0.5f*(states[5] + lastVelocity[1])*dtIMU; states[9] = states[9] + 0.5f*(states[6] + lastVelocity[2])*dtIMU; + // Constrain states (to protect against filter divergence) + ConstrainStates(); } void CovariancePrediction(float dt) From bce67c6b036cddbe1542f910c6e605256283768f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 18 Mar 2014 18:28:54 +0100 Subject: [PATCH 229/381] Init / reinit improvements --- .../fw_att_pos_estimator/estimator.cpp | 41 ++++++++++++++++++- src/modules/fw_att_pos_estimator/estimator.h | 8 ++++ .../fw_att_pos_estimator_main.cpp | 21 +++++++++- 3 files changed, 68 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index fa81d4dfa0..36b9f4491b 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -1651,7 +1651,7 @@ void StoreStates(uint64_t timestamp_ms) storeIndex = 0; } -void ResetStates() +void ResetStoredStates() { // reset all stored states memset(&storedStates[0][0], 0, sizeof(storedStates)); @@ -2032,6 +2032,45 @@ void ResetVelocity(void) } } + +/** + * Check the filter inputs and bound its operational state + * + * This check will reset the filter states if required + * due to a failure of consistency or timeout checks. + * it should be run after the measurement data has been + * updated, but before any of the fusion steps are + * executed. + */ +void CheckAndBound() +{ + + // Store the old filter state + bool currStaticMode = staticMode; + + // Reset the filter if the IMU data is too old + if (dtIMU > 0.2f) { + ResetVelocity(); + ResetPosition(); + ResetHeight(); + ResetStoredStates(); + + // that's all we can do here, return + return; + } + + // Check if we're on ground - this also sets static mode. + OnGroundCheck(); + + // Check if we switched between states + if (currStaticMode != staticMode) { + ResetVelocity(); + ResetPosition(); + ResetHeight(); + ResetStoredStates(); + } +} + void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat) { float initialRoll, initialPitch; diff --git a/src/modules/fw_att_pos_estimator/estimator.h b/src/modules/fw_att_pos_estimator/estimator.h index b8b218e396..256aff7716 100644 --- a/src/modules/fw_att_pos_estimator/estimator.h +++ b/src/modules/fw_att_pos_estimator/estimator.h @@ -158,6 +158,8 @@ void StoreStates(uint64_t timestamp_ms); // recall stste vector stored at closest time to the one specified by msec void RecallStates(float statesForFusion[n_states], uint64_t msec); +void ResetStoredStates(); + void quat2Tbn(Mat3f &Tbn, const float (&quat)[4]); void calcEarthRateNED(Vector3f &omega, float latitude); @@ -186,5 +188,11 @@ void ConstrainStates(); void ForceSymmetry(); +void CheckAndBound(); + +void ResetPosition(); + +void ResetVelocity(); + uint32_t millis(); diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index df2608f831..8b41e7479d 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -604,6 +604,9 @@ FixedwingEstimator::task_main() orb_check(_gps_sub, &gps_updated); if (gps_updated) { + + uint64_t last_gps = _gps.timestamp_position; + orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps); perf_count(_perf_gps); @@ -612,6 +615,14 @@ FixedwingEstimator::task_main() newDataGps = false; } else { + + /* check if we had a GPS outage for a long time */ + if (hrt_elapsed_time(&last_gps) > 5 * 1000 * 1000) { + ResetPosition(); + ResetVelocity(); + ResetStoredStates(); + } + /* fuse GPS updates */ //_gps.timestamp / 1e3; @@ -687,6 +698,12 @@ FixedwingEstimator::task_main() } + /** + * CHECK IF THE INPUT DATA IS SANE + */ + CheckAndBound(); + + /** * PART TWO: EXECUTE THE FILTER **/ @@ -715,7 +732,9 @@ FixedwingEstimator::task_main() _local_pos.ref_timestamp = _gps.timestamp_position; // Store - _baro_ref = baroHgt; + orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); + _baro_ref = _baro.altitude; + baroHgt = _baro.altitude - _baro_ref; _baro_gps_offset = baroHgt - alt; // XXX this is not multithreading safe From c5a1f4617c6d3e1782dde5d4552122059200db84 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 18 Mar 2014 18:30:05 +0100 Subject: [PATCH 230/381] Fixed MS4525 error flag handling --- src/drivers/meas_airspeed/meas_airspeed.cpp | 115 +++++++++++++------- 1 file changed, 73 insertions(+), 42 deletions(-) diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 06d89abf77..7baa8738f6 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -104,7 +104,7 @@ class MEASAirspeed : public Airspeed { public: - MEASAirspeed(int bus, int address = I2C_ADDRESS_MS4525DO, const char* path = PATH_MS4525); + MEASAirspeed(int bus, int address = I2C_ADDRESS_MS4525DO, const char *path = PATH_MS4525); protected: @@ -123,7 +123,7 @@ protected: */ extern "C" __EXPORT int meas_airspeed_main(int argc, char *argv[]); -MEASAirspeed::MEASAirspeed(int bus, int address, const char* path) : Airspeed(bus, address, +MEASAirspeed::MEASAirspeed(int bus, int address, const char *path) : Airspeed(bus, address, CONVERSION_INTERVAL, path) { @@ -161,23 +161,30 @@ MEASAirspeed::collect() ret = transfer(nullptr, 0, &val[0], 4); if (ret < 0) { - perf_count(_comms_errors); - perf_end(_sample_perf); + perf_count(_comms_errors); + perf_end(_sample_perf); return ret; } - uint8_t status = val[0] & 0xC0; + uint8_t status = (val[0] & 0xC0) >> 6; - if (status == 2) { - log("err: stale data"); - perf_count(_comms_errors); - perf_end(_sample_perf); - return ret; - } else if (status == 3) { - log("err: fault"); - perf_count(_comms_errors); - perf_end(_sample_perf); - return ret; + switch (status) { + case 0: break; + + case 1: + perf_count(_comms_errors); + perf_end(_sample_perf); + return -EAGAIN; + + case 2: + perf_count(_comms_errors); + perf_end(_sample_perf); + return -EAGAIN; + + case 3: + perf_count(_comms_errors); + perf_end(_sample_perf); + return -EAGAIN; } int16_t dp_raw = 0, dT_raw = 0; @@ -193,19 +200,21 @@ MEASAirspeed::collect() */ const float P_min = -1.0f; const float P_max = 1.0f; - float diff_press_pa = fabsf( ( ((float)dp_raw - 0.1f*16383.0f) * (P_max-P_min)/(0.8f*16383.0f) + P_min) * 6894.8f) - _diff_pres_offset; - if (diff_press_pa < 0.0f) - diff_press_pa = 0.0f; + float diff_press_pa = fabsf((((float)dp_raw - 0.1f * 16383.0f) * (P_max - P_min) / (0.8f * 16383.0f) + P_min) * 6894.8f) - _diff_pres_offset; + + if (diff_press_pa < 0.0f) { + diff_press_pa = 0.0f; + } 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; + _max_differential_pressure_pa = diff_press_pa; } report.timestamp = hrt_absolute_time(); - report.error_count = perf_event_count(_comms_errors); + report.error_count = perf_event_count(_comms_errors); report.temperature = temperature; report.differential_pressure_pa = diff_press_pa; report.voltage = 0; @@ -261,8 +270,9 @@ MEASAirspeed::cycle() } /* measurement phase */ - if (OK != measure()) + if (OK != measure()) { log("measure error"); + } /* next phase is collection */ _collect_phase = true; @@ -303,15 +313,17 @@ start(int i2c_bus) { int fd; - if (g_dev != nullptr) + if (g_dev != nullptr) { errx(1, "already started"); + } /* create the driver, try the MS4525DO first */ g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS4525DO, PATH_MS4525); /* check if the MS4525DO was instantiated */ - if (g_dev == nullptr) + if (g_dev == nullptr) { goto fail; + } /* try the MS5525DSO next if init fails */ if (OK != g_dev->Airspeed::init()) { @@ -319,22 +331,26 @@ start(int i2c_bus) g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS5525DSO, PATH_MS5525); /* check if the MS5525DSO was instantiated */ - if (g_dev == nullptr) + if (g_dev == nullptr) { goto fail; + } /* both versions failed if the init for the MS5525DSO fails, give up */ - if (OK != g_dev->Airspeed::init()) + if (OK != g_dev->Airspeed::init()) { goto fail; + } } /* set the poll rate to default, starts automatic data collection */ fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); - if (fd < 0) + if (fd < 0) { goto fail; + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { goto fail; + } exit(0); @@ -379,21 +395,24 @@ test() int fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "%s open failed (try 'meas_airspeed start' if the driver is not running", AIRSPEED_DEVICE_PATH); + } /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); - if (sz != sizeof(report)) + if (sz != sizeof(report)) { err(1, "immediate read failed"); + } warnx("single read"); warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa); /* 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++) { @@ -404,14 +423,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("diff pressure: %8.4f pa", (double)report.differential_pressure_pa); @@ -419,8 +440,9 @@ test() } /* reset the sensor polling to its default rate */ - if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { errx(1, "failed to set default rate"); + } errx(0, "PASS"); } @@ -433,14 +455,17 @@ reset() { int fd = open(AIRSPEED_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); } @@ -451,8 +476,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(); @@ -491,32 +517,37 @@ meas_airspeed_main(int argc, char *argv[]) /* * Start/load the driver. */ - if (!strcmp(argv[1], "start")) + if (!strcmp(argv[1], "start")) { meas_airspeed::start(i2c_bus); + } /* * Stop the driver */ - if (!strcmp(argv[1], "stop")) + if (!strcmp(argv[1], "stop")) { meas_airspeed::stop(); + } /* * Test the driver/device. */ - if (!strcmp(argv[1], "test")) + if (!strcmp(argv[1], "test")) { meas_airspeed::test(); + } /* * Reset the driver. */ - if (!strcmp(argv[1], "reset")) + if (!strcmp(argv[1], "reset")) { meas_airspeed::reset(); + } /* * Print driver information. */ - if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) + if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) { meas_airspeed::info(); + } meas_airspeed_usage(); exit(0); From fc877f9c20aaeb59275c7e1953bad818ba27662c Mon Sep 17 00:00:00 2001 From: dominiho Date: Wed, 19 Mar 2014 16:13:17 +0100 Subject: [PATCH 231/381] changed px4flow I2C address to default value --- 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 50089282ab..f214b5964f 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -74,7 +74,7 @@ /* Configuration Constants */ #define PX4FLOW_BUS PX4_I2C_BUS_EXPANSION -#define I2C_FLOW_ADDRESS 0x45 //* 7-bit address. 8-bit address is 0x8A +#define I2C_FLOW_ADDRESS 0x42 //* 7-bit address. 8-bit address is 0x84 //range 0x42 - 0x49 /* PX4FLOW Registers addresses */ From 952fd1ec6cbc615d6bd9213061dcce0f9cf8c5f0 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 20 Mar 2014 11:38:41 +0100 Subject: [PATCH 232/381] mavlink: camera mode with rate multiplier --- 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 2457a7cae8..dca248dc96 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1773,8 +1773,8 @@ Mavlink::task_main(int argc, char *argv[]) case MAVLINK_MODE_CAMERA: configure_stream("SYS_STATUS", 1.0f); - configure_stream("ATTITUDE", 20.0f); - configure_stream("GLOBAL_POSITION_INT", 20.0f); + configure_stream("ATTITUDE", 20.0f * rate_mult); + configure_stream("GLOBAL_POSITION_INT", 20.0f * rate_mult); configure_stream("CAMERA_CAPTURE", 1.0f); break; From a989dd6eec5ec15e6387769d8f657a29d68fabff Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 20 Mar 2014 14:23:23 +0100 Subject: [PATCH 233/381] mavlink: correct verbose info, bytes not bits --- 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 dca248dc96..576b3ddec8 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1709,7 +1709,7 @@ Mavlink::task_main(int argc, char *argv[]) _mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER; - warnx("data rate: %d bps, port: %s, baud: %d", _datarate, _device_name, (int)_baudrate); + warnx("data rate: %d Bytes/s, port: %s, baud: %d", _datarate, _device_name, _baudrate); /* flush stdout in case MAVLink is about to take it over */ fflush(stdout); From bf69a7b6473adde22b3220b6e72c0d065c897bc5 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 20 Mar 2014 14:26:45 +0100 Subject: [PATCH 234/381] mavlink: camera mode rate is now correct --- 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 576b3ddec8..a36ad0bf6b 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1773,8 +1773,8 @@ Mavlink::task_main(int argc, char *argv[]) case MAVLINK_MODE_CAMERA: configure_stream("SYS_STATUS", 1.0f); - configure_stream("ATTITUDE", 20.0f * rate_mult); - configure_stream("GLOBAL_POSITION_INT", 20.0f * rate_mult); + configure_stream("ATTITUDE", 15.0f * rate_mult); + configure_stream("GLOBAL_POSITION_INT", 15.0f * rate_mult); configure_stream("CAMERA_CAPTURE", 1.0f); break; From 2c32cdf16bb02a9ae4d47b60cb32553fefb33738 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sun, 16 Mar 2014 15:54:35 -0400 Subject: [PATCH 235/381] Fixed block param template. --- src/modules/controllib/block/BlockParam.cpp | 25 +++++++++++++ src/modules/controllib/block/BlockParam.hpp | 39 ++++++--------------- 2 files changed, 36 insertions(+), 28 deletions(-) diff --git a/src/modules/controllib/block/BlockParam.cpp b/src/modules/controllib/block/BlockParam.cpp index fd12e365dc..8f98da74fa 100644 --- a/src/modules/controllib/block/BlockParam.cpp +++ b/src/modules/controllib/block/BlockParam.cpp @@ -76,4 +76,29 @@ BlockParamBase::BlockParamBase(Block *parent, const char *name, bool parent_pref printf("error finding param: %s\n", fullname); }; +template +BlockParam::BlockParam(Block *block, const char *name, + bool parent_prefix) : + BlockParamBase(block, name, parent_prefix), + _val() { + update(); +} + +template +T BlockParam::get() { return _val; } + +template +void BlockParam::set(T val) { _val = val; } + +template +void BlockParam::update() { + if (_handle != PARAM_INVALID) param_get(_handle, &_val); +} + +template +BlockParam::~BlockParam() {}; + +template class __EXPORT BlockParam; +template class __EXPORT BlockParam; + } // namespace control diff --git a/src/modules/controllib/block/BlockParam.hpp b/src/modules/controllib/block/BlockParam.hpp index 36bc8c24ba..735b2b2922 100644 --- a/src/modules/controllib/block/BlockParam.hpp +++ b/src/modules/controllib/block/BlockParam.hpp @@ -70,38 +70,21 @@ protected: * Parameters that are tied to blocks for updating and nameing. */ -class __EXPORT BlockParamFloat : public BlockParamBase +template +class BlockParam : public BlockParamBase { public: - BlockParamFloat(Block *block, const char *name, bool parent_prefix=true) : - BlockParamBase(block, name, parent_prefix), - _val() { - update(); - } - float get() { return _val; } - void set(float val) { _val = val; } - void update() { - if (_handle != PARAM_INVALID) param_get(_handle, &_val); - } + BlockParam(Block *block, const char *name, + bool parent_prefix=true); + T get(); + void set(T val); + void update(); + virtual ~BlockParam(); protected: - float _val; + T _val; }; -class __EXPORT BlockParamInt : public BlockParamBase -{ -public: - BlockParamInt(Block *block, const char *name, bool parent_prefix=true) : - BlockParamBase(block, name, parent_prefix), - _val() { - update(); - } - int get() { return _val; } - void set(int val) { _val = val; } - void update() { - if (_handle != PARAM_INVALID) param_get(_handle, &_val); - } -protected: - int _val; -}; +typedef BlockParam BlockParamFloat; +typedef BlockParam BlockParamInt; } // namespace control From fd6590cfa7e14436fa8af6a0569b6382cd39069a Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sun, 16 Mar 2014 16:14:56 -0400 Subject: [PATCH 236/381] Moved UOrbPubliction/Subscription to uORB::Publication/Subscription --- src/drivers/md25/md25.cpp | 4 +-- src/drivers/md25/md25.hpp | 4 +-- src/drivers/roboclaw/RoboClaw.cpp | 2 +- src/drivers/roboclaw/RoboClaw.hpp | 4 +-- .../att_pos_estimator_ekf/KalmanNav.hpp | 16 ++++----- src/modules/controllib/block/Block.cpp | 8 ++--- src/modules/controllib/block/Block.hpp | 16 +++++---- src/modules/controllib/module.mk | 2 -- src/modules/controllib/uorb/blocks.hpp | 22 ++++++------ .../Publication.cpp} | 4 +-- .../Publication.hpp} | 33 ++++++++--------- .../Subscription.cpp} | 10 +++--- .../Subscription.hpp} | 35 +++++++++---------- src/modules/uORB/module.mk | 4 ++- 14 files changed, 81 insertions(+), 83 deletions(-) rename src/modules/{controllib/uorb/UOrbPublication.cpp => uORB/Publication.cpp} (96%) rename src/modules/{controllib/uorb/UOrbPublication.hpp => uORB/Publication.hpp} (85%) rename src/modules/{controllib/uorb/UOrbSubscription.cpp => uORB/Subscription.cpp} (92%) rename src/modules/{controllib/uorb/UOrbSubscription.hpp => uORB/Subscription.hpp} (85%) diff --git a/src/drivers/md25/md25.cpp b/src/drivers/md25/md25.cpp index d43e3aef9e..6d5e805ea1 100644 --- a/src/drivers/md25/md25.cpp +++ b/src/drivers/md25/md25.cpp @@ -52,7 +52,7 @@ #include #include -#include +#include #include #include @@ -587,7 +587,7 @@ int md25Sine(const char *deviceName, uint8_t bus, uint8_t address, float amplitu float prev_revolution = md25.getRevolutions1(); // debug publication - control::UOrbPublication debug_msg(NULL, + uORB::Publication debug_msg(NULL, ORB_ID(debug_key_value)); // sine wave for motor 1 diff --git a/src/drivers/md25/md25.hpp b/src/drivers/md25/md25.hpp index 1661f67f9a..962c6b881d 100644 --- a/src/drivers/md25/md25.hpp +++ b/src/drivers/md25/md25.hpp @@ -46,7 +46,7 @@ #include #include -#include +#include #include #include @@ -270,7 +270,7 @@ private: struct pollfd _controlPoll; /** actuator controls subscription */ - control::UOrbSubscription _actuators; + uORB::Subscription _actuators; // local copy of data from i2c device uint8_t _version; diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index d65a9be361..dd5e4d3e0c 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -53,7 +53,7 @@ #include #include -#include +#include #include #include diff --git a/src/drivers/roboclaw/RoboClaw.hpp b/src/drivers/roboclaw/RoboClaw.hpp index e9f35cf956..58994d6fa6 100644 --- a/src/drivers/roboclaw/RoboClaw.hpp +++ b/src/drivers/roboclaw/RoboClaw.hpp @@ -45,7 +45,7 @@ #include #include -#include +#include #include #include @@ -169,7 +169,7 @@ private: struct pollfd _controlPoll; /** actuator controls subscription */ - control::UOrbSubscription _actuators; + uORB::Subscription _actuators; // private data float _motor1Position; diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp index 46ee4b6c8e..caf93bc787 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp @@ -47,8 +47,8 @@ #include #include #include -#include -#include +#include +#include #include #include @@ -138,13 +138,13 @@ protected: math::Matrix<3,3> C_nb; /**< direction cosine matrix from body to nav frame */ math::Quaternion q; /**< quaternion from body to nav frame */ // subscriptions - control::UOrbSubscription _sensors; /**< sensors sub. */ - control::UOrbSubscription _gps; /**< gps sub. */ - control::UOrbSubscription _param_update; /**< parameter update sub. */ + uORB::Subscription _sensors; /**< sensors sub. */ + uORB::Subscription _gps; /**< gps sub. */ + uORB::Subscription _param_update; /**< parameter update sub. */ // publications - control::UOrbPublication _pos; /**< position pub. */ - control::UOrbPublication _localPos; /**< local position pub. */ - control::UOrbPublication _att; /**< attitude pub. */ + uORB::Publication _pos; /**< position pub. */ + uORB::Publication _localPos; /**< local position pub. */ + uORB::Publication _att; /**< attitude pub. */ // time stamps uint64_t _pubTimeStamp; /**< output data publication time stamp */ uint64_t _predictTimeStamp; /**< prediction time stamp */ diff --git a/src/modules/controllib/block/Block.cpp b/src/modules/controllib/block/Block.cpp index b964d40a31..0b4cce9792 100644 --- a/src/modules/controllib/block/Block.cpp +++ b/src/modules/controllib/block/Block.cpp @@ -43,8 +43,8 @@ #include "Block.hpp" #include "BlockParam.hpp" -#include "../uorb/UOrbSubscription.hpp" -#include "../uorb/UOrbPublication.hpp" +#include "uORB/Subscription.hpp" +#include "uORB/Publication.hpp" namespace control { @@ -100,7 +100,7 @@ void Block::updateParams() void Block::updateSubscriptions() { - UOrbSubscriptionBase *sub = getSubscriptions().getHead(); + uORB::SubscriptionBase *sub = getSubscriptions().getHead(); int count = 0; while (sub != NULL) { @@ -118,7 +118,7 @@ void Block::updateSubscriptions() void Block::updatePublications() { - UOrbPublicationBase *pub = getPublications().getHead(); + uORB::PublicationBase *pub = getPublications().getHead(); int count = 0; while (pub != NULL) { diff --git a/src/modules/controllib/block/Block.hpp b/src/modules/controllib/block/Block.hpp index 258701f279..6a8e1b5b9c 100644 --- a/src/modules/controllib/block/Block.hpp +++ b/src/modules/controllib/block/Block.hpp @@ -44,6 +44,12 @@ #include "List.hpp" +// forward declaration +namespace uORB { + class SubscriptionBase; + class PublicationBase; +} + namespace control { @@ -55,8 +61,6 @@ static const uint8_t blockNameLengthMax = 80; // forward declaration class BlockParamBase; -class UOrbSubscriptionBase; -class UOrbPublicationBase; class SuperBlock; /** @@ -79,15 +83,15 @@ public: protected: // accessors SuperBlock *getParent() { return _parent; } - List & getSubscriptions() { return _subscriptions; } - List & getPublications() { return _publications; } + List & getSubscriptions() { return _subscriptions; } + List & getPublications() { return _publications; } List & getParams() { return _params; } // attributes const char *_name; SuperBlock *_parent; float _dt; - List _subscriptions; - List _publications; + List _subscriptions; + List _publications; List _params; }; diff --git a/src/modules/controllib/module.mk b/src/modules/controllib/module.mk index d815a8feb5..f0139a4b70 100644 --- a/src/modules/controllib/module.mk +++ b/src/modules/controllib/module.mk @@ -37,7 +37,5 @@ SRCS = test_params.c \ block/Block.cpp \ block/BlockParam.cpp \ - uorb/UOrbPublication.cpp \ - uorb/UOrbSubscription.cpp \ uorb/blocks.cpp \ blocks.cpp diff --git a/src/modules/controllib/uorb/blocks.hpp b/src/modules/controllib/uorb/blocks.hpp index 7c80c4b2ba..a8a70507e9 100644 --- a/src/modules/controllib/uorb/blocks.hpp +++ b/src/modules/controllib/uorb/blocks.hpp @@ -62,8 +62,8 @@ extern "C" { } #include "../blocks.hpp" -#include "UOrbSubscription.hpp" -#include "UOrbPublication.hpp" +#include +#include namespace control { @@ -94,16 +94,16 @@ class __EXPORT BlockUorbEnabledAutopilot : public SuperBlock { protected: // subscriptions - UOrbSubscription _att; - UOrbSubscription _attCmd; - UOrbSubscription _ratesCmd; - UOrbSubscription _pos; - UOrbSubscription _missionCmd; - UOrbSubscription _manual; - UOrbSubscription _status; - UOrbSubscription _param_update; + uORB::Subscription _att; + uORB::Subscription _attCmd; + uORB::Subscription _ratesCmd; + uORB::Subscription _pos; + uORB::Subscription _missionCmd; + uORB::Subscription _manual; + uORB::Subscription _status; + uORB::Subscription _param_update; // publications - UOrbPublication _actuators; + uORB::Publication _actuators; public: BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name); virtual ~BlockUorbEnabledAutopilot(); diff --git a/src/modules/controllib/uorb/UOrbPublication.cpp b/src/modules/uORB/Publication.cpp similarity index 96% rename from src/modules/controllib/uorb/UOrbPublication.cpp rename to src/modules/uORB/Publication.cpp index f69b39d90d..ed67b485de 100644 --- a/src/modules/controllib/uorb/UOrbPublication.cpp +++ b/src/modules/uORB/Publication.cpp @@ -32,8 +32,8 @@ ****************************************************************************/ /** - * @file UOrbPublication.cpp + * @file Publication.cpp * */ -#include "UOrbPublication.hpp" +#include "Publication.hpp" diff --git a/src/modules/controllib/uorb/UOrbPublication.hpp b/src/modules/uORB/Publication.hpp similarity index 85% rename from src/modules/controllib/uorb/UOrbPublication.hpp rename to src/modules/uORB/Publication.hpp index 6f1f3fc1c0..7fa6bcc175 100644 --- a/src/modules/controllib/uorb/UOrbPublication.hpp +++ b/src/modules/uORB/Publication.hpp @@ -32,32 +32,29 @@ ****************************************************************************/ /** - * @file UOrbPublication.h + * @file Publication.h * */ #pragma once #include -#include "../block/Block.hpp" -#include "../block/List.hpp" +#include -namespace control +namespace uORB { -class Block; - /** * Base publication warapper class, used in list traversal * of various publications. */ -class __EXPORT UOrbPublicationBase : public ListNode +class __EXPORT PublicationBase : public ListNode { public: - UOrbPublicationBase( - List * list, + PublicationBase( + List * list, const struct orb_metadata *meta) : _meta(meta), _handle(-1) { @@ -71,7 +68,7 @@ public: } } virtual void *getDataVoidPtr() = 0; - virtual ~UOrbPublicationBase() { + virtual ~PublicationBase() { orb_unsubscribe(getHandle()); } const struct orb_metadata *getMeta() { return _meta; } @@ -83,12 +80,12 @@ protected: }; /** - * UOrb Publication wrapper class + * Publication wrapper class */ template -class UOrbPublication : +class Publication : public T, // this must be first! - public UOrbPublicationBase + public PublicationBase { public: /** @@ -98,13 +95,13 @@ public: * @param meta The uORB metadata (usually from the ORB_ID() macro) * for the topic. */ - UOrbPublication( - List * list, + Publication( + List * list, const struct orb_metadata *meta) : T(), // initialize data structure to zero - UOrbPublicationBase(list, meta) { + PublicationBase(list, meta) { } - virtual ~UOrbPublication() {} + virtual ~Publication() {} /* * XXX * This function gets the T struct, assuming @@ -115,4 +112,4 @@ public: void *getDataVoidPtr() { return (void *)(T *)(this); } }; -} // namespace control +} // namespace uORB diff --git a/src/modules/controllib/uorb/UOrbSubscription.cpp b/src/modules/uORB/Subscription.cpp similarity index 92% rename from src/modules/controllib/uorb/UOrbSubscription.cpp rename to src/modules/uORB/Subscription.cpp index 022cadd245..6e8830708b 100644 --- a/src/modules/controllib/uorb/UOrbSubscription.cpp +++ b/src/modules/uORB/Subscription.cpp @@ -32,20 +32,20 @@ ****************************************************************************/ /** - * @file UOrbSubscription.cpp + * @file Subscription.cpp * */ -#include "UOrbSubscription.hpp" +#include "Subscription.hpp" -namespace control +namespace uORB { -bool __EXPORT UOrbSubscriptionBase::updated() +bool __EXPORT SubscriptionBase::updated() { bool isUpdated = false; orb_check(_handle, &isUpdated); return isUpdated; } -} // namespace control +} // namespace uORB diff --git a/src/modules/controllib/uorb/UOrbSubscription.hpp b/src/modules/uORB/Subscription.hpp similarity index 85% rename from src/modules/controllib/uorb/UOrbSubscription.hpp rename to src/modules/uORB/Subscription.hpp index d337d89a88..55fb95d51c 100644 --- a/src/modules/controllib/uorb/UOrbSubscription.hpp +++ b/src/modules/uORB/Subscription.hpp @@ -32,28 +32,25 @@ ****************************************************************************/ /** - * @file UOrbSubscription.h + * @file Subscription.h * */ #pragma once #include -#include "../block/Block.hpp" -#include "../block/List.hpp" +#include -namespace control +namespace uORB { -class Block; - /** * Base subscription warapper class, used in list traversal * of various subscriptions. */ -class __EXPORT UOrbSubscriptionBase : - public ListNode +class __EXPORT SubscriptionBase : + public ListNode { public: // methods @@ -64,8 +61,8 @@ public: * @param meta The uORB metadata (usually from the ORB_ID() macro) * for the topic. */ - UOrbSubscriptionBase( - List * list, + SubscriptionBase( + List * list, const struct orb_metadata *meta) : _meta(meta), _handle() { @@ -78,7 +75,7 @@ public: } } virtual void *getDataVoidPtr() = 0; - virtual ~UOrbSubscriptionBase() { + virtual ~SubscriptionBase() { orb_unsubscribe(_handle); } // accessors @@ -93,12 +90,12 @@ protected: }; /** - * UOrb Subscription wrapper class + * Subscription wrapper class */ template -class __EXPORT UOrbSubscription : +class __EXPORT Subscription : public T, // this must be first! - public UOrbSubscriptionBase + public SubscriptionBase { public: /** @@ -109,11 +106,11 @@ public: * for the topic. * @param interval The minimum interval in milliseconds between updates */ - UOrbSubscription( - List * list, + Subscription( + List * list, const struct orb_metadata *meta, unsigned interval) : T(), // initialize data structure to zero - UOrbSubscriptionBase(list, meta) { + SubscriptionBase(list, meta) { setHandle(orb_subscribe(getMeta())); orb_set_interval(getHandle(), interval); } @@ -121,7 +118,7 @@ public: /** * Deconstructor */ - virtual ~UOrbSubscription() {} + virtual ~Subscription() {} /* * XXX @@ -134,4 +131,4 @@ public: T getData() { return T(*this); } }; -} // namespace control +} // namespace uORB diff --git a/src/modules/uORB/module.mk b/src/modules/uORB/module.mk index 5ec31ab010..0c29101fec 100644 --- a/src/modules/uORB/module.mk +++ b/src/modules/uORB/module.mk @@ -41,4 +41,6 @@ MODULE_COMMAND = uorb MODULE_STACKSIZE = 4096 SRCS = uORB.cpp \ - objects_common.cpp + objects_common.cpp \ + Publication.cpp \ + Subscription.cpp From da9dab27998fa9e8b7a66d53a00aa3cae93573ec Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sun, 16 Mar 2014 16:21:06 -0400 Subject: [PATCH 237/381] Moved List.hpp from controllib to src/include/containers. --- .../controllib/block => include/containers}/List.hpp | 6 +++--- src/modules/controllib/block/Block.hpp | 2 +- src/modules/controllib/block/BlockParam.hpp | 2 +- src/modules/uORB/Publication.hpp | 2 +- src/modules/uORB/Subscription.hpp | 2 +- 5 files changed, 7 insertions(+), 7 deletions(-) rename src/{modules/controllib/block => include/containers}/List.hpp (96%) diff --git a/src/modules/controllib/block/List.hpp b/src/include/containers/List.hpp similarity index 96% rename from src/modules/controllib/block/List.hpp rename to src/include/containers/List.hpp index 96b0b94d10..13cbda9382 100644 --- a/src/modules/controllib/block/List.hpp +++ b/src/include/containers/List.hpp @@ -32,9 +32,9 @@ ****************************************************************************/ /** - * @file Node.h + * @file List.hpp * - * A node of a linked list. + * A linked list. */ #pragma once @@ -43,7 +43,7 @@ template class __EXPORT ListNode { public: - ListNode() : _sibling(NULL) { + ListNode() : _sibling(nullptr) { } void setSibling(T sibling) { _sibling = sibling; } T getSibling() { return _sibling; } diff --git a/src/modules/controllib/block/Block.hpp b/src/modules/controllib/block/Block.hpp index 6a8e1b5b9c..736698e213 100644 --- a/src/modules/controllib/block/Block.hpp +++ b/src/modules/controllib/block/Block.hpp @@ -42,7 +42,7 @@ #include #include -#include "List.hpp" +#include // forward declaration namespace uORB { diff --git a/src/modules/controllib/block/BlockParam.hpp b/src/modules/controllib/block/BlockParam.hpp index 735b2b2922..a64d0139e3 100644 --- a/src/modules/controllib/block/BlockParam.hpp +++ b/src/modules/controllib/block/BlockParam.hpp @@ -42,7 +42,7 @@ #include #include "Block.hpp" -#include "List.hpp" +#include namespace control { diff --git a/src/modules/uORB/Publication.hpp b/src/modules/uORB/Publication.hpp index 7fa6bcc175..21840ed701 100644 --- a/src/modules/uORB/Publication.hpp +++ b/src/modules/uORB/Publication.hpp @@ -39,7 +39,7 @@ #pragma once #include -#include +#include namespace uORB diff --git a/src/modules/uORB/Subscription.hpp b/src/modules/uORB/Subscription.hpp index 55fb95d51c..d839f80239 100644 --- a/src/modules/uORB/Subscription.hpp +++ b/src/modules/uORB/Subscription.hpp @@ -39,7 +39,7 @@ #pragma once #include -#include +#include namespace uORB From afb2c37bfc20150718114c4ef56e40a7c4d4722f Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sun, 16 Mar 2014 17:01:03 -0400 Subject: [PATCH 238/381] Fixed uORB Pub/Sub templates for GCC 4.7 --- src/modules/uORB/Publication.cpp | 27 ++++++++++++++++++++++++++ src/modules/uORB/Publication.hpp | 12 ++++-------- src/modules/uORB/Subscription.cpp | 32 +++++++++++++++++++++++++++++++ src/modules/uORB/Subscription.hpp | 14 ++++---------- 4 files changed, 67 insertions(+), 18 deletions(-) diff --git a/src/modules/uORB/Publication.cpp b/src/modules/uORB/Publication.cpp index ed67b485de..78a5cd2045 100644 --- a/src/modules/uORB/Publication.cpp +++ b/src/modules/uORB/Publication.cpp @@ -37,3 +37,30 @@ */ #include "Publication.hpp" +#include "topics/vehicle_attitude.h" +#include "topics/vehicle_local_position.h" +#include "topics/vehicle_global_position.h" + +namespace uORB { + +template +Publication::Publication( + List * list, + const struct orb_metadata *meta) : + T(), // initialize data structure to zero + PublicationBase(list, meta) { +} + +template +Publication::~Publication() {} + +template +void * Publication::getDataVoidPtr() { + return (void *)(T *)(this); +} + +template class __EXPORT Publication; +template class __EXPORT Publication; +template class __EXPORT Publication; + +} diff --git a/src/modules/uORB/Publication.hpp b/src/modules/uORB/Publication.hpp index 21840ed701..8650b3df89 100644 --- a/src/modules/uORB/Publication.hpp +++ b/src/modules/uORB/Publication.hpp @@ -95,13 +95,9 @@ public: * @param meta The uORB metadata (usually from the ORB_ID() macro) * for the topic. */ - Publication( - List * list, - const struct orb_metadata *meta) : - T(), // initialize data structure to zero - PublicationBase(list, meta) { - } - virtual ~Publication() {} + Publication(List * list, + const struct orb_metadata *meta); + virtual ~Publication(); /* * XXX * This function gets the T struct, assuming @@ -109,7 +105,7 @@ public: * should use dynamic cast, but doesn't * seem to be available */ - void *getDataVoidPtr() { return (void *)(T *)(this); } + void *getDataVoidPtr(); }; } // namespace uORB diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp index 6e8830708b..16da3b66f4 100644 --- a/src/modules/uORB/Subscription.cpp +++ b/src/modules/uORB/Subscription.cpp @@ -37,6 +37,10 @@ */ #include "Subscription.hpp" +#include "topics/parameter_update.h" +#include "topics/actuator_controls.h" +#include "topics/vehicle_gps_position.h" +#include "topics/sensor_combined.h" namespace uORB { @@ -48,4 +52,32 @@ bool __EXPORT SubscriptionBase::updated() return isUpdated; } +template +Subscription::Subscription( + List * list, + const struct orb_metadata *meta, unsigned interval) : + T(), // initialize data structure to zero + SubscriptionBase(list, meta) { + setHandle(orb_subscribe(getMeta())); + orb_set_interval(getHandle(), interval); +} + +template +Subscription::~Subscription() {} + +template +void * Subscription::getDataVoidPtr() { + return (void *)(T *)(this); +} + +template +T Subscription::getData() { + return T(*this); +} + +template class __EXPORT Subscription; +template class __EXPORT Subscription; +template class __EXPORT Subscription; +template class __EXPORT Subscription; + } // namespace uORB diff --git a/src/modules/uORB/Subscription.hpp b/src/modules/uORB/Subscription.hpp index d839f80239..34e9a83e03 100644 --- a/src/modules/uORB/Subscription.hpp +++ b/src/modules/uORB/Subscription.hpp @@ -108,17 +108,11 @@ public: */ Subscription( List * list, - const struct orb_metadata *meta, unsigned interval) : - T(), // initialize data structure to zero - SubscriptionBase(list, meta) { - setHandle(orb_subscribe(getMeta())); - orb_set_interval(getHandle(), interval); - } - + const struct orb_metadata *meta, unsigned interval); /** * Deconstructor */ - virtual ~Subscription() {} + virtual ~Subscription(); /* * XXX @@ -127,8 +121,8 @@ public: * should use dynamic cast, but doesn't * seem to be available */ - void *getDataVoidPtr() { return (void *)(T *)(this); } - T getData() { return T(*this); } + void *getDataVoidPtr(); + T getData(); }; } // namespace uORB From 1b7472ef4c009caa8e76d6a4d90979677f14b0f4 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sun, 16 Mar 2014 17:06:37 -0400 Subject: [PATCH 239/381] Fix for md25 and uORB update. --- src/drivers/md25/md25.cpp | 2 +- src/modules/uORB/Publication.cpp | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/drivers/md25/md25.cpp b/src/drivers/md25/md25.cpp index 6d5e805ea1..5d1f58b854 100644 --- a/src/drivers/md25/md25.cpp +++ b/src/drivers/md25/md25.cpp @@ -52,7 +52,7 @@ #include #include -#include +#include #include #include diff --git a/src/modules/uORB/Publication.cpp b/src/modules/uORB/Publication.cpp index 78a5cd2045..dbd56e6425 100644 --- a/src/modules/uORB/Publication.cpp +++ b/src/modules/uORB/Publication.cpp @@ -40,6 +40,7 @@ #include "topics/vehicle_attitude.h" #include "topics/vehicle_local_position.h" #include "topics/vehicle_global_position.h" +#include "topics/debug_key_value.h" namespace uORB { @@ -62,5 +63,6 @@ void * Publication::getDataVoidPtr() { template class __EXPORT Publication; template class __EXPORT Publication; template class __EXPORT Publication; +template class __EXPORT Publication; } From 8f0b223c874b33bf09a8ceebcaf40cd60ee3800d Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sun, 16 Mar 2014 17:16:30 -0400 Subject: [PATCH 240/381] Fixed include style for List in Block. --- src/modules/controllib/block/Block.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/controllib/block/Block.cpp b/src/modules/controllib/block/Block.cpp index 0b4cce9792..6768bfa7ed 100644 --- a/src/modules/controllib/block/Block.cpp +++ b/src/modules/controllib/block/Block.cpp @@ -41,10 +41,11 @@ #include #include +#include +#include + #include "Block.hpp" #include "BlockParam.hpp" -#include "uORB/Subscription.hpp" -#include "uORB/Publication.hpp" namespace control { From eaef67f21df3d38a6523a79bb966fe62d44092ae Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sun, 16 Mar 2014 18:35:26 -0400 Subject: [PATCH 241/381] Added encoder uORB message/ fixedwing_backside working if enabled. --- src/modules/fixedwing_backside/fixedwing.cpp | 6 +- src/modules/uORB/Publication.cpp | 12 ++++ src/modules/uORB/Subscription.cpp | 20 ++++++ src/modules/uORB/objects_common.cpp | 3 + src/modules/uORB/topics/encoders.h | 66 ++++++++++++++++++++ 5 files changed, 104 insertions(+), 3 deletions(-) create mode 100644 src/modules/uORB/topics/encoders.h diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp index f7c0b61488..cfae072752 100644 --- a/src/modules/fixedwing_backside/fixedwing.cpp +++ b/src/modules/fixedwing_backside/fixedwing.cpp @@ -175,14 +175,14 @@ void BlockMultiModeBacksideAutopilot::update() // the min/max velocity float v = _vLimit.update(sqrtf( _pos.vel_n * _pos.vel_n + - _pos.vy * _pos.vy + + _pos.vel_e * _pos.vel_e + _pos.vel_d * _pos.vel_d)); // limit velocity command between min/max velocity float vCmd = _vLimit.update(_vCmd.get()); // altitude hold - float dThrottle = _h2Thr.update(_missionCmd.current.altitude - _pos.alt); + float dThrottle = _h2Thr.update(_missionCmd.current.alt - _pos.alt); // heading hold float psiError = _wrap_pi(_guide.getPsiCmd() - _att.yaw); @@ -237,7 +237,7 @@ void BlockMultiModeBacksideAutopilot::update() // the min/max velocity float v = _vLimit.update(sqrtf( _pos.vel_n * _pos.vel_n + - _pos.vy * _pos.vy + + _pos.vel_e * _pos.vel_e + _pos.vel_d * _pos.vel_d)); // pitch channel -> rate of climb diff --git a/src/modules/uORB/Publication.cpp b/src/modules/uORB/Publication.cpp index dbd56e6425..5a59816172 100644 --- a/src/modules/uORB/Publication.cpp +++ b/src/modules/uORB/Publication.cpp @@ -41,6 +41,12 @@ #include "topics/vehicle_local_position.h" #include "topics/vehicle_global_position.h" #include "topics/debug_key_value.h" +#include "topics/actuator_controls.h" +#include "topics/vehicle_global_velocity_setpoint.h" +#include "topics/vehicle_attitude_setpoint.h" +#include "topics/vehicle_rates_setpoint.h" +#include "topics/actuator_outputs.h" +#include "topics/encoders.h" namespace uORB { @@ -64,5 +70,11 @@ 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; +template class __EXPORT Publication; +template class __EXPORT Publication; +template class __EXPORT Publication; } diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp index 16da3b66f4..c1d1a938f1 100644 --- a/src/modules/uORB/Subscription.cpp +++ b/src/modules/uORB/Subscription.cpp @@ -41,6 +41,16 @@ #include "topics/actuator_controls.h" #include "topics/vehicle_gps_position.h" #include "topics/sensor_combined.h" +#include "topics/vehicle_attitude.h" +#include "topics/vehicle_global_position.h" +#include "topics/encoders.h" +#include "topics/position_setpoint_triplet.h" +#include "topics/vehicle_status.h" +#include "topics/manual_control_setpoint.h" +#include "topics/vehicle_local_position_setpoint.h" +#include "topics/vehicle_local_position.h" +#include "topics/vehicle_attitude_setpoint.h" +#include "topics/vehicle_rates_setpoint.h" namespace uORB { @@ -79,5 +89,15 @@ template class __EXPORT Subscription; template class __EXPORT Subscription; template class __EXPORT Subscription; template class __EXPORT Subscription; +template class __EXPORT Subscription; +template class __EXPORT Subscription; +template class __EXPORT Subscription; +template class __EXPORT Subscription; +template class __EXPORT Subscription; +template class __EXPORT Subscription; +template class __EXPORT Subscription; +template class __EXPORT Subscription; +template class __EXPORT Subscription; +template class __EXPORT Subscription; } // namespace uORB diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 4c84c1f25e..fb24de8d16 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -190,3 +190,6 @@ ORB_DEFINE(navigation_capabilities, struct navigation_capabilities_s); #include "topics/esc_status.h" ORB_DEFINE(esc_status, struct esc_status_s); + +#include "topics/encoders.h" +ORB_DEFINE(encoders, struct encoders_s); diff --git a/src/modules/uORB/topics/encoders.h b/src/modules/uORB/topics/encoders.h new file mode 100644 index 0000000000..588c0ddb13 --- /dev/null +++ b/src/modules/uORB/topics/encoders.h @@ -0,0 +1,66 @@ +/**************************************************************************** + * + * 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 encoders.h + * + * Encoders topic. + * + */ + +#ifndef TOPIC_ENCODERS_H +#define TOPIC_ENCODERS_H + +#include +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +#define NUM_ENCODERS 4 + +struct encoders_s { + uint64_t timestamp; + int64_t counts[NUM_ENCODERS]; // counts of encoder + float velocity[NUM_ENCODERS]; // counts of encoder/ second +}; + +/** + * @} + */ + +ORB_DECLARE(encoders); + +#endif From ab088c8ac65bfb220ac5ed0853b243026e77600a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 21 Mar 2014 08:06:06 +0100 Subject: [PATCH 242/381] Altitute wip --- .../fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 8b41e7479d..e46dffde3c 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -735,7 +735,7 @@ FixedwingEstimator::task_main() orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); _baro_ref = _baro.altitude; baroHgt = _baro.altitude - _baro_ref; - _baro_gps_offset = baroHgt - alt; + _baro_gps_offset = _baro_ref - _local_pos.ref_alt; // XXX this is not multithreading safe map_projection_init(lat, lon); @@ -984,10 +984,11 @@ FixedwingEstimator::task_main() _global_pos.vel_e = 0.0f; } - _global_pos.alt = _local_pos.ref_alt - _local_pos.z; + /* local pos alt is negative, change sign and add alt offset */ + _global_pos.alt = _local_pos.ref_alt + (-_local_pos.z); if (_local_pos.z_valid) { - _global_pos.baro_alt = _baro_ref - _baro_gps_offset - _local_pos.z; + _global_pos.baro_alt = _local_pos.ref_alt - _baro_gps_offset - _local_pos.z; } if (_local_pos.v_z_valid) { From 98ef3b9ab882da077c67f81eb2f7852c44badb36 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 21 Mar 2014 08:07:56 +0100 Subject: [PATCH 243/381] Make driver start less verbose --- src/drivers/ets_airspeed/ets_airspeed.cpp | 1 - src/drivers/meas_airspeed/meas_airspeed.cpp | 13 ++++--------- 2 files changed, 4 insertions(+), 10 deletions(-) diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index b6e80ce1d0..e77f31e872 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -132,7 +132,6 @@ ETSAirspeed::measure() if (OK != ret) { perf_count(_comms_errors); - log("i2c::transfer returned %d", ret); } return ret; diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 7baa8738f6..4f64d85859 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -169,18 +169,13 @@ MEASAirspeed::collect() uint8_t status = (val[0] & 0xC0) >> 6; switch (status) { - case 0: break; + case 0: + break; case 1: - perf_count(_comms_errors); - perf_end(_sample_perf); - return -EAGAIN; - + /* fallthrough */ case 2: - perf_count(_comms_errors); - perf_end(_sample_perf); - return -EAGAIN; - + /* fallthrough */ case 3: perf_count(_comms_errors); perf_end(_sample_perf); From 8af83f70747d104511cc100e4f94ff4d5c6ac6d4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 21 Mar 2014 08:11:28 +0100 Subject: [PATCH 244/381] Make error message less dramatic, as its part of a scanning progress --- src/drivers/ets_airspeed/ets_airspeed.cpp | 2 +- src/drivers/meas_airspeed/meas_airspeed.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index e77f31e872..d27ab9727a 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -307,7 +307,7 @@ fail: g_dev = nullptr; } - errx(1, "driver start failed"); + errx(1, "no ETS airspeed sensor connected"); } /** diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 4f64d85859..beca28e7a4 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -356,7 +356,7 @@ fail: g_dev = nullptr; } - errx(1, "driver start failed"); + errx(1, "no MS4525 airspeed sensor connected"); } /** From b8afcf5863fd59d740fd81a77cfc97cc9ecdc07a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 21 Mar 2014 08:32:54 +0100 Subject: [PATCH 245/381] Ensure that the mavlink start call only returns once the new instance is fully initialized. This avoids race conditions in getopt() and it ensures that the mavlink debug fd is ready when other processes start --- src/modules/mavlink/mavlink_main.cpp | 24 ++++++++++++++++++++++-- 1 file changed, 22 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 9c2e0178a3..c9f4835ba8 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -254,7 +254,6 @@ Mavlink::Mavlink() : break; } - LL_APPEND(_mavlink_instances, this); } Mavlink::~Mavlink() @@ -1778,6 +1777,9 @@ Mavlink::task_main(int argc, char *argv[]) /* set main loop delay depending on data rate to minimize CPU overhead */ _main_loop_delay = MAIN_LOOP_DELAY / rate_mult; + /* now the instance is fully initialized and we can bump the instance count */ + LL_APPEND(_mavlink_instances, this); + while (!_task_should_exit) { /* main loop */ usleep(_main_loop_delay); @@ -1924,10 +1926,18 @@ int Mavlink::start_helper(int argc, char *argv[]) int Mavlink::start(int argc, char *argv[]) { + // Wait for the instance count to go up one + // before returning to the shell + int ic = Mavlink::instance_count(); + // Instantiate thread char buf[24]; - sprintf(buf, "mavlink_if%d", Mavlink::instance_count()); + sprintf(buf, "mavlink_if%d", ic); + // This is where the control flow splits + // between the starting task and the spawned + // task - start_helper() only returns + // when the started task exits. task_spawn_cmd(buf, SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, @@ -1935,6 +1945,16 @@ Mavlink::start(int argc, char *argv[]) (main_t)&Mavlink::start_helper, (const char **)argv); + // Ensure that this shell command + // does not return before the instance + // is fully initialized. As this is also + // the only path to create a new instance, + // this is effectively a lock on concurrent + // instance starting. XXX do a real lock. + while (ic == Mavlink::instance_count()) { + ::usleep(500); + } + return OK; } From 055e45935534b409cd3ed68c837399461e448b3e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 21 Mar 2014 08:33:49 +0100 Subject: [PATCH 246/381] Remove unneeded headers --- src/modules/mavlink/mavlink.c | 25 ------------------------- 1 file changed, 25 deletions(-) diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 542bf0d7e4..ad435b251b 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -45,31 +45,6 @@ #include #include "mavlink_bridge_header.h" #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include - -// -// #include -// #include -// #include -// #include - -// #include "waypoints.h" -// #include "orb_topics.h" -// #include "mavlink_hil.h" -// #include "util.h" -// #include "waypoints.h" -// #include "mavlink_parameters.h" - -// #include /* define MAVLink specific parameters */ /** From 2988136e7eaa1f55c41376b74b58766af9f8fcb9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 21 Mar 2014 10:44:31 +0100 Subject: [PATCH 247/381] Implemented last missing messages, added stream config for USB, made stream config fails for non-existing mavlink links non-fatal --- ROMFS/px4fmu_common/init.d/rc.usb | 4 + src/modules/mavlink/mavlink_main.cpp | 21 ++- src/modules/mavlink/mavlink_messages.cpp | 156 +++++++++++++---------- 3 files changed, 110 insertions(+), 71 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index 558be42755..2af790fc43 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -6,6 +6,10 @@ echo "Starting MAVLink on this USB console" mavlink start -r 10000 -d /dev/ttyACM0 +# Enable a number of interesting streams we want via USB +mavlink stream -d /dev/ttyACM0 -s NAMED_VALUE_FLOAT -r 10 +mavlink stream -d /dev/ttyACM0 -s OPTICAL_FLOW -r 10 +mavlink stream -d /dev/ttyACM0 -s ATTITUDE_CONTROL -r 30 # Exit shell to make it available to MAVLink exit diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index c9f4835ba8..00d906dcd9 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -588,7 +588,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * /* setup output flow control */ if (enable_flow_control(true)) { - warnx("ERR FLOW CTRL EN"); + warnx("hardware flow control not supported"); } } @@ -1762,6 +1762,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("GLOBAL_POSITION_INT", 3.0f * rate_mult); configure_stream("LOCAL_POSITION_NED", 3.0f * rate_mult); configure_stream("RC_CHANNELS_RAW", 1.0f * rate_mult); + configure_stream("NAMED_VALUE_FLOAT", 1.0f * rate_mult); break; default: @@ -1951,8 +1952,17 @@ Mavlink::start(int argc, char *argv[]) // the only path to create a new instance, // this is effectively a lock on concurrent // instance starting. XXX do a real lock. - while (ic == Mavlink::instance_count()) { - ::usleep(500); + + // Sleep 500 us between each attempt + const unsigned sleeptime = 500; + + // Wait 100 ms max for the startup. + const unsigned limit = 100 * 1000 / sleeptime; + + unsigned count = 0; + while (ic == Mavlink::instance_count() && count < limit) { + ::usleep(sleeptime); + count++; } return OK; @@ -2008,7 +2018,10 @@ Mavlink::stream(int argc, char *argv[]) inst->configure_stream_threadsafe(stream_name, rate); } else { - errx(1, "mavlink for device %s is not running", device_name); + + // If the link is not running we should complain, but not fall over + // because this is so easy to get wrong and not fatal. Warning is sufficient. + errx(0, "mavlink for device %s is not running", device_name); } } else { diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 7d388f88da..dc01935ca3 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1127,6 +1127,93 @@ protected: } }; +class MavlinkStreamAttitudeControls : public MavlinkStream +{ +public: + const char *get_name() + { + return "ATTITUDE_CONTROLS"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamAttitudeControls(); + } + +private: + MavlinkOrbSubscription *att_ctrl_sub; + struct actuator_controls_s *att_ctrl; + +protected: + void subscribe(Mavlink *mavlink) + { + att_ctrl_sub = mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + att_ctrl = (struct actuator_controls_s *)att_ctrl_sub->get_data(); + } + + void send(const hrt_abstime t) + { + if (att_ctrl_sub->update(t)) { + + // send, add spaces so that string buffer is at least 10 chars long + mavlink_msg_named_value_float_send(_channel, + att_ctrl->timestamp / 1000, + "rll ctrl ", + att_ctrl->control[0]); + mavlink_msg_named_value_float_send(_channel, + att_ctrl->timestamp / 1000, + "ptch ctrl ", + att_ctrl->control[1]); + mavlink_msg_named_value_float_send(_channel, + att_ctrl->timestamp / 1000, + "yaw ctrl ", + att_ctrl->control[2]); + mavlink_msg_named_value_float_send(_channel, + att_ctrl->timestamp / 1000, + "thr ctrl ", + att_ctrl->control[3]); + } + } +}; + +class MavlinkStreamNamedValueFloat : public MavlinkStream +{ +public: + const char *get_name() + { + return "NAMED_VALUE_FLOAT"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamNamedValueFloat(); + } + +private: + MavlinkOrbSubscription *debug_sub; + struct debug_key_value_s *debug; + +protected: + void subscribe(Mavlink *mavlink) + { + debug_sub = mavlink->add_orb_subscription(ORB_ID(debug_key_value)); + debug = (struct debug_key_value_s *)debug_sub->get_data(); + } + + void send(const hrt_abstime t) + { + if (debug_sub->update(t)) { + + // Enforce null termination + debug->key[sizeof(debug->key) - 1] = '\0'; + + mavlink_msg_named_value_float_send(_channel, + debug->timestamp_ms, + debug->key, + debug->value); + } + } +}; MavlinkStream *streams_list[] = { new MavlinkStreamHeartbeat(), @@ -1151,72 +1238,7 @@ MavlinkStream *streams_list[] = { new MavlinkStreamRCChannelsRaw(), new MavlinkStreamManualControl(), new MavlinkStreamOpticalFlow(), + new MavlinkStreamAttitudeControls(), + new MavlinkStreamNamedValueFloat(), nullptr }; - - - - - - - -// -// -// -// -// -// -//void -//MavlinkOrbListener::l_vehicle_attitude_controls(const struct listener *l) -//{ -// orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, l->mavlink->get_subs()->actuators_sub, &l->listener->actuators_0); -// -// if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) { -// /* send, add spaces so that string buffer is at least 10 chars long */ -// mavlink_msg_named_value_float_send(l->mavlink->get_chan(), -// l->listener->last_sensor_timestamp / 1000, -// "ctrl0 ", -// l->listener->actuators_0.control[0]); -// mavlink_msg_named_value_float_send(l->mavlink->get_chan(), -// l->listener->last_sensor_timestamp / 1000, -// "ctrl1 ", -// l->listener->actuators_0.control[1]); -// mavlink_msg_named_value_float_send(l->mavlink->get_chan(), -// l->listener->last_sensor_timestamp / 1000, -// "ctrl2 ", -// l->listener->actuators_0.control[2]); -// mavlink_msg_named_value_float_send(l->mavlink->get_chan(), -// l->listener->last_sensor_timestamp / 1000, -// "ctrl3 ", -// l->listener->actuators_0.control[3]); -// } -//} -// -//void -//MavlinkOrbListener::l_debug_key_value(const struct listener *l) -//{ -// struct debug_key_value_s debug; -// -// orb_copy(ORB_ID(debug_key_value), l->mavlink->get_subs()->debug_key_value, &debug); -// -// /* Enforce null termination */ -// debug.key[sizeof(debug.key) - 1] = '\0'; -// -// mavlink_msg_named_value_float_send(l->mavlink->get_chan(), -// l->listener->last_sensor_timestamp / 1000, -// debug.key, -// debug.value); -//} -// -//void -//MavlinkOrbListener::l_nav_cap(const struct listener *l) -//{ -// -// orb_copy(ORB_ID(navigation_capabilities), l->mavlink->get_subs()->navigation_capabilities_sub, &l->listener->nav_cap); -// -// mavlink_msg_named_value_float_send(l->mavlink->get_chan(), -// hrt_absolute_time() / 1000, -// "turn dist", -// l->listener->nav_cap.turn_distance); -// -//} From 057bcf3172f3c8d1bab561e7e4cad14977cd74d0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 21 Mar 2014 10:45:12 +0100 Subject: [PATCH 248/381] Changed RC scaling for fixed wing defaults --- ROMFS/px4fmu_common/init.d/rc.fw_defaults | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults index 3a50fcf56f..4cd73e23fe 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults @@ -11,4 +11,6 @@ then param set NAV_RTL_ALT 100 param set NAV_RTL_LAND_T -1 param set NAV_ACCEPT_RAD 50 + param set RC_SCALE_ROLL 1 + param set RC_SCALE_PITCH 1 fi \ No newline at end of file From b2f8fcb9d904901127b937551c149e904c8aa3a9 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 21 Mar 2014 18:49:39 +0400 Subject: [PATCH 249/381] position_estimator_inav: added NaN checks --- .../position_estimator_inav/inertial_filter.c | 24 ++++++++++++------- 1 file changed, 15 insertions(+), 9 deletions(-) diff --git a/src/modules/position_estimator_inav/inertial_filter.c b/src/modules/position_estimator_inav/inertial_filter.c index 7cd0769486..2f1b3c0145 100644 --- a/src/modules/position_estimator_inav/inertial_filter.c +++ b/src/modules/position_estimator_inav/inertial_filter.c @@ -5,24 +5,30 @@ * Author: Anton Babushkin */ +#include + #include "inertial_filter.h" void inertial_filter_predict(float dt, float x[3]) { - x[0] += x[1] * dt + x[2] * dt * dt / 2.0f; - x[1] += x[2] * dt; + if (isfinite(dt)) { + x[0] += x[1] * dt + x[2] * dt * dt / 2.0f; + x[1] += x[2] * dt; + } } void inertial_filter_correct(float e, float dt, float x[3], int i, float w) { - float ewdt = e * w * dt; - x[i] += ewdt; + if (isfinite(e) && isfinite(w) && isfinite(dt)) { + float ewdt = e * w * dt; + x[i] += ewdt; - if (i == 0) { - x[1] += w * ewdt; - x[2] += w * w * ewdt / 3.0; + if (i == 0) { + x[1] += w * ewdt; + x[2] += w * w * ewdt / 3.0; - } else if (i == 1) { - x[2] += w * ewdt; + } else if (i == 1) { + x[2] += w * ewdt; + } } } From b1e59f37fe3f652002f2e92a205de5994c9c8120 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 21 Mar 2014 19:48:53 +0400 Subject: [PATCH 250/381] rc.usb: typo fixed --- ROMFS/px4fmu_common/init.d/rc.usb | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index 2af790fc43..2aeea5cbe7 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -9,7 +9,7 @@ mavlink start -r 10000 -d /dev/ttyACM0 # Enable a number of interesting streams we want via USB mavlink stream -d /dev/ttyACM0 -s NAMED_VALUE_FLOAT -r 10 mavlink stream -d /dev/ttyACM0 -s OPTICAL_FLOW -r 10 -mavlink stream -d /dev/ttyACM0 -s ATTITUDE_CONTROL -r 30 +mavlink stream -d /dev/ttyACM0 -s ATTITUDE_CONTROLS -r 30 # Exit shell to make it available to MAVLink exit From 1c49d768fbf2b69bf2500a4dc48ac2a95c0d2ba1 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 21 Mar 2014 20:06:34 +0400 Subject: [PATCH 251/381] mavlink: code style fixed --- src/modules/mavlink/mavlink_main.cpp | 41 ++++++++--- src/modules/mavlink/mavlink_messages.cpp | 90 ++++++++++-------------- 2 files changed, 68 insertions(+), 63 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index ff1c070cb6..9a8e1e5a75 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -151,8 +151,9 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length } /* no valid instance, bail */ - if (!instance) + if (!instance) { return; + } int uart = instance->get_uart_fd(); @@ -165,12 +166,12 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length int buf_free = 0; if (instance->get_flow_control_enabled() - && ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) { + && ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) { if (buf_free == 0) { if (last_write_times[(unsigned)channel] != 0 && - hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500*1000UL) { + hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500 * 1000UL) { warnx("DISABLING HARDWARE FLOW CONTROL"); instance->enable_flow_control(false); @@ -225,30 +226,37 @@ Mavlink::Mavlink() : case 0: _channel = MAVLINK_COMM_0; break; + case 1: _channel = MAVLINK_COMM_1; break; + case 2: _channel = MAVLINK_COMM_2; break; + case 3: _channel = MAVLINK_COMM_3; break; #ifdef MAVLINK_COMM_4 + case 4: _channel = MAVLINK_COMM_4; break; #endif #ifdef MAVLINK_COMM_5 - case 5: + + case 5: _channel = MAVLINK_COMM_5; break; #endif #ifdef MAVLINK_COMM_6 + case 6: _channel = MAVLINK_COMM_6; break; #endif + default: errx(1, "instance ID is out of range"); break; @@ -279,6 +287,7 @@ Mavlink::~Mavlink() } } while (_task_running); } + LL_DELETE(_mavlink_instances, this); } @@ -604,12 +613,16 @@ Mavlink::enable_flow_control(bool enabled) } struct termios uart_config; + int ret = tcgetattr(_uart_fd, &uart_config); + if (enabled) { uart_config.c_cflag |= CRTSCTS; + } else { uart_config.c_cflag &= ~CRTSCTS; } + ret = tcsetattr(_uart_fd, TCSANOW, &uart_config); if (!ret) { @@ -1652,6 +1665,7 @@ Mavlink::task_main(int argc, char *argv[]) case 'm': if (strcmp(optarg, "custom") == 0) { _mode = MAVLINK_MODE_CUSTOM; + } else if (strcmp(optarg, "camera") == 0) { _mode = MAVLINK_MODE_CAMERA; } @@ -1973,6 +1987,7 @@ Mavlink::start(int argc, char *argv[]) const unsigned limit = 100 * 1000 / sleeptime; unsigned count = 0; + while (ic == Mavlink::instance_count() && count < limit) { ::usleep(sleeptime); count++; @@ -2003,20 +2018,26 @@ Mavlink::stream(int argc, char *argv[]) bool err_flag = false; int i = 0; + while (i < argc) { - if (0 == strcmp(argv[i], "-r") && i < argc - 1 ) { - rate = strtod(argv[i+1], nullptr); + if (0 == strcmp(argv[i], "-r") && i < argc - 1) { + rate = strtod(argv[i + 1], nullptr); + if (rate < 0.0f) { err_flag = true; } + i++; - } else if (0 == strcmp(argv[i], "-d") && i < argc - 1 ) { - device_name = argv[i+1]; + + } else if (0 == strcmp(argv[i], "-d") && i < argc - 1) { + device_name = argv[i + 1]; i++; - } else if (0 == strcmp(argv[i], "-s") && i < argc - 1 ) { - stream_name = argv[i+1]; + + } else if (0 == strcmp(argv[i], "-s") && i < argc - 1) { + stream_name = argv[i + 1]; i++; + } else { err_flag = true; } diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index a4a3c681f8..037999af75 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -262,7 +262,6 @@ protected: void send(const hrt_abstime t) { if (status_sub->update(t)) { - mavlink_msg_sys_status_send(_channel, status->onboard_control_sensors_present, status->onboard_control_sensors_enabled, @@ -318,7 +317,6 @@ protected: void send(const hrt_abstime t) { if (sensor_sub->update(t)) { - uint16_t fields_updated = 0; if (accel_timestamp != sensor->accelerometer_timestamp) { @@ -385,7 +383,6 @@ protected: void send(const hrt_abstime t) { if (att_sub->update(t)) { - mavlink_msg_attitude_send(_channel, att->timestamp / 1000, att->roll, att->pitch, att->yaw, @@ -422,7 +419,6 @@ protected: void send(const hrt_abstime t) { if (att_sub->update(t)) { - mavlink_msg_attitude_quaternion_send(_channel, att->timestamp / 1000, att->q[0], @@ -494,7 +490,6 @@ protected: updated |= airspeed_sub->update(t); if (updated) { - float groundspeed = sqrtf(pos->vel_n * pos->vel_n + pos->vel_e * pos->vel_e); uint16_t heading = _wrap_2pi(att->yaw) * M_RAD_TO_DEG_F; float throttle = armed->armed ? act->control[3] * 100.0f : 0.0f; @@ -538,7 +533,6 @@ protected: void send(const hrt_abstime t) { if (gps_sub->update(t)) { - mavlink_msg_gps_raw_int_send(_channel, gps->timestamp_position, gps->fix_type, @@ -592,15 +586,15 @@ protected: if (updated) { mavlink_msg_global_position_int_send(_channel, - pos->timestamp / 1000, - pos->lat * 1e7, - pos->lon * 1e7, - pos->alt * 1000.0f, - (pos->alt - home->alt) * 1000.0f, - pos->vel_n * 100.0f, - pos->vel_e * 100.0f, - pos->vel_d * 100.0f, - _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f); + pos->timestamp / 1000, + pos->lat * 1e7, + pos->lon * 1e7, + pos->alt * 1000.0f, + (pos->alt - home->alt) * 1000.0f, + pos->vel_n * 100.0f, + pos->vel_e * 100.0f, + pos->vel_d * 100.0f, + _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f); } } }; @@ -633,7 +627,6 @@ protected: void send(const hrt_abstime t) { if (pos_sub->update(t)) { - mavlink_msg_local_position_ned_send(_channel, pos->timestamp / 1000, pos->x, @@ -730,7 +723,6 @@ protected: void send(const hrt_abstime t) { if (act_sub->update(t)) { - mavlink_msg_servo_output_raw_send(_channel, act->timestamp / 1000, _n, @@ -790,7 +782,6 @@ protected: updated |= act_sub->update(t); if (updated) { - /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state; uint8_t mavlink_base_mode; @@ -870,7 +861,6 @@ protected: void send(const hrt_abstime t) { if (pos_sp_triplet_sub->update(t)) { - mavlink_msg_global_position_setpoint_int_send(_channel, MAV_FRAME_GLOBAL, (int32_t)(pos_sp_triplet->current.lat * 1e7), @@ -908,14 +898,14 @@ protected: void send(const hrt_abstime t) { - pos_sp_sub->update(t); - - mavlink_msg_local_position_setpoint_send(_channel, - MAV_FRAME_LOCAL_NED, - pos_sp->x, - pos_sp->y, - pos_sp->z, - pos_sp->yaw); + if (pos_sp_sub->update(t)) { + mavlink_msg_local_position_setpoint_send(_channel, + MAV_FRAME_LOCAL_NED, + pos_sp->x, + pos_sp->y, + pos_sp->z, + pos_sp->yaw); + } } }; @@ -947,7 +937,6 @@ protected: void send(const hrt_abstime t) { if (att_sp_sub->update(t)) { - mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel, att_sp->timestamp / 1000, att_sp->roll_body, @@ -986,7 +975,6 @@ protected: void send(const hrt_abstime t) { if (att_rates_sp_sub->update(t)) { - mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel, att_rates_sp->timestamp / 1000, att_rates_sp->roll, @@ -1025,7 +1013,6 @@ protected: void send(const hrt_abstime t) { if (rc_sub->update(t)) { - const unsigned port_width = 8; for (unsigned i = 0; (i * port_width) < rc->channel_count; i++) { @@ -1075,7 +1062,6 @@ protected: void send(const hrt_abstime t) { if (manual_sub->update(t)) { - mavlink_msg_manual_control_send(_channel, mavlink_system.sysid, manual->roll * 1000, @@ -1115,7 +1101,6 @@ protected: void send(const hrt_abstime t) { if (flow_sub->update(t)) { - mavlink_msg_optical_flow_send(_channel, flow->timestamp, flow->sensor_id, @@ -1154,24 +1139,23 @@ protected: void send(const hrt_abstime t) { if (att_ctrl_sub->update(t)) { - - // send, add spaces so that string buffer is at least 10 chars long - mavlink_msg_named_value_float_send(_channel, - att_ctrl->timestamp / 1000, - "rll ctrl ", - att_ctrl->control[0]); - mavlink_msg_named_value_float_send(_channel, - att_ctrl->timestamp / 1000, - "ptch ctrl ", - att_ctrl->control[1]); - mavlink_msg_named_value_float_send(_channel, - att_ctrl->timestamp / 1000, - "yaw ctrl ", - att_ctrl->control[2]); - mavlink_msg_named_value_float_send(_channel, - att_ctrl->timestamp / 1000, - "thr ctrl ", - att_ctrl->control[3]); + /* send, add spaces so that string buffer is at least 10 chars long */ + mavlink_msg_named_value_float_send(_channel, + att_ctrl->timestamp / 1000, + "rll ctrl ", + att_ctrl->control[0]); + mavlink_msg_named_value_float_send(_channel, + att_ctrl->timestamp / 1000, + "ptch ctrl ", + att_ctrl->control[1]); + mavlink_msg_named_value_float_send(_channel, + att_ctrl->timestamp / 1000, + "yaw ctrl ", + att_ctrl->control[2]); + mavlink_msg_named_value_float_send(_channel, + att_ctrl->timestamp / 1000, + "thr ctrl ", + att_ctrl->control[3]); } } }; @@ -1203,8 +1187,7 @@ protected: void send(const hrt_abstime t) { if (debug_sub->update(t)) { - - // Enforce null termination + /* enforce null termination */ debug->key[sizeof(debug->key) - 1] = '\0'; mavlink_msg_named_value_float_send(_channel, @@ -1246,10 +1229,11 @@ protected: (void)status_sub->update(t); if (status->arming_state == ARMING_STATE_ARMED - || status->arming_state == ARMING_STATE_ARMED_ERROR) { + || status->arming_state == ARMING_STATE_ARMED_ERROR) { /* send camera capture on */ mavlink_msg_command_long_send(_channel, 42, 30, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); + } else { /* send camera capture off */ mavlink_msg_command_long_send(_channel, 42, 30, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0); From 7fa6b48a36d80ed0b0b3b81cebf45810c0d6f42d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 21 Mar 2014 17:51:46 +0100 Subject: [PATCH 252/381] wingwing startup script: add FW_THR_CRUISE param --- ROMFS/px4fmu_common/init.d/3033_wingwing | 1 + 1 file changed, 1 insertion(+) diff --git a/ROMFS/px4fmu_common/init.d/3033_wingwing b/ROMFS/px4fmu_common/init.d/3033_wingwing index 22d63fad57..7ff381d974 100644 --- a/ROMFS/px4fmu_common/init.d/3033_wingwing +++ b/ROMFS/px4fmu_common/init.d/3033_wingwing @@ -29,6 +29,7 @@ then param set FW_RR_FF 0.6 param set FW_RR_IMA 0.2 param set FW_RR_P 0.09 + param set FW_THR_CRUISE 0.65 fi set MIXER FMU_Q From 69bc0c8e3998f87358bbf8a5f97f5c31dca43f76 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 21 Mar 2014 18:12:02 +0100 Subject: [PATCH 253/381] Reduced param rate - as long as we do not have proper QoS (= reducing all rates at once if link becomes lossy) we need to open-loop match the link --- 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 9a8e1e5a75..18df577fea 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1800,7 +1800,7 @@ Mavlink::task_main(int argc, char *argv[]) _mavlink_param_queue_index = param_count(); MavlinkRateLimiter slow_rate_limiter(2000000.0f / rate_mult); - MavlinkRateLimiter fast_rate_limiter(20000.0f / rate_mult); + MavlinkRateLimiter fast_rate_limiter(30000.0f / rate_mult); /* set main loop delay depending on data rate to minimize CPU overhead */ _main_loop_delay = MAIN_LOOP_DELAY / rate_mult; From 5f5a200f4a46ed63a96be4011249d94aa312401b Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sat, 22 Mar 2014 10:09:30 +0100 Subject: [PATCH 254/381] Add support for the SkyHunter 1800 --- ROMFS/px4fmu_common/init.d/2103_skyhunter_1800 | 5 +++++ ROMFS/px4fmu_common/init.d/rc.autostart | 6 ++++++ 2 files changed, 11 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/2103_skyhunter_1800 diff --git a/ROMFS/px4fmu_common/init.d/2103_skyhunter_1800 b/ROMFS/px4fmu_common/init.d/2103_skyhunter_1800 new file mode 100644 index 0000000000..9bc0262d83 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/2103_skyhunter_1800 @@ -0,0 +1,5 @@ +#!nsh + +sh /etc/init.d/rc.fw_defaults + +set MIXER FMU_AET diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 4ae6a27940..5ec735d393 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -68,6 +68,12 @@ then set MODE custom fi +if param compare SYS_AUTOSTART 2103 103 +then + sh /etc/init.d/2103_skyhunter_1800 + set MODE custom +fi + # # Flying wing # From 6b610dc1f88eab46cbfe9c297256d2ac6aa81baf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 22 Mar 2014 10:44:14 +0100 Subject: [PATCH 255/381] Hotfix for comment: Removed syntax-breaking symbol --- src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 0909135e15..37f06dbe5e 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 @@ -245,7 +245,7 @@ PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f); /** * Maximum vertical acceleration * - * This is the maximum vertical acceleration (in metres/second^2) + * 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 From 2a178f9847008e5263a51df877856b9b10199c20 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 22 Mar 2014 17:09:32 +0100 Subject: [PATCH 256/381] wingwing startup: fix typo --- ROMFS/px4fmu_common/init.d/3033_wingwing | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/3033_wingwing b/ROMFS/px4fmu_common/init.d/3033_wingwing index 7ff381d974..6e1a531cfa 100644 --- a/ROMFS/px4fmu_common/init.d/3033_wingwing +++ b/ROMFS/px4fmu_common/init.d/3033_wingwing @@ -27,7 +27,7 @@ then param set FW_PR_IMAX 0.2 param set FW_PR_P 0.06 param set FW_RR_FF 0.6 - param set FW_RR_IMA 0.2 + param set FW_RR_IMAX 0.2 param set FW_RR_P 0.09 param set FW_THR_CRUISE 0.65 fi From 2ce39a4afee147eb302d01590db091f85554c51e Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sat, 22 Mar 2014 13:30:43 -0400 Subject: [PATCH 257/381] Added youcompleteme config. --- .ycm_extra_conf.py | 173 +++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 173 insertions(+) create mode 100644 .ycm_extra_conf.py diff --git a/.ycm_extra_conf.py b/.ycm_extra_conf.py new file mode 100644 index 0000000000..fed5cd28a0 --- /dev/null +++ b/.ycm_extra_conf.py @@ -0,0 +1,173 @@ +# This file is NOT licensed under the GPLv3, which is the license for the rest +# of YouCompleteMe. +# +# Here's the license text for this file: +# +# This is free and unencumbered software released into the public domain. +# +# Anyone is free to copy, modify, publish, use, compile, sell, or +# distribute this software, either in source code form or as a compiled +# binary, for any purpose, commercial or non-commercial, and by any +# means. +# +# In jurisdictions that recognize copyright laws, the author or authors +# of this software dedicate any and all copyright interest in the +# software to the public domain. We make this dedication for the benefit +# of the public at large and to the detriment of our heirs and +# successors. We intend this dedication to be an overt act of +# relinquishment in perpetuity of all present and future rights to this +# software under copyright law. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, +# EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. +# IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR +# OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, +# ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR +# OTHER DEALINGS IN THE SOFTWARE. +# +# For more information, please refer to + +import os +import ycm_core + +# These are the compilation flags that will be used in case there's no +# compilation database set (by default, one is not set). +# CHANGE THIS LIST OF FLAGS. YES, THIS IS THE DROID YOU HAVE BEEN LOOKING FOR. +flags = [ +'-Wall', +'-Wextra', +'-Werror', +#'-Wc++98-compat', +'-Wno-long-long', +'-Wno-variadic-macros', +'-fexceptions', +'-DNDEBUG', +# You 100% do NOT need -DUSE_CLANG_COMPLETER in your flags; only the YCM +# source code needs it. +#'-DUSE_CLANG_COMPLETER', +# THIS IS IMPORTANT! Without a "-std=" flag, clang won't know which +# language to use when compiling headers. So it will guess. Badly. So C++ +# headers will be compiled as C headers. You don't want that so ALWAYS specify +# a "-std=". +# For a C project, you would set this to something like 'c99' instead of +# 'c++11'. +'-std=c++11', +# ...and the same thing goes for the magic -x option which specifies the +# language that the files to be compiled are written in. This is mostly +# relevant for c++ headers. +# For a C project, you would set this to 'c' instead of 'c++'. +'-x', +'c++', +'-undef', # get rid of standard definitions to allow us to include arm math header +'-I', os.path.join(os.path.expanduser("~"),'gcc-arm-none-eabi-4_7-2013q3/arm-none-eabi/include'), +'-I', 'Build/px4io-v1_default.build/nuttx-export/include/', +'-I', 'Build/px4io-v2_default.build/nuttx-export/include/', +'-I', './NuttX/nuttx/arch/arm/include', +'-include', './src/include/visibility.h', +'-I', './src', +'-I', './src/modules', +'-I', './src/include', +'-I', './src/lib', +'-I', './NuttX', +] + + +# Set this to the absolute path to the folder (NOT the file!) containing the +# compile_commands.json file to use that instead of 'flags'. See here for +# more details: http://clang.llvm.org/docs/JSONCompilationDatabase.html +# +# Most projects will NOT need to set this to anything; you can just change the +# 'flags' list of compilation flags. Notice that YCM itself uses that approach. +compilation_database_folder = '' + +if os.path.exists( compilation_database_folder ): + database = ycm_core.CompilationDatabase( compilation_database_folder ) +else: + database = None + +SOURCE_EXTENSIONS = [ '.cpp', '.cxx', '.cc', '.c', '.m', '.mm' ] + +def DirectoryOfThisScript(): + return os.path.dirname( os.path.abspath( __file__ ) ) + + +def MakeRelativePathsInFlagsAbsolute( flags, working_directory ): + if not working_directory: + return list( flags ) + new_flags = [] + make_next_absolute = False + path_flags = [ '-isystem', '-I', '-iquote', '--sysroot=' ] + for flag in flags: + new_flag = flag + + if make_next_absolute: + make_next_absolute = False + if not flag.startswith( '/' ): + new_flag = os.path.join( working_directory, flag ) + + for path_flag in path_flags: + if flag == path_flag: + make_next_absolute = True + break + + if flag.startswith( path_flag ): + path = flag[ len( path_flag ): ] + new_flag = path_flag + os.path.join( working_directory, path ) + break + + if new_flag: + new_flags.append( new_flag ) + return new_flags + + +def IsHeaderFile( filename ): + extension = os.path.splitext( filename )[ 1 ] + return extension in [ '.h', '.hxx', '.hpp', '.hh' ] + + +def GetCompilationInfoForFile( filename ): + # The compilation_commands.json file generated by CMake does not have entries + # for header files. So we do our best by asking the db for flags for a + # corresponding source file, if any. If one exists, the flags for that file + # should be good enough. + if IsHeaderFile( filename ): + basename = os.path.splitext( filename )[ 0 ] + for extension in SOURCE_EXTENSIONS: + replacement_file = basename + extension + if os.path.exists( replacement_file ): + compilation_info = database.GetCompilationInfoForFile( + replacement_file ) + if compilation_info.compiler_flags_: + return compilation_info + return None + return database.GetCompilationInfoForFile( filename ) + + +def FlagsForFile( filename, **kwargs ): + if database: + # Bear in mind that compilation_info.compiler_flags_ does NOT return a + # python list, but a "list-like" StringVec object + compilation_info = GetCompilationInfoForFile( filename ) + if not compilation_info: + return None + + final_flags = MakeRelativePathsInFlagsAbsolute( + compilation_info.compiler_flags_, + compilation_info.compiler_working_dir_ ) + + # NOTE: This is just for YouCompleteMe; it's highly likely that your project + # does NOT need to remove the stdlib flag. DO NOT USE THIS IN YOUR + # ycm_extra_conf IF YOU'RE NOT 100% SURE YOU NEED IT. + #try: + # final_flags.remove( '-stdlib=libc++' ) + #except ValueError: + # pass + else: + relative_to = DirectoryOfThisScript() + final_flags = MakeRelativePathsInFlagsAbsolute( flags, relative_to ) + + return { + 'flags': final_flags, + 'do_cache': True + } From 17c18947a4b06d1973faa3c102f7a985c13c31e3 Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Sun, 23 Mar 2014 01:05:06 -0400 Subject: [PATCH 258/381] Remove uneccesary dependencies and update credits --- src/modules/dataman/dataman.c | 14 ++++---------- src/modules/dataman/module.mk | 2 -- 2 files changed, 4 insertions(+), 12 deletions(-) diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index fa88dfaffb..d8652aaf90 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -1,8 +1,10 @@ /**************************************************************************** * * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier - * Jean Cyr + * Author: Jean Cyr + * Lorenz Meier + * Julian Oes + * Thomas Gubler * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -40,16 +42,8 @@ #include #include #include -#include -#include #include -#include -#include -#include -#include -#include #include -#include #include #include "dataman.h" diff --git a/src/modules/dataman/module.mk b/src/modules/dataman/module.mk index dce7a62358..27670dd3fd 100644 --- a/src/modules/dataman/module.mk +++ b/src/modules/dataman/module.mk @@ -38,5 +38,3 @@ MODULE_COMMAND = dataman SRCS = dataman.c - -INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink From debb4f70219c6fbe95c44783fc9768aa0016cd1e Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Sun, 23 Mar 2014 01:35:13 -0400 Subject: [PATCH 259/381] Coding style astyle according to http://pixhawk.org/dev/code_style --- src/modules/dataman/dataman.c | 20 +++++++++++++------- 1 file changed, 13 insertions(+), 7 deletions(-) diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index d8652aaf90..34d20e4851 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -169,8 +169,10 @@ create_work_item(void) /* Try to reuse item from free item queue */ lock_queue(&g_free_q); + if ((item = (work_q_item_t *)sq_remfirst(&(g_free_q.q)))) g_free_q.size--; + unlock_queue(&g_free_q); /* If we there weren't any free items then obtain memory for a new one */ @@ -283,11 +285,11 @@ _write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const v offset = calculate_offset(item, index); /* If item type or index out of range, return error */ - if (offset < 0) + if (offset < 0) return -1; /* Make sure caller has not given us more data than we can handle */ - if (count > DM_MAX_DATA_SIZE) + if (count > DM_MAX_DATA_SIZE) return -1; /* Write out the data, prefixed with length and persistence level */ @@ -333,6 +335,7 @@ _read(dm_item_t item, unsigned char index, void *buf, size_t count) /* Read the prefix and data */ len = -1; + if (lseek(g_task_fd, offset, SEEK_SET) == offset) len = read(g_task_fd, buffer, count + DM_SECTOR_HDR_SIZE); @@ -486,7 +489,7 @@ dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const return -1; /* get a work item and queue up a write request */ - if ((work = create_work_item()) == NULL) + if ((work = create_work_item()) == NULL) return -1; work->func = dm_write_func; @@ -593,17 +596,20 @@ task_main(int argc, char *argv[]) /* Open or create the data manager file */ g_task_fd = open(k_data_manager_device_path, O_RDWR | O_CREAT | O_BINARY); + if (g_task_fd < 0) { warnx("Could not open data manager file %s", k_data_manager_device_path); sem_post(&g_init_sema); /* Don't want to hang startup */ return -1; } + if (lseek(g_task_fd, max_offset, SEEK_SET) != max_offset) { close(g_task_fd); warnx("Could not seek data manager file %s", k_data_manager_device_path); sem_post(&g_init_sema); /* Don't want to hang startup */ return -1; } + fsync(g_task_fd); /* We use two file descriptors, one for the caller context and one for the worker thread */ @@ -761,10 +767,10 @@ dataman_main(int argc, char *argv[]) stop(); else if (!strcmp(argv[1], "status")) status(); - else if (!strcmp(argv[1], "poweronrestart")) - dm_restart(DM_INIT_REASON_POWER_ON); - else if (!strcmp(argv[1], "inflightrestart")) - dm_restart(DM_INIT_REASON_IN_FLIGHT); + else if (!strcmp(argv[1], "poweronrestart")) + dm_restart(DM_INIT_REASON_POWER_ON); + else if (!strcmp(argv[1], "inflightrestart")) + dm_restart(DM_INIT_REASON_IN_FLIGHT); else usage(); From 50d3fe2a1ace205f32ab9e236c3ddb1832d4f19c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 10 Mar 2014 23:37:26 +0100 Subject: [PATCH 260/381] fw pos ctrl: add wrapper function for tecs --- .../fw_pos_control_l1_main.cpp | 96 ++++++++++--------- 1 file changed, 53 insertions(+), 43 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 7f13df7854..d6c706b3b7 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 @@ -356,6 +356,15 @@ private: * Reset landing state */ void reset_landing_state(); + + /* + * Call TECS + */ + void tecs_update_pitch_throttle(float alt_sp, float v_sp, float eas2tas, + float pitch_min_rad, float pitch_max_rad, + float throttle_min, float throttle_max, float throttle_cruise, + bool climbout_mode, float climbout_pitch_min_rad); + }; namespace l1_control @@ -819,11 +828,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, math::radians(_parameters.pitch_limit_min), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + false, math::radians(_parameters.pitch_limit_min)); } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER) { @@ -833,11 +841,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, math::radians(_parameters.pitch_limit_min), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + false, math::radians(_parameters.pitch_limit_min)); } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { @@ -929,11 +936,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi land_stayonground = true; } - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, flare_curve_alt, calculate_target_airspeed(airspeed_land), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, flare_pitch_angle_rad, - 0.0f, throttle_max, throttle_land, - flare_pitch_angle_rad, math::radians(15.0f)); + tecs_update_pitch_throttle(flare_curve_alt, 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); if (!land_noreturn_vertical) { mavlink_log_info(_mavlink_fd, "#audio: Landing, flaring"); @@ -962,11 +968,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi altitude_desired = math::max(_global_pos.alt, L_altitude); } - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, altitude_desired, calculate_target_airspeed(airspeed_approach), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, math::radians(_parameters.pitch_limit_min), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + tecs_update_pitch_throttle(altitude_desired, calculate_target_airspeed(airspeed_approach), eas2tas, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + false, math::radians(_parameters.pitch_limit_min)); } } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF) { @@ -1004,22 +1009,19 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (altitude_error > 15.0f) { /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(1.3f * _parameters.airspeed_min), - _airspeed.indicated_airspeed_m_s, eas2tas, - true, math::max(math::radians(pos_sp_triplet.current.pitch_min), math::radians(10.0f)), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(1.3f * _parameters.airspeed_min), eas2tas, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + true, math::max(math::radians(pos_sp_triplet.current.pitch_min), math::radians(10.0f))); /* limit roll motion to ensure enough lift */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f)); } else { - - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, math::radians(_parameters.pitch_limit_min), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + false, math::radians(_parameters.pitch_limit_min)); } } else { @@ -1083,12 +1085,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f, - seatbelt_airspeed, - _airspeed.indicated_airspeed_m_s, eas2tas, - false, _parameters.pitch_limit_min, - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - _parameters.pitch_limit_min, _parameters.pitch_limit_max); + tecs_update_pitch_throttle(_global_pos.alt + _manual.pitch * 2.0f, seatbelt_airspeed, eas2tas, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + false, math::radians(_parameters.pitch_limit_min)); } else if (0/* seatbelt mode enabled */) { @@ -1127,12 +1127,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed); _att_sp.roll_body = _manual.roll; _att_sp.yaw_body = _manual.yaw; - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f, - seatbelt_airspeed, - _airspeed.indicated_airspeed_m_s, eas2tas, - climb_out, _parameters.pitch_limit_min, - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - _parameters.pitch_limit_min, _parameters.pitch_limit_max); + tecs_update_pitch_throttle(_global_pos.alt + _manual.pitch * 2.0f, seatbelt_airspeed, eas2tas, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + climb_out, math::radians(_parameters.pitch_limit_min)); } else { @@ -1328,6 +1326,18 @@ void FixedwingPositionControl::reset_landing_state() land_onslope = false; } +void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_sp, float eas2tas, + float pitch_min_rad, float pitch_max_rad, + float throttle_min, float throttle_max, float throttle_cruise, + bool climbout_mode, float climbout_pitch_min_rad) +{ + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, alt_sp, v_sp, + _airspeed.indicated_airspeed_m_s, eas2tas, + climbout_mode, climbout_pitch_min_rad, + throttle_min, throttle_max, throttle_cruise, + pitch_min_rad, pitch_max_rad); +} + int FixedwingPositionControl::start() { From 9c751fe9c5f604613a26b1d82e0735db44c9da3c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 19 Mar 2014 00:05:57 +0100 Subject: [PATCH 261/381] mtecs --- .../fw_pos_control_l1_main.cpp | 101 +++++--- src/modules/fw_pos_control_l1/module.mk | 4 +- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 152 ++++++++++++ src/modules/fw_pos_control_l1/mtecs/mTecs.h | 113 +++++++++ .../fw_pos_control_l1/mtecs/mTecs_blocks.h | 198 +++++++++++++++ .../fw_pos_control_l1/mtecs/mTecs_params.c | 231 ++++++++++++++++++ 6 files changed, 766 insertions(+), 33 deletions(-) create mode 100644 src/modules/fw_pos_control_l1/mtecs/mTecs.cpp create mode 100644 src/modules/fw_pos_control_l1/mtecs/mTecs.h create mode 100644 src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h create mode 100644 src/modules/fw_pos_control_l1/mtecs/mTecs_params.c diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index d6c706b3b7..1e8447aed5 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 @@ -90,6 +90,7 @@ #include #include #include "landingslope.h" +#include "mtecs/mTecs.h" /** @@ -198,6 +199,8 @@ private: ECL_L1_Pos_Controller _l1_control; TECS _tecs; + fwPosctrl::mTecs _mTecs; + bool _was_pos_control_mode; struct { float l1_period; @@ -331,11 +334,11 @@ private: /** * Control position. */ - bool control_position(const math::Vector<2> &global_pos, const math::Vector<2> &ground_speed, + bool control_position(const math::Vector<2> &global_pos, const math::Vector<3> &ground_speed, const struct position_setpoint_triplet_s &_pos_sp_triplet); float calculate_target_airspeed(float airspeed_demand); - void calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet); + void calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed_2d, const struct position_setpoint_triplet_s &pos_sp_triplet); /** * Shim for calling task_main from task_create. @@ -363,7 +366,8 @@ private: void tecs_update_pitch_throttle(float alt_sp, float v_sp, float eas2tas, float pitch_min_rad, float pitch_max_rad, float throttle_min, float throttle_max, float throttle_cruise, - bool climbout_mode, float climbout_pitch_min_rad); + bool climbout_mode, float climbout_pitch_min_rad, + const math::Vector<3> &ground_speed); }; @@ -426,7 +430,9 @@ FixedwingPositionControl::FixedwingPositionControl() : _control_mode(), _global_pos(), _pos_sp_triplet(), - _sensor_combined() + _sensor_combined(), + _mTecs(), + _was_pos_control_mode(false) { _nav_capabilities.turn_distance = 0.0f; @@ -583,6 +589,9 @@ FixedwingPositionControl::parameters_update() /* Update Launch Detector Parameters */ launchDetector.updateParams(); + /* Update the mTecs */ + _mTecs.updateParams(); + return OK; } @@ -714,7 +723,7 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand) } void -FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet) +FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed_2d, const struct position_setpoint_triplet_s &pos_sp_triplet) { if (_global_pos_valid && !(pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER)) { @@ -722,7 +731,7 @@ FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> &c /* rotate ground speed vector with current attitude */ math::Vector<2> yaw_vector(_R_nb(0, 0), _R_nb(1, 0)); yaw_vector.normalize(); - float ground_speed_body = yaw_vector * ground_speed; + float ground_speed_body = yaw_vector * ground_speed_2d; /* The minimum desired ground speed is the minimum airspeed projected on to the ground using the altitude and horizontal difference between the waypoints if available*/ float distance = 0.0f; @@ -764,12 +773,13 @@ void FixedwingPositionControl::navigation_capabilities_publish() } bool -FixedwingPositionControl::control_position(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, +FixedwingPositionControl::control_position(const math::Vector<2> ¤t_position, const math::Vector<3> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet) { bool setpoint = true; - calculate_gndspeed_undershoot(current_position, ground_speed, pos_sp_triplet); + math::Vector<2> ground_speed_2d = {ground_speed(0), ground_speed(1)}; + calculate_gndspeed_undershoot(current_position, ground_speed_2d, pos_sp_triplet); float eas2tas = 1.0f; // XXX calculate actual number based on current measurements @@ -792,6 +802,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // else integrators should be constantly reset. if (_control_mode.flag_control_position_enabled) { + if (!_was_pos_control_mode) { + /* reset integrators */ + if (_mTecs.getEnabled()) { + _mTecs.resetIntegrators(); + } + } + + _was_pos_control_mode = true; + /* get circle mode */ bool was_circle_mode = _l1_control.circle_mode(); @@ -824,27 +843,27 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (pos_sp_triplet.current.type == SETPOINT_TYPE_NORMAL) { /* waypoint is a plain navigation waypoint */ - _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed); + _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(); tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - false, math::radians(_parameters.pitch_limit_min)); + false, math::radians(_parameters.pitch_limit_min), ground_speed); } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER) { /* waypoint is a loiter waypoint */ _l1_control.navigate_loiter(curr_wp, current_position, pos_sp_triplet.current.loiter_radius, - pos_sp_triplet.current.loiter_direction, ground_speed); + pos_sp_triplet.current.loiter_direction, ground_speed_2d); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - false, math::radians(_parameters.pitch_limit_min)); + false, math::radians(_parameters.pitch_limit_min), ground_speed); } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { @@ -869,7 +888,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw)); - _l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed); + _l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed_2d); /* limit roll motion to prevent wings from touching the ground first */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f)); @@ -879,7 +898,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else { /* normal navigation */ - _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed); + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); } _att_sp.roll_body = _l1_control.nav_roll(); @@ -939,7 +958,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi tecs_update_pitch_throttle(flare_curve_alt, 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); + false, flare_pitch_angle_rad, ground_speed); if (!land_noreturn_vertical) { mavlink_log_info(_mavlink_fd, "#audio: Landing, flaring"); @@ -971,7 +990,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi tecs_update_pitch_throttle(altitude_desired, calculate_target_airspeed(airspeed_approach), eas2tas, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - false, math::radians(_parameters.pitch_limit_min)); + false, math::radians(_parameters.pitch_limit_min), ground_speed); } } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF) { @@ -998,7 +1017,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } } - _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed); + _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(); @@ -1012,7 +1031,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(1.3f * _parameters.airspeed_min), eas2tas, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - true, math::max(math::radians(pos_sp_triplet.current.pitch_min), math::radians(10.0f))); + true, math::max(math::radians(pos_sp_triplet.current.pitch_min), math::radians(10.0f)), ground_speed); /* 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)); @@ -1021,7 +1040,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - false, math::radians(_parameters.pitch_limit_min)); + false, math::radians(_parameters.pitch_limit_min), ground_speed); } } else { @@ -1055,6 +1074,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else if (0/* easy mode enabled */) { + _was_pos_control_mode = false; + /** EASY FLIGHT **/ if (0/* switched from another mode to easy */) { @@ -1082,16 +1103,18 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi (_parameters.airspeed_max - _parameters.airspeed_min) * _manual.throttle; - _l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed); + _l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed_2d); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); tecs_update_pitch_throttle(_global_pos.alt + _manual.pitch * 2.0f, seatbelt_airspeed, eas2tas, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - false, math::radians(_parameters.pitch_limit_min)); + false, math::radians(_parameters.pitch_limit_min), ground_speed); } else if (0/* seatbelt mode enabled */) { + _was_pos_control_mode = false; + /** SEATBELT FLIGHT **/ if (0/* switched from another mode to seatbelt */) { @@ -1124,16 +1147,18 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi climb_out = true; } - _l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed); + _l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed_2d); _att_sp.roll_body = _manual.roll; _att_sp.yaw_body = _manual.yaw; tecs_update_pitch_throttle(_global_pos.alt + _manual.pitch * 2.0f, seatbelt_airspeed, eas2tas, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - climb_out, math::radians(_parameters.pitch_limit_min)); + climb_out, math::radians(_parameters.pitch_limit_min), ground_speed); } else { + _was_pos_control_mode = false; + /** MANUAL FLIGHT **/ /* no flight mode applies, do not publish an attitude setpoint */ @@ -1150,9 +1175,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); } else { - _att_sp.thrust = math::min(_tecs.get_throttle_demand(), throttle_max); + _att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() :_tecs.get_throttle_demand(), throttle_max); } - _att_sp.pitch_body = _tecs.get_pitch_demand(); + _att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand(); if (_control_mode.flag_control_position_enabled) { last_manual = false; @@ -1264,7 +1289,7 @@ FixedwingPositionControl::task_main() vehicle_airspeed_poll(); // vehicle_baro_poll(); - math::Vector<2> ground_speed(_global_pos.vel_n, _global_pos.vel_e); + math::Vector<3> ground_speed(_global_pos.vel_n, _global_pos.vel_e, _global_pos.vel_d); math::Vector<2> current_position((float)_global_pos.lat, (float)_global_pos.lon); /* @@ -1329,13 +1354,25 @@ void FixedwingPositionControl::reset_landing_state() void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_sp, float eas2tas, float pitch_min_rad, float pitch_max_rad, float throttle_min, float throttle_max, float throttle_cruise, - bool climbout_mode, float climbout_pitch_min_rad) + bool climbout_mode, float climbout_pitch_min_rad, + const math::Vector<3> &ground_speed) { - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, alt_sp, v_sp, - _airspeed.indicated_airspeed_m_s, eas2tas, - climbout_mode, climbout_pitch_min_rad, - throttle_min, throttle_max, throttle_cruise, - pitch_min_rad, pitch_max_rad); + if (_mTecs.getEnabled()) { + + float flightPathAngle = 0.0f; + float ground_speed_length = ground_speed.length(); + if (ground_speed_length > FLT_EPSILON) { + flightPathAngle = -asinf(ground_speed(2)/ground_speed_length); + } + _mTecs.updateAltitudeSpeed(flightPathAngle, _global_pos.alt, alt_sp, _airspeed.true_airspeed_m_s, v_sp); + + } else { + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, alt_sp, v_sp, + _airspeed.indicated_airspeed_m_s, eas2tas, + climbout_mode, climbout_pitch_min_rad, + throttle_min, throttle_max, throttle_cruise, + pitch_min_rad, pitch_max_rad); + } } int diff --git a/src/modules/fw_pos_control_l1/module.mk b/src/modules/fw_pos_control_l1/module.mk index cf419ec7ea..5d752437fb 100644 --- a/src/modules/fw_pos_control_l1/module.mk +++ b/src/modules/fw_pos_control_l1/module.mk @@ -39,4 +39,6 @@ MODULE_COMMAND = fw_pos_control_l1 SRCS = fw_pos_control_l1_main.cpp \ fw_pos_control_l1_params.c \ - landingslope.cpp + landingslope.cpp \ + mtecs/mTecs.cpp\ + mtecs/mTecs_params.c diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp new file mode 100644 index 0000000000..ad108e7eec --- /dev/null +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -0,0 +1,152 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Thomas Gubler + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + +/** + * @file mTecs.cpp + * + * @author Thomas Gubler + */ + +#include "mTecs.h" + +#include +#include + +namespace fwPosctrl { + +mTecs::mTecs() : + SuperBlock(NULL, "MT"), + /* Parameters */ + _mTecsEnabled(this, "ENABLED"), + /* control blocks */ + _controlTotalEnergy(this, "THR"), + _controlEnergyDistribution(this, "PIT", true), + _controlAltitude(this, "FPA", true), + _controlAirSpeed(this, "ACC"), + _airspeedDerivative(this, "AD"), + _throttleSp(0.0f), + _pitchSp(0.0f), + timestampLastIteration(hrt_absolute_time()), + _firstIterationAfterReset(true) +{ +} + +mTecs::~mTecs() +{ +} + +void mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, float airspeedSp) +{ + warnx("***"); + float flightPathAngleSp = _controlAltitude.update(altitudeSp - altitude); + warnx("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)flightPathAngleSp); + updateFlightPathAngleSpeed(flightPathAngle, flightPathAngleSp, airspeed, airspeedSp); +} + +void mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, float airspeedSp) { + + float accelerationLongitudinalSp = _controlAirSpeed.update(airspeedSp - airspeed); + warnx("updateFlightPathAngleSpeed airspeedSp %.4f, airspeed %.4f, accelerationLongitudinalSp%.4f", (double)airspeedSp, (double)airspeed, (double)accelerationLongitudinalSp); + updateFlightPathAngleAcceleration(flightPathAngle, flightPathAngleSp, airspeed, accelerationLongitudinalSp); +} + +void mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed, float accelerationLongitudinalSp) +{ + /* time measurement */ + float deltaTSeconds = 0.0f; + if (!_firstIterationAfterReset) { + hrt_abstime timestampNow = hrt_absolute_time(); + deltaTSeconds = (float)(timestampNow - timestampLastIteration) * 1e-6f; + timestampLastIteration = timestampNow; + } + setDt(deltaTSeconds); + + /* update parameters first */ + updateParams(); + + /* calculate values (energies) */ + float flightPathAngleError = flightPathAngleSp - flightPathAngle; + float airspeedDerivative = 0.0f; + if(_airspeedDerivative.getDt() > 0.0f) { + airspeedDerivative = _airspeedDerivative.update(airspeed); + } + float airspeedDerivativeNorm = airspeedDerivative / CONSTANTS_ONE_G; + float airspeedSpDerivative = accelerationLongitudinalSp; + float airspeedSpDerivativeNorm = airspeedSpDerivative / CONSTANTS_ONE_G; + float airspeedSpDerivativeNormError = airspeedSpDerivativeNorm - airspeedDerivativeNorm; + + float totalEnergyRate = flightPathAngle + airspeedDerivativeNorm; + float totalEnergyRateError = flightPathAngleError + airspeedSpDerivativeNormError; + float totalEnergyRateSp = flightPathAngleSp + airspeedSpDerivativeNorm; + float totalEnergyRateError2 = totalEnergyRateSp - totalEnergyRate; + + float energyDistributionRate = flightPathAngle - airspeedDerivativeNorm; + float energyDistributionRateError = flightPathAngleError - airspeedSpDerivativeNormError; + float energyDistributionRateSp = flightPathAngleSp - airspeedSpDerivativeNorm; + float energyDistributionRateError2 = energyDistributionRateSp - energyDistributionRate; + + warnx("totalEnergyRateSp %.2f, totalEnergyRate %.2f, totalEnergyRateError %.2f totalEnergyRateError2 %.2f airspeedDerivativeNorm %.4f", + (double)totalEnergyRateSp, (double)totalEnergyRate, (double)totalEnergyRateError, (double)totalEnergyRateError2, (double)airspeedDerivativeNorm); + warnx("energyDistributionRateSp %.2f, energyDistributionRate %.2f, energyDistributionRateError %.2f energyDistributionRateError2 %.2f", + (double)energyDistributionRateSp, (double)energyDistributionRate, (double)energyDistributionRateError, (double)energyDistributionRateError2); + + /** update control blocks **/ + /* update total energy rate control block */ + _throttleSp = _controlTotalEnergy.update(totalEnergyRateSp, totalEnergyRateError); + + /* update energy distribution rate control block */ + _pitchSp = _controlEnergyDistribution.update(energyDistributionRateSp, energyDistributionRateError); + + warnx("_throttleSp %.1f, _pitchSp %.1f, flightPathAngleSp %.1f, flightPathAngle %.1f accelerationLongitudinalSp %.1f, airspeedDerivative %.1f", + (double)_throttleSp, (double)_pitchSp, + (double)flightPathAngleSp, (double)flightPathAngle, + (double)accelerationLongitudinalSp, (double)airspeedDerivative); + + + /* clean up */ + _firstIterationAfterReset = false; + +} + +void mTecs::resetIntegrators() +{ + _controlTotalEnergy.getIntegral().setY(0.0f); + _controlEnergyDistribution.getIntegral().setY(0.0f); + timestampLastIteration = hrt_absolute_time(); + _firstIterationAfterReset = true; +} + +} /* namespace fwPosctrl */ + diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h new file mode 100644 index 0000000000..5877a8312b --- /dev/null +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -0,0 +1,113 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Thomas Gubler + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + +/** + * @file mTecs.h + * + * @author Thomas Gubler + */ + + +#ifndef MTECS_H_ +#define MTECS_H_ + +#include "mTecs_blocks.h" + +#include +#include + +namespace fwPosctrl +{ + +/* Main class of the mTecs */ +class mTecs : public control::SuperBlock +{ +public: + mTecs(); + virtual ~mTecs(); + + /* + * Control in altitude setpoint and speed mode + */ + void updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, float airspeedSp); + + /* + * Control in flightPathAngle setpoint (flollow a slope etc.) and speed mode + */ + void updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, float airspeedSp); + + /* + * Control in flightPathAngle setpoint (flollow a slope etc.) and acceleration mode (base case) + */ + void updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed, float accelerationLongitudinalSp); + + /* + * Reset all integrators + */ + void resetIntegrators(); + + + /* Accessors */ + bool getEnabled() {return _mTecsEnabled.get() > 0;} + float getThrottleSetpoint() {return _throttleSp;} + float getPitchSetpoint() {return _pitchSp;} + +protected: + /* parameters */ + control::BlockParamInt _mTecsEnabled; /**< 1 if mTecs is enabled */ + + /* control blocks */ + BlockFFPILimited _controlTotalEnergy; /**< FFPI controller for total energy control: output is throttle */ + BlockFFPILimited _controlEnergyDistribution; /**< FFPI controller for energy distribution control: output is pitch */ + BlockPLimited _controlAltitude; /**< P controller for altitude: output is the flight path angle setpoint */ + BlockPLimited _controlAirSpeed; /**< P controller for airspeed: output is acceleration setpoint */ + + /* Other calculation Blocks */ + control::BlockDerivative _airspeedDerivative; + + /* Output setpoints */ + float _throttleSp; /**< Throttle Setpoint from 0 to 1 */ + float _pitchSp; /**< Pitch Setpoint from -pi to pi */ + + /* Time measurements */ + hrt_abstime timestampLastIteration; /**< Saves the result of hrt_absolute_time() of the last iteration */ + + bool _firstIterationAfterReset; /**< True during the first iteration after a reset */ + +}; + +} /* namespace fwPosctrl */ + +#endif /* MTECS_H_ */ diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h new file mode 100644 index 0000000000..7dd5c7c2e2 --- /dev/null +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h @@ -0,0 +1,198 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Thomas Gubler + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + +/** + * @file mTecs_blocks.h + * + * Custom blocks for the mTecs + * + * @author Thomas Gubler + */ + +#pragma once + +#include +#include + +namespace fwPosctrl +{ + +using namespace control; + +/* Integrator without limit */ +class BlockIntegralNoLimit: public SuperBlock +{ +public: +// methods + BlockIntegralNoLimit(SuperBlock *parent, const char *name) : + SuperBlock(parent, name), + _y(0) {}; + virtual ~BlockIntegralNoLimit() {}; + float update(float input) { + setY(getY() + input * getDt()); + return getY(); + }; +// accessors + float getY() {return _y;} + void setY(float y) {_y = y;} +protected: +// attributes + float _y; /**< previous output */ +}; + +/* An block which can be used to limit the output */ +class BlockOutputLimiter: public SuperBlock +{ +public: +// methods + BlockOutputLimiter(SuperBlock *parent, const char *name, bool isAngularLimit = false) : + SuperBlock(parent, name), + _isAngularLimit(isAngularLimit), + _min(this, "MIN"), + _max(this, "MAX") + {}; + virtual ~BlockOutputLimiter() {}; + bool limit(float& value, float& difference) { + float minimum = isAngularLimit() ? getMin() * M_DEG_TO_RAD_F : getMin(); + float maximum = isAngularLimit() ? getMax() * M_DEG_TO_RAD_F : getMax(); + char name[blockNameLengthMax]; + getName(name, blockNameLengthMax); + warnx("%s minimum %.2f maximum %.2f, getMin() %.2f, getMax() %.2f, isAngularLimit() %u", + name,(double)minimum,(double)maximum, (double)getMin(), (double)getMax(), isAngularLimit()); + if (value < minimum) { + difference = value - minimum; + value = minimum; + return true; + } else if (value > maximum) { + difference = value - maximum; + value = maximum; + return true; + } + return false; + } +//accessor: + bool isAngularLimit() {return _isAngularLimit ;} + float getMin() { return _min.get(); } + float getMax() { return _max.get(); } +protected: +//attributes + bool _isAngularLimit; + control::BlockParamFloat _min; + control::BlockParamFloat _max; +}; + +typedef + +/* A combination of feed forward, P and I gain using the output limiter*/ +class BlockFFPILimited: public SuperBlock +{ +public: +// methods + BlockFFPILimited(SuperBlock *parent, const char *name, bool isAngularLimit = false) : + SuperBlock(parent, name), + _integral(this, "I"), + _kFF(this, "FF"), + _kP(this, "P"), + _kI(this, "I"), + _offset(this, "OFF"), + _outputLimiter(this, "", isAngularLimit) + {}; + virtual ~BlockFFPILimited() {}; + float update(float inputValue, float inputError) { + float difference = 0.0f; + float integralYPrevious = _integral.getY(); + float output = getOffset() + getKFF() * inputValue + getKP() * inputError + getKI() * getIntegral().update(inputError); + char name[blockNameLengthMax]; + getName(name, blockNameLengthMax); + warnx("%s output %.2f getKFF() %.2f, inputValue %.2f, getKP() %.2f, getKI() %.2f, getIntegral().getY() %.2f, inputError %.2f getIntegral().getDt() %.2f", name, + (double)output, (double)getKFF(), (double)inputValue, (double)getKP(), (double)getKI(), (double)getIntegral().getY(), (double)inputError, (double)getIntegral().getDt()); + if(!getOutputLimiter().limit(output, difference) && + (((difference < 0) && (getKI() * getIntegral().update(inputError) < 0)) || + ((difference > 0) && (getKI() * getIntegral().update(inputError) > 0)))) { + getIntegral().setY(integralYPrevious); + } + warnx("%s output limited %.2f", + name,(double)output); + return output; + } +// accessors + BlockIntegralNoLimit &getIntegral() { return _integral; } + float getKFF() { return _kFF.get(); } + float getKP() { return _kP.get(); } + float getKI() { return _kI.get(); } + float getOffset() { return _offset.get(); } + BlockOutputLimiter &getOutputLimiter() { return _outputLimiter; }; +private: + BlockIntegralNoLimit _integral; + BlockParamFloat _kFF; + BlockParamFloat _kP; + BlockParamFloat _kI; + BlockParamFloat _offset; + BlockOutputLimiter _outputLimiter; +}; + +/* A combination of P gain and output limiter */ +class BlockPLimited: public SuperBlock +{ +public: +// methods + BlockPLimited(SuperBlock *parent, const char *name, bool isAngularLimit = false) : + SuperBlock(parent, name), + _kP(this, "P"), + _outputLimiter(this, "", isAngularLimit) + {}; + virtual ~BlockPLimited() {}; + float update(float input) { + float difference = 0.0f; + float output = getKP() * input; + char name[blockNameLengthMax]; + getName(name, blockNameLengthMax); + warnx("%s output %.2f _kP.get() %.2f, input", + name,(double)output, (double)_kP.get(), (double)input); + getOutputLimiter().limit(output, difference); + warnx("%s output limited %.2f", + name,(double)output); + return output; + } +// accessors + BlockOutputLimiter &getOutputLimiter() { return _outputLimiter; }; + float getKP() { return _kP.get(); } +private: + control::BlockParamFloat _kP; + BlockOutputLimiter _outputLimiter; +}; + +} + diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c new file mode 100644 index 0000000000..76ed953033 --- /dev/null +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c @@ -0,0 +1,231 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Thomas Gubler + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + +/** + * @file mTecs_params.c + * + * @author Thomas Gubler + */ + +#include +#include + +/* + * Controller parameters, accessible via MAVLink + */ + +/** + * mTECS enabled + * + * Set to 1 to enable mTECS + * + * @min 0 + * @max 1 + * @group mTECS + */ +PARAM_DEFINE_INT32(MT_ENABLED, 0); + +/** + * Total Energy Rate Control FF + * + * @min 0.0 + * @max 10.0 + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_THR_FF, 1.0f); + +/** + * Total Energy Rate Control P + * + * @min 0.0 + * @max 10.0 + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_THR_P, 0.1f); + +/** + * Total Energy Rate Control I + * + * @min 0.0 + * @max 10.0 + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_THR_I, 0.0f); + +/** + * Total Energy Rate Control OFF (Cruise throttle) + * + * @min 0.0 + * @max 10.0 + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_THR_OFF, 0.7f); + +/** + * Energy Distribution Rate Control FF + * + * @min 0.0 + * @max 10.0 + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_PIT_FF, 1.0f); + +/** + * Energy Distribution Rate Control P + * + * @min 0.0 + * @max 10.0 + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_PIT_P, 0.1f); + +/** + * Energy Distribution Rate Control I + * + * @min 0.0 + * @max 10.0 + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_PIT_I, 0.0f); + + +/** + * Total Energy Distribution OFF (Cruise pitch sp) + * + * @min 0.0 + * @max 10.0 + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_PIT_OFF, 0.0f); + +/** + * Minimal Throttle Setpoint + * + * @min 0.0 + * @max 1.0 + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_THR_MIN, 0.0f); + +/** + * Maximal Throttle Setpoint + * + * @min 0.0 + * @max 1.0 + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_THR_MAX, 1.0f); + +/** + * Minimal Pitch Setpoint in Degrees + * + * @min -90.0f + * @max 90.0f + * @unit deg + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_PIT_MIN, -45.0f); + +/** + * Maximal Pitch Setpoint in Degrees + * + * @min -90.0f + * @max 90.0f + * @unit deg + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_PIT_MAX, 45.0f); + +/** + * P gain for the altitude control + * + * @min 0.0f + * @max 10.0f + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_FPA_P, 0.1f); + +/** + * Minimal flight path angle setpoint + * + * @min -90.0f + * @max 90.0f + * @unit deg + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_FPA_MIN, -30.0f); + +/** + * Maximal flight path angle setpoint + * + * @min -90.0f + * @max 90.0f + * @unit deg + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_FPA_MAX, 30.0f); + + +/** + * P gain for the airspeed control + * + * @min 0.0f + * @max 10.0f + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_ACC_P, 0.1f); + +/** + * Minimal acceleration (air) + * + * @unit m/s^2 + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_ACC_MIN, 0.0f); + +/** + * Maximal acceleration (air) + * +* @unit m/s^2 + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_ACC_MAX, 40.0f); + +/** + * Airspeed derivative lowpass + * + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_AD_LP, 1.0f); From 6c9a40b714bb311e2841289c9449f80effdf14ce Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Mar 2014 14:34:15 +0100 Subject: [PATCH 262/381] Fixed the HIL actuator range to what it should be --- ROMFS/px4fmu_common/init.d/rc.usb | 3 ++- src/modules/mavlink/mavlink_messages.cpp | 4 ++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index 2aeea5cbe7..9f80219b1d 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -10,6 +10,7 @@ mavlink start -r 10000 -d /dev/ttyACM0 mavlink stream -d /dev/ttyACM0 -s NAMED_VALUE_FLOAT -r 10 mavlink stream -d /dev/ttyACM0 -s OPTICAL_FLOW -r 10 mavlink stream -d /dev/ttyACM0 -s ATTITUDE_CONTROLS -r 30 +mavlink stream -d /dev/ttyACM0 -s SERVO_OUTPUT_RAW_0 -r 10 # Exit shell to make it available to MAVLink -exit +exit diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 037999af75..96852bb0ea 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -811,8 +811,8 @@ protected: for (unsigned i = 0; i < 8; i++) { if (i < n) { if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { - /* scale fake PWM out 900..1200 us to 0..1*/ - out[i] = (act->output[i] - 900.0f) / 1200.0f; + /* scale fake PWM out 900..2100 us to -1..1*/ + out[i] = (act->output[i] - 1500.0f) / 600.0f; } else { /* send 0 when disarmed */ From 42bba678939a58e59282d4c63dbdef53f962e0ed Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Mar 2014 16:56:07 +0100 Subject: [PATCH 263/381] Move changes regarding the filtered airspeed consistently across sensors, use actual air temperature instead of board temperature --- src/drivers/ets_airspeed/ets_airspeed.cpp | 5 ++++- src/modules/sensors/sensors.cpp | 6 +++++- src/modules/uORB/topics/differential_pressure.h | 2 +- 3 files changed, 10 insertions(+), 3 deletions(-) diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index d27ab9727a..d873a1132e 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -176,11 +176,14 @@ ETSAirspeed::collect() _max_differential_pressure_pa = diff_pres_pa; } - // XXX we may want to smooth out the readings to remove noise. 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.temperature = -1000.0f; report.voltage = 0; report.max_differential_pressure_pa = _max_differential_pressure_pa; diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 91a8d5670f..13afe27c8b 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1031,10 +1031,12 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) raw.differential_pressure_timestamp = _diff_pres.timestamp; raw.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa; + float air_temperature_celcius = (_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, raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG); + raw.baro_pres_mbar * 1e2f, air_temperature_celcius); /* announce the airspeed if needed, just publish else */ if (_airspeed_pub > 0) { @@ -1239,6 +1241,8 @@ Sensors::adc_poll(struct sensor_combined_s &raw) _diff_pres.timestamp = t; _diff_pres.differential_pressure_pa = diff_pres_pa; + _diff_pres.differential_pressure_filtered_pa = diff_pres_pa; + _diff_pres.temperature = -1000.0f; _diff_pres.voltage = voltage; /* 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 3592c023c5..ff88b04c6e 100644 --- a/src/modules/uORB/topics/differential_pressure.h +++ b/src/modules/uORB/topics/differential_pressure.h @@ -58,7 +58,7 @@ struct differential_pressure_s { float differential_pressure_filtered_pa; /**< Low pass filtered differential pressure reading */ float max_differential_pressure_pa; /**< Maximum differential pressure reading */ float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */ - float temperature; /**< Temperature provided by sensor */ + float temperature; /**< Temperature provided by sensor, -1000.0f if unknown */ }; From 5113b4a365216987ca2f95b986538f3d3de8734f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Mar 2014 16:56:46 +0100 Subject: [PATCH 264/381] mavlink: Hotfixed HIL battery status publication --- 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 489d2bdcb7..d7e300670a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -634,13 +634,15 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) hil_battery_status.timestamp = timestamp; hil_battery_status.voltage_v = 11.1f; + hil_battery_status.voltage_filtered_v = 11.1f; hil_battery_status.current_a = 10.0f; + hil_battery_status.discharged_mah = -1.0f; if (_battery_pub > 0) { orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); } else { - _baro_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); + _battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); } } From 57fdb40a4efb943b0b14593b314ea2f887215d68 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Mar 2014 16:56:46 +0100 Subject: [PATCH 265/381] mavlink: Hotfixed HIL battery status publication --- 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 489d2bdcb7..d7e300670a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -634,13 +634,15 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) hil_battery_status.timestamp = timestamp; hil_battery_status.voltage_v = 11.1f; + hil_battery_status.voltage_filtered_v = 11.1f; hil_battery_status.current_a = 10.0f; + hil_battery_status.discharged_mah = -1.0f; if (_battery_pub > 0) { orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); } else { - _baro_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); + _battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); } } From 5693a9d3b6e923b2a2c05594f10cb5366ee6c495 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Mar 2014 18:28:13 +0100 Subject: [PATCH 266/381] new fixed wing estimator: Fix the symmetry force step of the covariance prediction. --- src/modules/fw_att_pos_estimator/estimator.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index 36b9f4491b..e6f1fee874 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -876,7 +876,7 @@ void CovariancePrediction(float dt) nextP[20][19] = P[20][19]; nextP[20][20] = P[20][20]; - for (uint8_t i=0; i<= 20; i++) + for (unsigned i = 0; i < n_states; i++) { nextP[i][i] = nextP[i][i] + processNoise[i]; } @@ -942,7 +942,16 @@ void CovariancePrediction(float dt) P[i][i] = nextP[i][i]; } - ForceSymmetry(); + // force symmetry for observable states + for (unsigned i = 1; i < n_states; i++) + { + for (uint8_t j = 0; j < i; j++) + { + P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]); + P[j][i] = P[i][j]; + } + } + } ConstrainVariances(); From 01f33df70718b7a7dba342fb5920d91b3cd83c09 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 24 Mar 2014 09:04:35 +0100 Subject: [PATCH 267/381] Added EKF filter health status reporting, added dynamic in-air reset. --- .../fw_att_pos_estimator/estimator.cpp | 209 +++++++++++++----- src/modules/fw_att_pos_estimator/estimator.h | 29 ++- .../fw_att_pos_estimator_main.cpp | 65 +++++- src/modules/uORB/objects_common.cpp | 3 + src/modules/uORB/topics/estimator_status.h | 81 +++++++ 5 files changed, 328 insertions(+), 59 deletions(-) create mode 100644 src/modules/uORB/topics/estimator_status.h diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index e6f1fee874..bde46ba036 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -74,12 +74,8 @@ bool staticMode = true; ///< boolean true if no position feedback is fused bool useAirspeed = true; ///< boolean true if airspeed data is being used bool useCompass = true; ///< boolean true if magnetometer data is being used -bool velHealth; -bool posHealth; -bool hgtHealth; -bool velTimeout; -bool posTimeout; -bool hgtTimeout; +struct ekf_status_report current_ekf_state; +struct ekf_status_report last_ekf_error; bool numericalProtection = true; @@ -965,9 +961,6 @@ void FuseVelposNED() uint32_t gpsRetryTimeNoTAS = 5000; // retry time if no TAS measurement available uint32_t hgtRetryTime = 5000; // height measurement retry time uint32_t horizRetryTime; - static uint32_t velFailTime = 0; - static uint32_t posFailTime = 0; - static uint32_t hgtFailTime = 0; // declare variables used to check measurement errors float velInnov[3] = {0.0f,0.0f,0.0f}; @@ -1033,16 +1026,16 @@ void FuseVelposNED() varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS[i]; } // apply a 5-sigma threshold - velHealth = (sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) < 25.0*(varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2]); - velTimeout = (millis() - velFailTime) > horizRetryTime; - if (velHealth || velTimeout) + current_ekf_state.velHealth = (sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) < 25.0*(varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2]); + current_ekf_state.velTimeout = (millis() - current_ekf_state.velFailTime) > horizRetryTime; + if (current_ekf_state.velHealth || current_ekf_state.velTimeout) { - velHealth = true; - velFailTime = millis(); + current_ekf_state.velHealth = true; + current_ekf_state.velFailTime = millis(); } else { - velHealth = false; + current_ekf_state.velHealth = false; } } if (fusePosData) @@ -1053,16 +1046,16 @@ void FuseVelposNED() varInnovVelPos[3] = P[7][7] + R_OBS[3]; varInnovVelPos[4] = P[8][8] + R_OBS[4]; // apply a 10-sigma threshold - posHealth = (sq(posInnov[0]) + sq(posInnov[1])) < 100.0f*(varInnovVelPos[3] + varInnovVelPos[4]); - posTimeout = (millis() - posFailTime) > horizRetryTime; - if (posHealth || posTimeout) + current_ekf_state.posHealth = (sq(posInnov[0]) + sq(posInnov[1])) < 100.0f*(varInnovVelPos[3] + varInnovVelPos[4]); + current_ekf_state.posTimeout = (millis() - current_ekf_state.posFailTime) > horizRetryTime; + if (current_ekf_state.posHealth || current_ekf_state.posTimeout) { - posHealth = true; - posFailTime = millis(); + current_ekf_state.posHealth = true; + current_ekf_state.posFailTime = millis(); } else { - posHealth = false; + current_ekf_state.posHealth = false; } } // test height measurements @@ -1071,37 +1064,37 @@ void FuseVelposNED() hgtInnov = statesAtHgtTime[9] + hgtMea; varInnovVelPos[5] = P[9][9] + R_OBS[5]; // apply a 10-sigma threshold - hgtHealth = sq(hgtInnov) < 100.0f*varInnovVelPos[5]; - hgtTimeout = (millis() - hgtFailTime) > hgtRetryTime; - if (hgtHealth || hgtTimeout) + current_ekf_state.hgtHealth = sq(hgtInnov) < 100.0f*varInnovVelPos[5]; + current_ekf_state.hgtTimeout = (millis() - current_ekf_state.hgtFailTime) > hgtRetryTime; + if (current_ekf_state.hgtHealth || current_ekf_state.hgtTimeout) { - hgtHealth = true; - hgtFailTime = millis(); + current_ekf_state.hgtHealth = true; + current_ekf_state.hgtFailTime = millis(); } else { - hgtHealth = false; + current_ekf_state.hgtHealth = false; } } // Set range for sequential fusion of velocity and position measurements depending // on which data is available and its health - if (fuseVelData && fusionModeGPS == 0 && velHealth) + if (fuseVelData && fusionModeGPS == 0 && current_ekf_state.velHealth) { fuseData[0] = true; fuseData[1] = true; fuseData[2] = true; } - if (fuseVelData && fusionModeGPS == 1 && velHealth) + if (fuseVelData && fusionModeGPS == 1 && current_ekf_state.velHealth) { fuseData[0] = true; fuseData[1] = true; } - if (fusePosData && fusionModeGPS <= 2 && posHealth) + if (fusePosData && fusionModeGPS <= 2 && current_ekf_state.posHealth) { fuseData[3] = true; fuseData[4] = true; } - if (fuseHgtData && hgtHealth) + if (fuseHgtData && current_ekf_state.hgtHealth) { fuseData[5] = true; } @@ -1986,7 +1979,7 @@ bool FilterHealthy() // XXX Check state vector for NaNs and ill-conditioning // Check if any of the major inputs timed out - if (posTimeout || velTimeout || hgtTimeout) { + if (current_ekf_state.posTimeout || current_ekf_state.velTimeout || current_ekf_state.hgtTimeout) { return false; } @@ -2042,6 +2035,65 @@ void ResetVelocity(void) } +void FillErrorReport(struct ekf_status_report *err) +{ + for (int i = 0; i < n_states; i++) + { + err->states[i] = states[i]; + } + + err->velHealth = current_ekf_state.velHealth; + err->posHealth = current_ekf_state.posHealth; + err->hgtHealth = current_ekf_state.hgtHealth; + err->velTimeout = current_ekf_state.velTimeout; + err->posTimeout = current_ekf_state.posTimeout; + err->hgtTimeout = current_ekf_state.hgtTimeout; +} + +bool StatesNaN(struct ekf_status_report *err_report) { + bool err = false; + + // check all states and covariance matrices + for (unsigned i = 0; i < n_states; i++) { + for (unsigned j = 0; j < n_states; j++) { + if (!isfinite(KH[i][j])) { + + err_report->covarianceNaN = true; + err = true; + } // intermediate result used for covariance updates + if (!isfinite(KHP[i][j])) { + + err_report->covarianceNaN = true; + err = true; + } // intermediate result used for covariance updates + if (!isfinite(P[i][j])) { + + err_report->covarianceNaN = true; + err = true; + } // covariance matrix + } + + if (!isfinite(Kfusion[i])) { + + err_report->kalmanGainsNaN = true; + err = true; + } // Kalman gains + + if (!isfinite(states[i])) { + + err_report->statesNaN = true; + err = true; + } // state matrix + } + + if (err) { + FillErrorReport(err_report); + } + + return err; + +} + /** * Check the filter inputs and bound its operational state * @@ -2051,21 +2103,30 @@ void ResetVelocity(void) * updated, but before any of the fusion steps are * executed. */ -void CheckAndBound() +int CheckAndBound() { // Store the old filter state bool currStaticMode = staticMode; + // Reset the filter if the states went NaN + if (StatesNaN(&last_ekf_error)) { + + InitializeDynamic(velNED); + + return 1; + } + // Reset the filter if the IMU data is too old if (dtIMU > 0.2f) { + ResetVelocity(); ResetPosition(); ResetHeight(); ResetStoredStates(); // that's all we can do here, return - return; + return 2; } // Check if we're on ground - this also sets static mode. @@ -2077,7 +2138,11 @@ void CheckAndBound() ResetPosition(); ResetHeight(); ResetStoredStates(); + + return 3; } + + return 0; } void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat) @@ -2116,28 +2181,13 @@ void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, fl initQuat[3] = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading; } -void InitialiseFilter(float (&initvelNED)[3]) +void InitializeDynamic(float (&initvelNED)[3]) { - // Do the data structure init - for (unsigned i = 0; i < n_states; i++) { - for (unsigned j = 0; j < n_states; j++) { - KH[i][j] = 0.0f; // intermediate result used for covariance updates - KHP[i][j] = 0.0f; // intermediate result used for covariance updates - P[i][j] = 0.0f; // covariance matrix - } - Kfusion[i] = 0.0f; // Kalman gains - states[i] = 0.0f; // state matrix - } + // Clear the init flag + statesInitialised = false; - for (unsigned i = 0; i < data_buffer_size; i++) { - - for (unsigned j = 0; j < n_states; j++) { - storedStates[j][i] = 0.0f; - } - - statetimeStamp[i] = 0; - } + ZeroVariables(); // Calculate initial filter quaternion states from raw measurements float initQuat[4]; @@ -2155,10 +2205,7 @@ void InitialiseFilter(float (&initvelNED)[3]) initMagNED.y = DCM.y.x*initMagXYZ.x + DCM.y.y*initMagXYZ.y + DCM.y.z*initMagXYZ.z; initMagNED.z = DCM.z.x*initMagXYZ.x + DCM.z.y*initMagXYZ.y + DCM.z.z*initMagXYZ.z; - //store initial lat,long and height - latRef = gpsLat; - lonRef = gpsLon; - hgtRef = gpsHgt; + // write to state vector for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions @@ -2187,3 +2234,51 @@ void InitialiseFilter(float (&initvelNED)[3]) summedDelVel.y = 0.0f; summedDelVel.z = 0.0f; } + +void InitialiseFilter(float (&initvelNED)[3]) +{ + //store initial lat,long and height + latRef = gpsLat; + lonRef = gpsLon; + hgtRef = gpsHgt; + + memset(&last_ekf_error, 0, sizeof(last_ekf_error)); + + InitializeDynamic(initvelNED); +} + +void ZeroVariables() +{ + // Do the data structure init + for (unsigned i = 0; i < n_states; i++) { + for (unsigned j = 0; j < n_states; j++) { + KH[i][j] = 0.0f; // intermediate result used for covariance updates + KHP[i][j] = 0.0f; // intermediate result used for covariance updates + P[i][j] = 0.0f; // covariance matrix + } + + Kfusion[i] = 0.0f; // Kalman gains + states[i] = 0.0f; // state matrix + } + + for (unsigned i = 0; i < data_buffer_size; i++) { + + for (unsigned j = 0; j < n_states; j++) { + storedStates[j][i] = 0.0f; + } + + statetimeStamp[i] = 0; + } + + memset(¤t_ekf_state, 0, sizeof(current_ekf_state)); +} + +void GetFilterState(struct ekf_status_report *state) +{ + memcpy(state, ¤t_ekf_state, sizeof(state)); +} + +void GetLastErrorState(struct ekf_status_report *last_error) +{ + memcpy(last_error, &last_ekf_error, sizeof(last_error)); +} diff --git a/src/modules/fw_att_pos_estimator/estimator.h b/src/modules/fw_att_pos_estimator/estimator.h index 256aff7716..55e6eb12e0 100644 --- a/src/modules/fw_att_pos_estimator/estimator.h +++ b/src/modules/fw_att_pos_estimator/estimator.h @@ -134,6 +134,22 @@ enum GPS_FIX { GPS_FIX_3D = 3 }; +struct ekf_status_report { + bool velHealth; + bool posHealth; + bool hgtHealth; + bool velTimeout; + bool posTimeout; + bool hgtTimeout; + uint32_t velFailTime; + uint32_t posFailTime; + uint32_t hgtFailTime; + float states[n_states]; + bool statesNaN; + bool covarianceNaN; + bool kalmanGainsNaN; +}; + void UpdateStrapdownEquationsNED(); void CovariancePrediction(float dt); @@ -188,11 +204,22 @@ void ConstrainStates(); void ForceSymmetry(); -void CheckAndBound(); +int CheckAndBound(); void ResetPosition(); void ResetVelocity(); +void ZeroVariables(); + +void GetFilterState(struct ekf_status_report *state); + +void GetLastErrorState(struct ekf_status_report *last_error); + +bool StatesNaN(struct ekf_status_report *err_report); +void FillErrorReport(struct ekf_status_report *err); + +void InitializeDynamic(float (&initvelNED)[3]); + uint32_t millis(); diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index e46dffde3c..e75b844fd3 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -71,6 +71,7 @@ #include #include #include +#include #include #include #include @@ -144,6 +145,7 @@ private: orb_advert_t _att_pub; /**< vehicle attitude */ orb_advert_t _global_pos_pub; /**< global position */ orb_advert_t _local_pos_pub; /**< position in local frame */ + orb_advert_t _estimator_status_pub; /**< status of the estimator */ struct vehicle_attitude_s _att; /**< vehicle attitude */ struct gyro_report _gyro; @@ -260,6 +262,7 @@ FixedwingEstimator::FixedwingEstimator() : _att_pub(-1), _global_pos_pub(-1), _local_pos_pub(-1), + _estimator_status_pub(-1), _baro_ref(0.0f), _baro_gps_offset(0.0f), @@ -701,7 +704,67 @@ FixedwingEstimator::task_main() /** * CHECK IF THE INPUT DATA IS SANE */ - CheckAndBound(); + int check = CheckAndBound(); + + switch (check) { + case 0: + /* all ok */ + break; + case 1: + { + const char* str = "NaN in states, resetting"; + warnx(str); + mavlink_log_critical(_mavlink_fd, str); + break; + } + case 2: + { + const char* str = "stale IMU data, resetting"; + warnx(str); + mavlink_log_critical(_mavlink_fd, str); + break; + } + case 3: + { + const char* str = "switching dynamic / static state"; + warnx(str); + mavlink_log_critical(_mavlink_fd, str); + break; + } + } + + // If non-zero, we got a problem + if (check) { + + struct ekf_status_report ekf_report; + + GetLastErrorState(&ekf_report); + + struct estimator_status_report rep; + memset(&rep, 0, sizeof(rep)); + rep.timestamp = hrt_absolute_time(); + + rep.states_nan = ekf_report.statesNaN; + rep.covariance_nan = ekf_report.covarianceNaN; + rep.kalman_gain_nan = ekf_report.kalmanGainsNaN; + + // Copy all states or at least all that we can fit + int i = 0; + unsigned ekf_n_states = (sizeof(ekf_report.states) / sizeof(ekf_report.states[0])); + unsigned max_states = (sizeof(rep.states) / sizeof(rep.states[0])); + rep.n_states = (ekf_n_states < max_states) ? ekf_n_states : max_states; + + while ((i < ekf_n_states) && (i < max_states)) { + + rep.states[i] = ekf_report.states[i]; + } + + if (_estimator_status_pub > 0) { + orb_publish(ORB_ID(estimator_status), _estimator_status_pub, &rep); + } else { + _estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &rep); + } + } /** diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index fb24de8d16..4b31cc8a49 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -193,3 +193,6 @@ ORB_DEFINE(esc_status, struct esc_status_s); #include "topics/encoders.h" ORB_DEFINE(encoders, struct encoders_s); + +#include "topics/estimator_status.h" +ORB_DEFINE(estimator_status, struct estimator_status_report); diff --git a/src/modules/uORB/topics/estimator_status.h b/src/modules/uORB/topics/estimator_status.h new file mode 100644 index 0000000000..66ca030199 --- /dev/null +++ b/src/modules/uORB/topics/estimator_status.h @@ -0,0 +1,81 @@ +/**************************************************************************** + * + * 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 estimator_status.h + * Definition of the estimator_status_report uORB topic. + * + * @author Lorenz Meier + */ + +#ifndef ESTIMATOR_STATUS_H_ +#define ESTIMATOR_STATUS_H_ + +#include +#include +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +/** + * Estimator status report. + * + * This is a generic status report struct which allows any of the onboard estimators + * to write the internal state to the system log. + * + */ +struct estimator_status_report { + + /* NOTE: Ordering of fields optimized to align to 32 bit / 4 bytes - change with consideration only */ + + uint64_t timestamp; /**< Timestamp in microseconds since boot */ + float states[32]; /**< Internal filter states */ + float n_states; /**< Number of states effectively used */ + bool states_nan; /**< If set to true, one of the states is NaN */ + bool covariance_nan; /**< If set to true, the covariance matrix went NaN */ + bool kalman_gain_nan; /**< If set to true, the Kalman gain matrix went NaN */ + bool states_out_of_bounds; /**< If set to true, one of the states is out of bounds */ + +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(estimator_status); + +#endif From d64c861ef84c6f1539dbb7cb1e5a5fd11e9f2656 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 24 Mar 2014 09:16:30 +0100 Subject: [PATCH 268/381] fixed wing estimator: Added trip command to test filter robustness --- .../fw_att_pos_estimator_main.cpp | 61 +++++++++++++++++++ 1 file changed, 61 insertions(+) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index e75b844fd3..1d57f705a8 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -72,6 +72,7 @@ #include #include #include +#include #include #include #include @@ -1146,6 +1147,55 @@ void print_status() (staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE"); } +int trip_nan() { + + int ret = 0; + + // If system is not armed, inject a NaN value into the filter + int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + + struct actuator_armed_s armed; + orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); + + if (armed.armed) { + warnx("ACTUATORS ARMED! NOT TRIPPING SYSTEM"); + ret = 1; + } else { + + float nan_val = 0.0f / 0.0f; + + warnx("system not armed, tripping state vector with NaN values"); + states[5] = nan_val; + usleep(100000); + + // warnx("tripping covariance #1 with NaN values"); + // KH[2][2] = nan_val; // intermediate result used for covariance updates + // usleep(100000); + + // warnx("tripping covariance #2 with NaN values"); + // KHP[5][5] = nan_val; // intermediate result used for covariance updates + // usleep(100000); + + warnx("tripping covariance #3 with NaN values"); + P[3][3] = nan_val; // covariance matrix + usleep(100000); + + warnx("tripping Kalman gains with NaN values"); + Kfusion[0] = nan_val; // Kalman gains + usleep(100000); + + warnx("tripping stored states[0] with NaN values"); + storedStates[0][0] = nan_val; + usleep(100000); + + warnx("\nDONE - FILTER STATE:"); + print_status(); + } + + close(armed_sub); + return ret; +} + int fw_att_pos_estimator_main(int argc, char *argv[]) { if (argc < 1) @@ -1192,6 +1242,17 @@ int fw_att_pos_estimator_main(int argc, char *argv[]) } } + if (!strcmp(argv[1], "trip")) { + if (estimator::g_estimator) { + int ret = trip_nan(); + + exit(ret); + + } else { + errx(1, "not running"); + } + } + warnx("unrecognized command"); return 1; } From 0874507a44347d10e10606061c411f0b0129dc5b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 24 Mar 2014 09:35:47 +0100 Subject: [PATCH 269/381] Added estimator status logging to sdlog2 --- src/modules/sdlog2/sdlog2.c | 18 ++++++++++++++++++ src/modules/sdlog2/sdlog2_messages.h | 11 +++++++++++ src/modules/uORB/topics/estimator_status.h | 1 - 3 files changed, 29 insertions(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index ad709d27dd..3f07eea533 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -83,6 +83,7 @@ #include #include #include +#include #include #include @@ -794,6 +795,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct battery_status_s battery; struct telemetry_status_s telemetry; struct range_finder_report range_finder; + struct estimator_status_report estimator_status; } buf; memset(&buf, 0, sizeof(buf)); @@ -825,6 +827,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_BATT_s log_BATT; struct log_DIST_s log_DIST; struct log_TELE_s log_TELE; + struct log_ESTM_s log_ESTM; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -855,6 +858,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int battery_sub; int telemetry_sub; int range_finder_sub; + int estimator_status_sub; } subs; subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); @@ -879,6 +883,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.battery_sub = orb_subscribe(ORB_ID(battery_status)); subs.telemetry_sub = orb_subscribe(ORB_ID(telemetry_status)); subs.range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder)); + subs.estimator_status_sub = orb_subscribe(ORB_ID(estimator_status)); thread_running = true; @@ -1243,6 +1248,19 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(DIST); } + /* --- ESTIMATOR STATUS --- */ + if (copy_if_updated(ORB_ID(estimator_status), subs.estimator_status_sub, &buf.estimator_status)) { + log_msg.msg_type = LOG_ESTM_MSG; + unsigned maxcopy = (sizeof(buf.estimator_status.states) < sizeof(log_msg.body.log_ESTM.s)) ? sizeof(buf.estimator_status.states) : sizeof(log_msg.body.log_ESTM.s); + memset(&(log_msg.body.log_ESTM.s), 0, sizeof(log_msg.body.log_ESTM.s)); + memcpy(&(log_msg.body.log_ESTM.s), buf.estimator_status.states, maxcopy); + log_msg.body.log_ESTM.n_states = buf.estimator_status.n_states; + log_msg.body.log_ESTM.states_nan = buf.estimator_status.states_nan; + log_msg.body.log_ESTM.covariance_nan = buf.estimator_status.covariance_nan; + log_msg.body.log_ESTM.kalman_gain_nan = buf.estimator_status.kalman_gain_nan; + LOGBUFFER_WRITE_AND_COUNT(DIST); + } + /* 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 fe500ad5f5..2b41755de9 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -299,6 +299,16 @@ struct log_PARM_s { float value; }; +/* --- ESTM - ESTIMATOR STATUS --- */ +#define LOG_ESTM_MSG 132 +struct log_ESTM_s { + float s[32]; + uint8_t n_states; + uint8_t states_nan; + uint8_t covariance_nan; + uint8_t kalman_gain_nan; +}; + #pragma pack(pop) /* construct list of all message formats */ @@ -331,6 +341,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(TIME, "Q", "StartTime"), LOG_FORMAT(VER, "NZ", "Arch,FwGit"), LOG_FORMAT(PARM, "Nf", "Name,Value"), + LOG_FORMAT(ESTM, "ffffffffffffffffffffffffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27,s28,s29,s30,s31,n_states,states_nan,cov_nan,kgain_nan"), }; static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s); diff --git a/src/modules/uORB/topics/estimator_status.h b/src/modules/uORB/topics/estimator_status.h index 66ca030199..5530cdb210 100644 --- a/src/modules/uORB/topics/estimator_status.h +++ b/src/modules/uORB/topics/estimator_status.h @@ -67,7 +67,6 @@ struct estimator_status_report { bool states_nan; /**< If set to true, one of the states is NaN */ bool covariance_nan; /**< If set to true, the covariance matrix went NaN */ bool kalman_gain_nan; /**< If set to true, the Kalman gain matrix went NaN */ - bool states_out_of_bounds; /**< If set to true, one of the states is out of bounds */ }; From 29abf6db39dbf337d27c7dd85669934971444fef Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 24 Mar 2014 10:13:22 +0100 Subject: [PATCH 270/381] Fixed missing increment across states --- src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 1d57f705a8..c9d75bce49 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -758,6 +758,7 @@ FixedwingEstimator::task_main() while ((i < ekf_n_states) && (i < max_states)) { rep.states[i] = ekf_report.states[i]; + i++; } if (_estimator_status_pub > 0) { From 8666ca53bf5b8eebbf60908c16273b711f80a19e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 24 Mar 2014 10:13:56 +0100 Subject: [PATCH 271/381] Reducing VFR and HUD update rates, we do not need 100 Hz for 30 Hz human vision --- ROMFS/px4fmu_common/init.d/rc.usb | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index 9f80219b1d..b7b5569453 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -9,6 +9,8 @@ mavlink start -r 10000 -d /dev/ttyACM0 # Enable a number of interesting streams we want via USB mavlink stream -d /dev/ttyACM0 -s NAMED_VALUE_FLOAT -r 10 mavlink stream -d /dev/ttyACM0 -s OPTICAL_FLOW -r 10 +mavlink stream -d /dev/ttyACM0 -s VFR_HUD -r 20 +mavlink stream -d /dev/ttyACM0 -s ATTITUDE -r 20 mavlink stream -d /dev/ttyACM0 -s ATTITUDE_CONTROLS -r 30 mavlink stream -d /dev/ttyACM0 -s SERVO_OUTPUT_RAW_0 -r 10 From 0022bbb5fbb0cd7237c9ce8b0006ec4ac0e14066 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 24 Mar 2014 10:29:13 +0100 Subject: [PATCH 272/381] Guard against invalid states --- .../fw_att_pos_estimator/estimator.cpp | 67 ++++++++++++------- src/modules/fw_att_pos_estimator/estimator.h | 14 +++- 2 files changed, 53 insertions(+), 28 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index bde46ba036..f03aa94741 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -1674,34 +1674,49 @@ void ResetStoredStates() } // Output the state vector stored at the time that best matches that specified by msec -void RecallStates(float statesForFusion[n_states], uint64_t msec) +int RecallStates(float statesForFusion[n_states], uint64_t msec) { - // int64_t bestTimeDelta = 200; - // unsigned bestStoreIndex = 0; - // for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++) - // { - // // The time delta can also end up as negative number, - // // since we might compare future to past or past to future - // // therefore cast to int64. - // int64_t timeDelta = (int64_t)msec - (int64_t)statetimeStamp[storeIndex]; - // if (timeDelta < 0) { - // timeDelta = -timeDelta; - // } + int ret = 0; - // if (timeDelta < bestTimeDelta) - // { - // bestStoreIndex = storeIndex; - // bestTimeDelta = timeDelta; - // } - // } - // if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error - // { - // for (uint8_t i=0; i < n_states; i++) statesForFusion[i] = storedStates[i][bestStoreIndex]; - // } - // else // otherwise output current state - // { - for (uint8_t i=0; i < n_states; i++) statesForFusion[i] = states[i]; - // } + int64_t bestTimeDelta = 200; + unsigned bestStoreIndex = 0; + for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++) + { + // The time delta can also end up as negative number, + // since we might compare future to past or past to future + // therefore cast to int64. + int64_t timeDelta = (int64_t)msec - (int64_t)statetimeStamp[storeIndex]; + if (timeDelta < 0) { + timeDelta = -timeDelta; + } + + if (timeDelta < bestTimeDelta) + { + bestStoreIndex = storeIndex; + bestTimeDelta = timeDelta; + } + } + if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error + { + for (uint8_t i=0; i < n_states; i++) { + if (isfinite(storedStates[i][bestStoreIndex])) { + statesForFusion[i] = storedStates[i][bestStoreIndex]; + } else if (isfinite(states[i])) { + statesForFusion[i] = states[i]; + } else { + // There is not much we can do here, except reporting the error we just + // found. + ret++; + } + } + else // otherwise output current state + { + for (uint8_t i=0; i < n_states; i++) { + statesForFusion[i] = states[i]; + } + } + + return ret; } void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]) diff --git a/src/modules/fw_att_pos_estimator/estimator.h b/src/modules/fw_att_pos_estimator/estimator.h index 55e6eb12e0..c5a5e9d8d9 100644 --- a/src/modules/fw_att_pos_estimator/estimator.h +++ b/src/modules/fw_att_pos_estimator/estimator.h @@ -171,8 +171,18 @@ void quatNorm(float (&quatOut)[4], const float quatIn[4]); // store staes along with system time stamp in msces void StoreStates(uint64_t timestamp_ms); -// recall stste vector stored at closest time to the one specified by msec -void RecallStates(float statesForFusion[n_states], uint64_t msec); +/** + * Recall the state vector. + * + * Recalls the vector stored at closest time to the one specified by msec + * + * @return zero on success, integer indicating the number of invalid states on failure. + * Does only copy valid states, if the statesForFusion vector was initialized + * correctly by the caller, the result can be safely used, but is a mixture + * time-wise where valid states were updated and invalid remained at the old + * value. + */ +int RecallStates(float statesForFusion[n_states], uint64_t msec); void ResetStoredStates(); From 60728bb6a6211407a1db12e7c015272cf998d928 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 24 Mar 2014 10:31:16 +0100 Subject: [PATCH 273/381] Now that the guard is updated disable time compensation again, but keep a guard against invalid state updates --- .../fw_att_pos_estimator/estimator.cpp | 64 ++++++++++--------- 1 file changed, 34 insertions(+), 30 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index f03aa94741..46e405526e 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -1678,43 +1678,47 @@ int RecallStates(float statesForFusion[n_states], uint64_t msec) { int ret = 0; - int64_t bestTimeDelta = 200; - unsigned bestStoreIndex = 0; - for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++) - { - // The time delta can also end up as negative number, - // since we might compare future to past or past to future - // therefore cast to int64. - int64_t timeDelta = (int64_t)msec - (int64_t)statetimeStamp[storeIndex]; - if (timeDelta < 0) { - timeDelta = -timeDelta; - } + // int64_t bestTimeDelta = 200; + // unsigned bestStoreIndex = 0; + // for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++) + // { + // // The time delta can also end up as negative number, + // // since we might compare future to past or past to future + // // therefore cast to int64. + // int64_t timeDelta = (int64_t)msec - (int64_t)statetimeStamp[storeIndex]; + // if (timeDelta < 0) { + // timeDelta = -timeDelta; + // } - if (timeDelta < bestTimeDelta) - { - bestStoreIndex = storeIndex; - bestTimeDelta = timeDelta; - } - } - if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error - { + // if (timeDelta < bestTimeDelta) + // { + // bestStoreIndex = storeIndex; + // bestTimeDelta = timeDelta; + // } + // } + // if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error + // { + // for (uint8_t i=0; i < n_states; i++) { + // if (isfinite(storedStates[i][bestStoreIndex])) { + // statesForFusion[i] = storedStates[i][bestStoreIndex]; + // } else if (isfinite(states[i])) { + // statesForFusion[i] = states[i]; + // } else { + // // There is not much we can do here, except reporting the error we just + // // found. + // ret++; + // } + // } + // else // otherwise output current state + // { for (uint8_t i=0; i < n_states; i++) { - if (isfinite(storedStates[i][bestStoreIndex])) { - statesForFusion[i] = storedStates[i][bestStoreIndex]; - } else if (isfinite(states[i])) { + if (isfinite(states[i])) { statesForFusion[i] = states[i]; } else { - // There is not much we can do here, except reporting the error we just - // found. ret++; } - } - else // otherwise output current state - { - for (uint8_t i=0; i < n_states; i++) { - statesForFusion[i] = states[i]; } - } + // } return ret; } From 2c2c4af59907a1a2dd9fb0635fd9a179f8c8860f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 25 Mar 2014 12:21:07 +0100 Subject: [PATCH 274/381] mavlink / HIL: Split handling of actuator feedback between fixed wing and multicopters --- src/modules/mavlink/mavlink_messages.cpp | 84 +++++++++++++++--------- 1 file changed, 53 insertions(+), 31 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 96852bb0ea..e292f52ba0 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -71,6 +71,7 @@ #include #include #include +#include #include "mavlink_messages.h" @@ -762,6 +763,9 @@ private: MavlinkOrbSubscription *act_sub; struct actuator_outputs_s *act; + MavlinkOrbSubscription *ctrl_sub; + struct actuator_controls_s *ctrl; + protected: void subscribe(Mavlink *mavlink) { @@ -773,6 +777,9 @@ protected: act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0)); act = (struct actuator_outputs_s *)act_sub->get_data(); + + ctrl_sub = mavlink->add_orb_subscription(ORB_ID(actuator_controls_0)); + ctrl = (struct actuator_controls_s *)ctrl_sub->get_data(); } void send(const hrt_abstime t) @@ -780,6 +787,7 @@ protected: bool updated = status_sub->update(t); updated |= pos_sp_triplet_sub->update(t); updated |= act_sub->update(t); + updated |= ctrl_sub->update(t); if (updated) { /* translate the current syste state to mavlink state and mode */ @@ -788,47 +796,61 @@ protected: uint32_t mavlink_custom_mode; get_mavlink_mode_state(status, pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); - /* set number of valid outputs depending on vehicle type */ - unsigned n; + if (mavlink_system.type == MAV_TYPE_QUADROTOR || + mavlink_system.type == MAV_TYPE_HEXAROTOR || + mavlink_system.type == MAV_TYPE_OCTOROTOR) { + /* set number of valid outputs depending on vehicle type */ + unsigned n; - switch (mavlink_system.type) { - case MAV_TYPE_QUADROTOR: - n = 4; - break; + switch (mavlink_system.type) { + case MAV_TYPE_QUADROTOR: + n = 4; + break; - case MAV_TYPE_HEXAROTOR: - n = 6; - break; + case MAV_TYPE_HEXAROTOR: + n = 6; + break; - default: - n = 8; - break; - } + default: + n = 8; + break; + } - /* scale / assign outputs depending on system type */ - float out[8]; + /* scale / assign outputs depending on system type */ + float out[8]; - for (unsigned i = 0; i < 8; i++) { - if (i < n) { - if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { - /* scale fake PWM out 900..2100 us to -1..1*/ - out[i] = (act->output[i] - 1500.0f) / 600.0f; + for (unsigned i = 0; i < 8; i++) { + if (i < n) { + if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { + /* scale fake PWM out 900..2100 us to 0..1 for normal multirotors */ + out[i] = (act->output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); + + } else { + /* send 0 when disarmed */ + out[i] = 0.0f; + } } else { - /* send 0 when disarmed */ - out[i] = 0.0f; + out[i] = -1.0f; } - - } else { - out[i] = -1.0f; } - } - mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], - mavlink_base_mode, - 0); + mavlink_msg_hil_controls_send(_channel, + hrt_absolute_time(), + out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], + mavlink_base_mode, + 0); + } else { + + /* no special handling required, just send the controls */ + + mavlink_msg_hil_controls_send(_channel, + hrt_absolute_time(), + ctrl->control[0], ctrl->control[1], ctrl->control[2], ctrl->control[3], + ctrl->control[4], ctrl->control[5], ctrl->control[6], ctrl->control[7], + mavlink_base_mode, + 0); + } } } }; From cb3a4f12670ce8d6cf608eb0ec97298a3bcb0919 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Tue, 25 Mar 2014 15:23:40 +0100 Subject: [PATCH 275/381] px4io driver: publish input_rc even if RC connection has been lost --- src/drivers/px4io/px4io.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 82f3ba044a..2c00785038 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1479,10 +1479,9 @@ PX4IO::io_publish_raw_rc() } else { rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN; - /* we do not know the RC input, only publish if RC OK flag is set */ - /* if no raw RC, just don't publish */ - if (!(_status & PX4IO_P_STATUS_FLAGS_RC_OK)) - return OK; + /* we do not know the RC input, but have to publish timestamp_published + * and rc_lost flag, so do not prematurely return here + */ } /* lazily advertise on first publication */ From 409fa565f48ffe164b7332c9186a876b2771922a Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Tue, 25 Mar 2014 15:42:44 +0100 Subject: [PATCH 276/381] px4io: do not include failsafe condition into rc_lost flag --- src/modules/px4iofirmware/controls.c | 138 +++++++++++++-------------- 1 file changed, 67 insertions(+), 71 deletions(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 941500f0d9..609dd33126 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -201,94 +201,90 @@ controls_tick() { /* update RC-received timestamp */ system_state.rc_channels_timestamp_received = hrt_absolute_time(); - /* do not command anything in failsafe, kick in the RC loss counter */ - if (!(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) { + /* update RC-received timestamp */ + system_state.rc_channels_timestamp_valid = system_state.rc_channels_timestamp_received; - /* update RC-received timestamp */ - system_state.rc_channels_timestamp_valid = system_state.rc_channels_timestamp_received; + /* map raw inputs to mapped inputs */ + /* XXX mapping should be atomic relative to protocol */ + for (unsigned i = 0; i < r_raw_rc_count; i++) { - /* map raw inputs to mapped inputs */ - /* XXX mapping should be atomic relative to protocol */ - for (unsigned i = 0; i < r_raw_rc_count; i++) { + /* map the input channel */ + uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE]; - /* map the input channel */ - uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE]; + if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { - if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { + uint16_t raw = r_raw_rc_values[i]; - uint16_t raw = r_raw_rc_values[i]; + int16_t scaled; - int16_t scaled; + /* + * 1) Constrain to min/max values, as later processing depends on bounds. + */ + if (raw < conf[PX4IO_P_RC_CONFIG_MIN]) + raw = conf[PX4IO_P_RC_CONFIG_MIN]; + if (raw > conf[PX4IO_P_RC_CONFIG_MAX]) + raw = conf[PX4IO_P_RC_CONFIG_MAX]; - /* - * 1) Constrain to min/max values, as later processing depends on bounds. - */ - if (raw < conf[PX4IO_P_RC_CONFIG_MIN]) - raw = conf[PX4IO_P_RC_CONFIG_MIN]; - if (raw > conf[PX4IO_P_RC_CONFIG_MAX]) - raw = conf[PX4IO_P_RC_CONFIG_MAX]; + /* + * 2) Scale around the mid point differently for lower and upper range. + * + * This is necessary as they don't share the same endpoints and slope. + * + * First normalize to 0..1 range with correct sign (below or above center), + * then scale to 20000 range (if center is an actual center, -10000..10000, + * if parameters only support half range, scale to 10000 range, e.g. if + * center == min 0..10000, if center == max -10000..0). + * + * As the min and max bounds were enforced in step 1), division by zero + * cannot occur, as for the case of center == min or center == max the if + * statement is mutually exclusive with the arithmetic NaN case. + * + * DO NOT REMOVE OR ALTER STEP 1! + */ + if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) { + scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])); - /* - * 2) Scale around the mid point differently for lower and upper range. - * - * This is necessary as they don't share the same endpoints and slope. - * - * First normalize to 0..1 range with correct sign (below or above center), - * then scale to 20000 range (if center is an actual center, -10000..10000, - * if parameters only support half range, scale to 10000 range, e.g. if - * center == min 0..10000, if center == max -10000..0). - * - * As the min and max bounds were enforced in step 1), division by zero - * cannot occur, as for the case of center == min or center == max the if - * statement is mutually exclusive with the arithmetic NaN case. - * - * DO NOT REMOVE OR ALTER STEP 1! - */ - if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) { - scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])); + } else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) { + scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN])); - } else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) { - scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN])); + } else { + /* in the configured dead zone, output zero */ + scaled = 0; + } - } else { - /* in the configured dead zone, output zero */ - scaled = 0; - } + /* invert channel if requested */ + if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) + scaled = -scaled; - /* invert channel if requested */ - if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) + /* and update the scaled/mapped version */ + unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]; + if (mapped < PX4IO_CONTROL_CHANNELS) { + + /* invert channel if pitch - pulling the lever down means pitching up by convention */ + if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */ scaled = -scaled; - /* and update the scaled/mapped version */ - unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]; - if (mapped < PX4IO_CONTROL_CHANNELS) { + r_rc_values[mapped] = SIGNED_TO_REG(scaled); + assigned_channels |= (1 << mapped); - /* invert channel if pitch - pulling the lever down means pitching up by convention */ - if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */ - scaled = -scaled; - - r_rc_values[mapped] = SIGNED_TO_REG(scaled); - assigned_channels |= (1 << mapped); - - } } } + } - /* set un-assigned controls to zero */ - for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) { - if (!(assigned_channels & (1 << i))) - r_rc_values[i] = 0; - } + /* set un-assigned controls to zero */ + for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) { + if (!(assigned_channels & (1 << i))) + r_rc_values[i] = 0; + } - /* set RC OK flag, as we got an update */ - r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK; + /* set RC OK flag, as we got an update */ + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK; - /* if we have enough channels (5) to control the vehicle, the mapping is ok */ - if (assigned_channels > 4) { - r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK; - } else { - r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK); - } + /* if we have enough channels (5) to control the vehicle, the mapping is ok */ + if (assigned_channels > 4) { + r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK; + } else { + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK); } /* @@ -316,8 +312,8 @@ controls_tick() { * Handle losing RC input */ - /* this kicks in if the receiver is gone or the system went to failsafe */ - if (rc_input_lost || (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) { + /* this kicks in if the receiver is gone, but there is not on failsafe (indicated by separate flag) */ + if (rc_input_lost) { /* Clear the RC input status flag, clear manual override flag */ r_status_flags &= ~( PX4IO_P_STATUS_FLAGS_OVERRIDE | From 506c16f12a8420991a0e64c616bbf5833ce1845c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 25 Mar 2014 17:37:44 +0100 Subject: [PATCH 277/381] Bring fixed wing HIL back to normal mode, keep multicopter unchanged --- src/modules/mavlink/mavlink_messages.cpp | 31 ++++++++++++++++-------- 1 file changed, 21 insertions(+), 10 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index e292f52ba0..4ca3840d47 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -763,9 +763,6 @@ private: MavlinkOrbSubscription *act_sub; struct actuator_outputs_s *act; - MavlinkOrbSubscription *ctrl_sub; - struct actuator_controls_s *ctrl; - protected: void subscribe(Mavlink *mavlink) { @@ -777,9 +774,6 @@ protected: act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0)); act = (struct actuator_outputs_s *)act_sub->get_data(); - - ctrl_sub = mavlink->add_orb_subscription(ORB_ID(actuator_controls_0)); - ctrl = (struct actuator_controls_s *)ctrl_sub->get_data(); } void send(const hrt_abstime t) @@ -787,7 +781,6 @@ protected: bool updated = status_sub->update(t); updated |= pos_sp_triplet_sub->update(t); updated |= act_sub->update(t); - updated |= ctrl_sub->update(t); if (updated) { /* translate the current syste state to mavlink state and mode */ @@ -842,12 +835,30 @@ protected: 0); } else { - /* no special handling required, just send the controls */ + /* fixed wing: scale all channels except throttle -1 .. 1 + * because we know that we set the mixers up this way + */ + + float out[8]; + + const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2; + + for (unsigned i = 0; i < 8; i++) { + if (i != 3) { + /* scale fake PWM out 900..2100 us to -1..+1 for normal channels */ + out[i] = (act->output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); + + } else { + + /* scale fake PWM out 900..2100 us to 0..1 for throttle */ + out[i] = (act->output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); + } + + } mavlink_msg_hil_controls_send(_channel, hrt_absolute_time(), - ctrl->control[0], ctrl->control[1], ctrl->control[2], ctrl->control[3], - ctrl->control[4], ctrl->control[5], ctrl->control[6], ctrl->control[7], + out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], mavlink_base_mode, 0); } From b61e6f2706fed68257c909bdd8a84feda5121344 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 25 Mar 2014 21:23:48 +0100 Subject: [PATCH 278/381] mtecs: don't flow the stdout too much --- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 35 ++++++++++++------- src/modules/fw_pos_control_l1/mtecs/mTecs.h | 1 + 2 files changed, 24 insertions(+), 12 deletions(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index ad108e7eec..fa9a6d9475 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -59,7 +59,8 @@ mTecs::mTecs() : _throttleSp(0.0f), _pitchSp(0.0f), timestampLastIteration(hrt_absolute_time()), - _firstIterationAfterReset(true) + _firstIterationAfterReset(true), + _counter(0) { } @@ -69,16 +70,21 @@ mTecs::~mTecs() void mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, float airspeedSp) { - warnx("***"); + float flightPathAngleSp = _controlAltitude.update(altitudeSp - altitude); - warnx("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)flightPathAngleSp); + if (_counter % 10 == 0) { + warnx("***"); + warnx("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)flightPathAngleSp); + } updateFlightPathAngleSpeed(flightPathAngle, flightPathAngleSp, airspeed, airspeedSp); } void mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, float airspeedSp) { float accelerationLongitudinalSp = _controlAirSpeed.update(airspeedSp - airspeed); - warnx("updateFlightPathAngleSpeed airspeedSp %.4f, airspeed %.4f, accelerationLongitudinalSp%.4f", (double)airspeedSp, (double)airspeed, (double)accelerationLongitudinalSp); + if (_counter % 10 == 0) { + warnx("updateFlightPathAngleSpeed airspeedSp %.4f, airspeed %.4f, accelerationLongitudinalSp%.4f", (double)airspeedSp, (double)airspeed, (double)accelerationLongitudinalSp); + } updateFlightPathAngleAcceleration(flightPathAngle, flightPathAngleSp, airspeed, accelerationLongitudinalSp); } @@ -117,10 +123,12 @@ void mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float fligh float energyDistributionRateSp = flightPathAngleSp - airspeedSpDerivativeNorm; float energyDistributionRateError2 = energyDistributionRateSp - energyDistributionRate; - warnx("totalEnergyRateSp %.2f, totalEnergyRate %.2f, totalEnergyRateError %.2f totalEnergyRateError2 %.2f airspeedDerivativeNorm %.4f", - (double)totalEnergyRateSp, (double)totalEnergyRate, (double)totalEnergyRateError, (double)totalEnergyRateError2, (double)airspeedDerivativeNorm); - warnx("energyDistributionRateSp %.2f, energyDistributionRate %.2f, energyDistributionRateError %.2f energyDistributionRateError2 %.2f", - (double)energyDistributionRateSp, (double)energyDistributionRate, (double)energyDistributionRateError, (double)energyDistributionRateError2); + if (_counter % 10 == 0) { + warnx("totalEnergyRateSp %.2f, totalEnergyRate %.2f, totalEnergyRateError %.2f totalEnergyRateError2 %.2f airspeedDerivativeNorm %.4f", + (double)totalEnergyRateSp, (double)totalEnergyRate, (double)totalEnergyRateError, (double)totalEnergyRateError2, (double)airspeedDerivativeNorm); + warnx("energyDistributionRateSp %.2f, energyDistributionRate %.2f, energyDistributionRateError %.2f energyDistributionRateError2 %.2f", + (double)energyDistributionRateSp, (double)energyDistributionRate, (double)energyDistributionRateError, (double)energyDistributionRateError2); + } /** update control blocks **/ /* update total energy rate control block */ @@ -129,15 +137,18 @@ void mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float fligh /* update energy distribution rate control block */ _pitchSp = _controlEnergyDistribution.update(energyDistributionRateSp, energyDistributionRateError); - warnx("_throttleSp %.1f, _pitchSp %.1f, flightPathAngleSp %.1f, flightPathAngle %.1f accelerationLongitudinalSp %.1f, airspeedDerivative %.1f", - (double)_throttleSp, (double)_pitchSp, - (double)flightPathAngleSp, (double)flightPathAngle, - (double)accelerationLongitudinalSp, (double)airspeedDerivative); + if (_counter % 10 == 0) { + warnx("_throttleSp %.1f, _pitchSp %.1f, flightPathAngleSp %.1f, flightPathAngle %.1f accelerationLongitudinalSp %.1f, airspeedDerivative %.1f", + (double)_throttleSp, (double)_pitchSp, + (double)flightPathAngleSp, (double)flightPathAngle, + (double)accelerationLongitudinalSp, (double)airspeedDerivative); + } /* clean up */ _firstIterationAfterReset = false; + _counter++; } void mTecs::resetIntegrators() diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h index 5877a8312b..32d9879db6 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -106,6 +106,7 @@ protected: bool _firstIterationAfterReset; /**< True during the first iteration after a reset */ + int _counter; }; } /* namespace fwPosctrl */ From c81aefc756c6587e306986312370cf9d22a5f534 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 25 Mar 2014 21:24:20 +0100 Subject: [PATCH 279/381] mtecs: commented out warnx --- .../fw_pos_control_l1/mtecs/mTecs_blocks.h | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h index 7dd5c7c2e2..aff01479d1 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h @@ -89,8 +89,8 @@ public: float maximum = isAngularLimit() ? getMax() * M_DEG_TO_RAD_F : getMax(); char name[blockNameLengthMax]; getName(name, blockNameLengthMax); - warnx("%s minimum %.2f maximum %.2f, getMin() %.2f, getMax() %.2f, isAngularLimit() %u", - name,(double)minimum,(double)maximum, (double)getMin(), (double)getMax(), isAngularLimit()); +// warnx("%s minimum %.2f maximum %.2f, getMin() %.2f, getMax() %.2f, isAngularLimit() %u", +// name,(double)minimum,(double)maximum, (double)getMin(), (double)getMax(), isAngularLimit()); if (value < minimum) { difference = value - minimum; value = minimum; @@ -136,15 +136,15 @@ public: float output = getOffset() + getKFF() * inputValue + getKP() * inputError + getKI() * getIntegral().update(inputError); char name[blockNameLengthMax]; getName(name, blockNameLengthMax); - warnx("%s output %.2f getKFF() %.2f, inputValue %.2f, getKP() %.2f, getKI() %.2f, getIntegral().getY() %.2f, inputError %.2f getIntegral().getDt() %.2f", name, - (double)output, (double)getKFF(), (double)inputValue, (double)getKP(), (double)getKI(), (double)getIntegral().getY(), (double)inputError, (double)getIntegral().getDt()); +// warnx("%s output %.2f getKFF() %.2f, inputValue %.2f, getKP() %.2f, getKI() %.2f, getIntegral().getY() %.2f, inputError %.2f getIntegral().getDt() %.2f", name, +// (double)output, (double)getKFF(), (double)inputValue, (double)getKP(), (double)getKI(), (double)getIntegral().getY(), (double)inputError, (double)getIntegral().getDt()); if(!getOutputLimiter().limit(output, difference) && (((difference < 0) && (getKI() * getIntegral().update(inputError) < 0)) || ((difference > 0) && (getKI() * getIntegral().update(inputError) > 0)))) { getIntegral().setY(integralYPrevious); } - warnx("%s output limited %.2f", - name,(double)output); +// warnx("%s output limited %.2f", +// name,(double)output); return output; } // accessors @@ -179,11 +179,11 @@ public: float output = getKP() * input; char name[blockNameLengthMax]; getName(name, blockNameLengthMax); - warnx("%s output %.2f _kP.get() %.2f, input", - name,(double)output, (double)_kP.get(), (double)input); +// warnx("%s output %.2f _kP.get() %.2f, input", +// name,(double)output, (double)_kP.get(), (double)input); getOutputLimiter().limit(output, difference); - warnx("%s output limited %.2f", - name,(double)output); +// warnx("%s output limited %.2f", +// name,(double)output); return output; } // accessors From f949395edf873b423cae3d62a95cd0800a2d344f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 25 Mar 2014 22:25:16 +0100 Subject: [PATCH 280/381] mtecs: correct a very wrong default param value --- src/modules/fw_pos_control_l1/mtecs/mTecs_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c index 76ed953033..705b2cd595 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c @@ -213,7 +213,7 @@ PARAM_DEFINE_FLOAT(MT_ACC_P, 0.1f); * @unit m/s^2 * @group mTECS */ -PARAM_DEFINE_FLOAT(MT_ACC_MIN, 0.0f); +PARAM_DEFINE_FLOAT(MT_ACC_MIN, -40.0f); /** * Maximal acceleration (air) From d8eeec6cdbff505ac7f50877cc552b33ccf8837c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 25 Mar 2014 23:03:06 +0100 Subject: [PATCH 281/381] mtecs: comment out debug code --- src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h index aff01479d1..c15d90cdb3 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h @@ -87,8 +87,8 @@ public: bool limit(float& value, float& difference) { float minimum = isAngularLimit() ? getMin() * M_DEG_TO_RAD_F : getMin(); float maximum = isAngularLimit() ? getMax() * M_DEG_TO_RAD_F : getMax(); - char name[blockNameLengthMax]; - getName(name, blockNameLengthMax); +// char name[blockNameLengthMax]; +// getName(name, blockNameLengthMax); // warnx("%s minimum %.2f maximum %.2f, getMin() %.2f, getMax() %.2f, isAngularLimit() %u", // name,(double)minimum,(double)maximum, (double)getMin(), (double)getMax(), isAngularLimit()); if (value < minimum) { @@ -134,8 +134,8 @@ public: float difference = 0.0f; float integralYPrevious = _integral.getY(); float output = getOffset() + getKFF() * inputValue + getKP() * inputError + getKI() * getIntegral().update(inputError); - char name[blockNameLengthMax]; - getName(name, blockNameLengthMax); +// char name[blockNameLengthMax]; +// getName(name, blockNameLengthMax); // warnx("%s output %.2f getKFF() %.2f, inputValue %.2f, getKP() %.2f, getKI() %.2f, getIntegral().getY() %.2f, inputError %.2f getIntegral().getDt() %.2f", name, // (double)output, (double)getKFF(), (double)inputValue, (double)getKP(), (double)getKI(), (double)getIntegral().getY(), (double)inputError, (double)getIntegral().getDt()); if(!getOutputLimiter().limit(output, difference) && @@ -177,8 +177,8 @@ public: float update(float input) { float difference = 0.0f; float output = getKP() * input; - char name[blockNameLengthMax]; - getName(name, blockNameLengthMax); +// char name[blockNameLengthMax]; +// getName(name, blockNameLengthMax); // warnx("%s output %.2f _kP.get() %.2f, input", // name,(double)output, (double)_kP.get(), (double)input); getOutputLimiter().limit(output, difference); From 66527eea02ca6c8b1e33047f60bce1b832f82071 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 27 Mar 2014 17:54:29 +0100 Subject: [PATCH 282/381] commander: workaround to prevent RC loss in HIL --- src/modules/commander/commander.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index d114a2e5ce..cf7ba757e2 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -119,6 +119,7 @@ extern struct system_load_s system_load; #define POSITION_TIMEOUT 1000000 /**< consider the local or global position estimate invalid after 1s */ #define RC_TIMEOUT 100000 +#define RC_TIMEOUT_HIL 500000 #define DIFFPRESS_TIMEOUT 2000000 #define PRINT_INTERVAL 5000000 @@ -1108,8 +1109,16 @@ int commander_thread_main(int argc, char *argv[]) } } + + /* + * XXX workaround: + * Prevent RC loss in HIL when sensors.cpp is only publishing sp_man at a low rate (e.g. 30Hz) + * which can trigger RC loss if the computer/simulator lags. + */ + uint64_t rc_timeout = status.hil_state == HIL_STATE_ON ? RC_TIMEOUT_HIL : RC_TIMEOUT; + /* start 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 + rc_timeout) { /* handle the case where RC signal was regained */ if (!status.rc_signal_found_once) { status.rc_signal_found_once = true; From 4824484497cf0d85d08fe9c9727aa5edc4446e67 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 27 Mar 2014 19:03:57 +0100 Subject: [PATCH 283/381] mtecs: add FPA D gain --- src/modules/fw_pos_control_l1/mtecs/mTecs.h | 2 +- .../fw_pos_control_l1/mtecs/mTecs_blocks.h | 48 ++++++++++++------- .../fw_pos_control_l1/mtecs/mTecs_params.c | 17 +++++++ 3 files changed, 50 insertions(+), 17 deletions(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h index 32d9879db6..502a8a8b0a 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -91,7 +91,7 @@ protected: /* control blocks */ BlockFFPILimited _controlTotalEnergy; /**< FFPI controller for total energy control: output is throttle */ BlockFFPILimited _controlEnergyDistribution; /**< FFPI controller for energy distribution control: output is pitch */ - BlockPLimited _controlAltitude; /**< P controller for altitude: output is the flight path angle setpoint */ + BlockPDLimited _controlAltitude; /**< P controller for altitude: output is the flight path angle setpoint */ BlockPLimited _controlAirSpeed; /**< P controller for airspeed: output is acceleration setpoint */ /* Other calculation Blocks */ diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h index c15d90cdb3..a357b24e28 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h @@ -87,10 +87,6 @@ public: bool limit(float& value, float& difference) { float minimum = isAngularLimit() ? getMin() * M_DEG_TO_RAD_F : getMin(); float maximum = isAngularLimit() ? getMax() * M_DEG_TO_RAD_F : getMax(); -// char name[blockNameLengthMax]; -// getName(name, blockNameLengthMax); -// warnx("%s minimum %.2f maximum %.2f, getMin() %.2f, getMax() %.2f, isAngularLimit() %u", -// name,(double)minimum,(double)maximum, (double)getMin(), (double)getMax(), isAngularLimit()); if (value < minimum) { difference = value - minimum; value = minimum; @@ -134,17 +130,11 @@ public: float difference = 0.0f; float integralYPrevious = _integral.getY(); float output = getOffset() + getKFF() * inputValue + getKP() * inputError + getKI() * getIntegral().update(inputError); -// char name[blockNameLengthMax]; -// getName(name, blockNameLengthMax); -// warnx("%s output %.2f getKFF() %.2f, inputValue %.2f, getKP() %.2f, getKI() %.2f, getIntegral().getY() %.2f, inputError %.2f getIntegral().getDt() %.2f", name, -// (double)output, (double)getKFF(), (double)inputValue, (double)getKP(), (double)getKI(), (double)getIntegral().getY(), (double)inputError, (double)getIntegral().getDt()); if(!getOutputLimiter().limit(output, difference) && (((difference < 0) && (getKI() * getIntegral().update(inputError) < 0)) || ((difference > 0) && (getKI() * getIntegral().update(inputError) > 0)))) { getIntegral().setY(integralYPrevious); } -// warnx("%s output limited %.2f", -// name,(double)output); return output; } // accessors @@ -177,13 +167,7 @@ public: float update(float input) { float difference = 0.0f; float output = getKP() * input; -// char name[blockNameLengthMax]; -// getName(name, blockNameLengthMax); -// warnx("%s output %.2f _kP.get() %.2f, input", -// name,(double)output, (double)_kP.get(), (double)input); getOutputLimiter().limit(output, difference); -// warnx("%s output limited %.2f", -// name,(double)output); return output; } // accessors @@ -194,5 +178,37 @@ private: BlockOutputLimiter _outputLimiter; }; +/* A combination of P, D gains and output limiter */ +class BlockPDLimited: public SuperBlock +{ +public: +// methods + BlockPDLimited(SuperBlock *parent, const char *name, bool isAngularLimit = false) : + SuperBlock(parent, name), + _kP(this, "P"), + _kD(this, "D"), + _derivative(this, "D"), + _outputLimiter(this, "", isAngularLimit) + {}; + virtual ~BlockPDLimited() {}; + float update(float input) { + float difference = 0.0f; + float output = getKP() * input + getKD() * getDerivative().update(input); + getOutputLimiter().limit(output, difference); + + return output; + } +// accessors + float getKP() { return _kP.get(); } + float getKD() { return _kD.get(); } + BlockDerivative &getDerivative() { return _derivative; } + BlockOutputLimiter &getOutputLimiter() { return _outputLimiter; }; +private: + control::BlockParamFloat _kP; + control::BlockParamFloat _kD; + BlockDerivative _derivative; + BlockOutputLimiter _outputLimiter; +}; + } diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c index 705b2cd595..f8db703041 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c @@ -177,6 +177,23 @@ PARAM_DEFINE_FLOAT(MT_PIT_MAX, 45.0f); */ PARAM_DEFINE_FLOAT(MT_FPA_P, 0.1f); +/** + * D gain for the altitude control + * + * @min 0.0f + * @max 10.0f + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_FPA_D, 0.0f); + +/** + * Lowpass for FPA error derivative (see MT_FPA_D) + * + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_FPA_D_LP, 1.0f); + + /** * Minimal flight path angle setpoint * From 0d526bddca9210bed75cfd26458a48ac87453c9e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 27 Mar 2014 21:47:34 +0100 Subject: [PATCH 284/381] fw_pos_control: whitespace in module.mk --- src/modules/fw_pos_control_l1/module.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/module.mk b/src/modules/fw_pos_control_l1/module.mk index 5d752437fb..c887223f4a 100644 --- a/src/modules/fw_pos_control_l1/module.mk +++ b/src/modules/fw_pos_control_l1/module.mk @@ -40,5 +40,5 @@ MODULE_COMMAND = fw_pos_control_l1 SRCS = fw_pos_control_l1_main.cpp \ fw_pos_control_l1_params.c \ landingslope.cpp \ - mtecs/mTecs.cpp\ + mtecs/mTecs.cpp \ mtecs/mTecs_params.c From d102afba8bdb526b85dce6dc47034ddd170d7240 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 27 Mar 2014 22:12:01 +0100 Subject: [PATCH 285/381] mtecs: make sure dt is calculated before any control calculations --- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 31 ++++++++++++++----- src/modules/fw_pos_control_l1/mtecs/mTecs.h | 8 ++++- 2 files changed, 31 insertions(+), 8 deletions(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index fa9a6d9475..3118ea5d12 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -60,6 +60,7 @@ mTecs::mTecs() : _pitchSp(0.0f), timestampLastIteration(hrt_absolute_time()), _firstIterationAfterReset(true), + dtCalculated(false), _counter(0) { } @@ -71,6 +72,9 @@ mTecs::~mTecs() void mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, float airspeedSp) { + /* time measurement */ + updateTimeMeasurement(); + float flightPathAngleSp = _controlAltitude.update(altitudeSp - altitude); if (_counter % 10 == 0) { warnx("***"); @@ -81,6 +85,9 @@ void mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alt void mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, float airspeedSp) { + /* time measurement */ + updateTimeMeasurement(); + float accelerationLongitudinalSp = _controlAirSpeed.update(airspeedSp - airspeed); if (_counter % 10 == 0) { warnx("updateFlightPathAngleSpeed airspeedSp %.4f, airspeed %.4f, accelerationLongitudinalSp%.4f", (double)airspeedSp, (double)airspeed, (double)accelerationLongitudinalSp); @@ -91,13 +98,7 @@ void mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAn void mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed, float accelerationLongitudinalSp) { /* time measurement */ - float deltaTSeconds = 0.0f; - if (!_firstIterationAfterReset) { - hrt_abstime timestampNow = hrt_absolute_time(); - deltaTSeconds = (float)(timestampNow - timestampLastIteration) * 1e-6f; - timestampLastIteration = timestampNow; - } - setDt(deltaTSeconds); + updateTimeMeasurement(); /* update parameters first */ updateParams(); @@ -147,6 +148,7 @@ void mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float fligh /* clean up */ _firstIterationAfterReset = false; + dtCalculated = false; _counter++; } @@ -159,5 +161,20 @@ void mTecs::resetIntegrators() _firstIterationAfterReset = true; } +void mTecs::updateTimeMeasurement() +{ + if (!dtCalculated) { + float deltaTSeconds = 0.0f; + if (!_firstIterationAfterReset) { + hrt_abstime timestampNow = hrt_absolute_time(); + deltaTSeconds = (float)(timestampNow - timestampLastIteration) * 1e-6f; + timestampLastIteration = timestampNow; + } + setDt(deltaTSeconds); + + dtCalculated = true; + } +} + } /* namespace fwPosctrl */ diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h index 502a8a8b0a..62c4d70140 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -104,9 +104,15 @@ protected: /* Time measurements */ hrt_abstime timestampLastIteration; /**< Saves the result of hrt_absolute_time() of the last iteration */ - bool _firstIterationAfterReset; /**< True during the first iteration after a reset */ + bool _firstIterationAfterReset; /**< True during the first iteration after a reset */ + bool dtCalculated; /**< True if dt has been calculated in this iteration */ int _counter; + + /* + * Measure and update the time step dt if this was not already done in the current iteration + */ + void updateTimeMeasurement(); }; } /* namespace fwPosctrl */ From d3ca12f136c9a890f7289d69bd783840dc86cfba Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 27 Mar 2014 22:34:23 +0100 Subject: [PATCH 286/381] mtecs: BlockPDLimited: make sure dt > 0 --- src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h index a357b24e28..6e90a82bae 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h @@ -193,7 +193,7 @@ public: virtual ~BlockPDLimited() {}; float update(float input) { float difference = 0.0f; - float output = getKP() * input + getKD() * getDerivative().update(input); + float output = getKP() * input + (getDerivative().getDt() > 0.0f ? getKD() * getDerivative().update(input) : 0.0f); getOutputLimiter().limit(output, difference); return output; From 3e9dfcb6f7bbda7653e6d8873b6273f2e9299d73 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 27 Mar 2014 22:57:29 +0100 Subject: [PATCH 287/381] mtecs: first rough version of takeoff mode --- .../fw_pos_control_l1_main.cpp | 11 ++++- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 25 +++++++--- src/modules/fw_pos_control_l1/mtecs/mTecs.h | 19 ++++++-- .../fw_pos_control_l1/mtecs/mTecs_blocks.h | 48 +++++++++++++------ .../fw_pos_control_l1/mtecs/mTecs_params.c | 39 +++++++++++++++ 5 files changed, 113 insertions(+), 29 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 1e8447aed5..44afa43d80 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 @@ -361,7 +361,7 @@ private: void reset_landing_state(); /* - * Call TECS + * Call TECS : a wrapper function to call one of the TECS implementations (mTECS is called only if enabled via parameter) */ void tecs_update_pitch_throttle(float alt_sp, float v_sp, float eas2tas, float pitch_min_rad, float pitch_max_rad, @@ -1364,7 +1364,14 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ if (ground_speed_length > FLT_EPSILON) { flightPathAngle = -asinf(ground_speed(2)/ground_speed_length); } - _mTecs.updateAltitudeSpeed(flightPathAngle, _global_pos.alt, alt_sp, _airspeed.true_airspeed_m_s, v_sp); + + if (!climbout_mode) { + /* Normal operation */ + _mTecs.updateAltitudeSpeed(flightPathAngle, _global_pos.alt, alt_sp, _airspeed.true_airspeed_m_s, v_sp, fwPosctrl::mTecs::TECS_MODE_NORMAL); + } else { + /* Climbout/Takeoff mode requested */ + _mTecs.updateAltitudeSpeed(flightPathAngle, _global_pos.alt, alt_sp, _airspeed.true_airspeed_m_s, v_sp, fwPosctrl::mTecs::TECS_MODE_TAKEOFF); + } } else { _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, alt_sp, v_sp, diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index 3118ea5d12..33bfd49622 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -58,6 +58,8 @@ mTecs::mTecs() : _airspeedDerivative(this, "AD"), _throttleSp(0.0f), _pitchSp(0.0f), + BlockOutputLimiterTakeoffThrottle(this, "TKF_THR"), + BlockOutputLimiterTakeoffPitch(this, "TKF_PIT", true), timestampLastIteration(hrt_absolute_time()), _firstIterationAfterReset(true), dtCalculated(false), @@ -69,7 +71,7 @@ mTecs::~mTecs() { } -void mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, float airspeedSp) +void mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, float airspeedSp, tecs_mode mode) { /* time measurement */ @@ -80,10 +82,10 @@ void mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alt warnx("***"); warnx("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)flightPathAngleSp); } - updateFlightPathAngleSpeed(flightPathAngle, flightPathAngleSp, airspeed, airspeedSp); + updateFlightPathAngleSpeed(flightPathAngle, flightPathAngleSp, airspeed, airspeedSp, mode); } -void mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, float airspeedSp) { +void mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, float airspeedSp, tecs_mode mode) { /* time measurement */ updateTimeMeasurement(); @@ -92,10 +94,10 @@ void mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAn if (_counter % 10 == 0) { warnx("updateFlightPathAngleSpeed airspeedSp %.4f, airspeed %.4f, accelerationLongitudinalSp%.4f", (double)airspeedSp, (double)airspeed, (double)accelerationLongitudinalSp); } - updateFlightPathAngleAcceleration(flightPathAngle, flightPathAngleSp, airspeed, accelerationLongitudinalSp); + updateFlightPathAngleAcceleration(flightPathAngle, flightPathAngleSp, airspeed, accelerationLongitudinalSp, mode); } -void mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed, float accelerationLongitudinalSp) +void mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed, float accelerationLongitudinalSp, tecs_mode mode) { /* time measurement */ updateTimeMeasurement(); @@ -131,12 +133,21 @@ void mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float fligh (double)energyDistributionRateSp, (double)energyDistributionRate, (double)energyDistributionRateError, (double)energyDistributionRateError2); } + /* Set special ouput limiters if we are not in TECS_MODE_NORMAL */ + BlockOutputLimiter *outputLimiterThrottle = NULL; + BlockOutputLimiter *outputLimiterPitch = NULL; + if (mode == TECS_MODE_TAKEOFF) { + outputLimiterThrottle = &BlockOutputLimiterTakeoffThrottle; //XXX: accept prelaunch values from launchdetector + outputLimiterPitch = &BlockOutputLimiterTakeoffPitch; + } + /** update control blocks **/ /* update total energy rate control block */ - _throttleSp = _controlTotalEnergy.update(totalEnergyRateSp, totalEnergyRateError); + _throttleSp = _controlTotalEnergy.update(totalEnergyRateSp, totalEnergyRateError, outputLimiterThrottle); /* update energy distribution rate control block */ - _pitchSp = _controlEnergyDistribution.update(energyDistributionRateSp, energyDistributionRateError); + _pitchSp = _controlEnergyDistribution.update(energyDistributionRateSp, energyDistributionRateError, outputLimiterPitch); + if (_counter % 10 == 0) { warnx("_throttleSp %.1f, _pitchSp %.1f, flightPathAngleSp %.1f, flightPathAngle %.1f accelerationLongitudinalSp %.1f, airspeedDerivative %.1f", diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h index 62c4d70140..6c30a458a9 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -58,20 +58,25 @@ public: mTecs(); virtual ~mTecs(); + typedef enum { + TECS_MODE_NORMAL, + TECS_MODE_TAKEOFF + } tecs_mode; + /* * Control in altitude setpoint and speed mode */ - void updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, float airspeedSp); + void updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, float airspeedSp, tecs_mode mode); /* * Control in flightPathAngle setpoint (flollow a slope etc.) and speed mode */ - void updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, float airspeedSp); + void updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, float airspeedSp, tecs_mode mode); /* * Control in flightPathAngle setpoint (flollow a slope etc.) and acceleration mode (base case) */ - void updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed, float accelerationLongitudinalSp); + void updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed, float accelerationLongitudinalSp, tecs_mode mode); /* * Reset all integrators @@ -89,8 +94,8 @@ protected: control::BlockParamInt _mTecsEnabled; /**< 1 if mTecs is enabled */ /* control blocks */ - BlockFFPILimited _controlTotalEnergy; /**< FFPI controller for total energy control: output is throttle */ - BlockFFPILimited _controlEnergyDistribution; /**< FFPI controller for energy distribution control: output is pitch */ + BlockFFPILimitedCustom _controlTotalEnergy; /**< FFPI controller for total energy control: output is throttle */ + BlockFFPILimitedCustom _controlEnergyDistribution; /**< FFPI controller for energy distribution control: output is pitch */ BlockPDLimited _controlAltitude; /**< P controller for altitude: output is the flight path angle setpoint */ BlockPLimited _controlAirSpeed; /**< P controller for airspeed: output is acceleration setpoint */ @@ -101,6 +106,10 @@ protected: float _throttleSp; /**< Throttle Setpoint from 0 to 1 */ float _pitchSp; /**< Pitch Setpoint from -pi to pi */ + /* Output Limits in special modes */ + BlockOutputLimiter BlockOutputLimiterTakeoffThrottle; /**< Throttle Limits during takeoff */ + BlockOutputLimiter BlockOutputLimiterTakeoffPitch; /**< Pitch Limit during takeoff */ + /* Time measurements */ hrt_abstime timestampLastIteration; /**< Saves the result of hrt_absolute_time() of the last iteration */ diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h index 6e90a82bae..f3dc9bcb28 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h @@ -111,32 +111,22 @@ protected: typedef -/* A combination of feed forward, P and I gain using the output limiter*/ +/* A combination of feed forward, P and I gain using the output limiter*/ class BlockFFPILimited: public SuperBlock { public: // methods BlockFFPILimited(SuperBlock *parent, const char *name, bool isAngularLimit = false) : SuperBlock(parent, name), + _outputLimiter(this, "", isAngularLimit), _integral(this, "I"), _kFF(this, "FF"), _kP(this, "P"), _kI(this, "I"), - _offset(this, "OFF"), - _outputLimiter(this, "", isAngularLimit) + _offset(this, "OFF") {}; virtual ~BlockFFPILimited() {}; - float update(float inputValue, float inputError) { - float difference = 0.0f; - float integralYPrevious = _integral.getY(); - float output = getOffset() + getKFF() * inputValue + getKP() * inputError + getKI() * getIntegral().update(inputError); - if(!getOutputLimiter().limit(output, difference) && - (((difference < 0) && (getKI() * getIntegral().update(inputError) < 0)) || - ((difference > 0) && (getKI() * getIntegral().update(inputError) > 0)))) { - getIntegral().setY(integralYPrevious); - } - return output; - } + float update(float inputValue, float inputError) { return calcLimitedOutput(inputValue, inputError, _outputLimiter); } // accessors BlockIntegralNoLimit &getIntegral() { return _integral; } float getKFF() { return _kFF.get(); } @@ -144,13 +134,41 @@ public: float getKI() { return _kI.get(); } float getOffset() { return _offset.get(); } BlockOutputLimiter &getOutputLimiter() { return _outputLimiter; }; +protected: + BlockOutputLimiter _outputLimiter; + + float calcUnlimitedOutput(float inputValue, float inputError) {return getOffset() + getKFF() * inputValue + getKP() * inputError + getKI() * getIntegral().update(inputError);} + float calcLimitedOutput(float inputValue, float inputError, BlockOutputLimiter &outputLimiter) { + float difference = 0.0f; + float integralYPrevious = _integral.getY(); + float output = calcUnlimitedOutput(inputValue, inputError); + if(!outputLimiter.limit(output, difference) && + (((difference < 0) && (getKI() * getIntegral().update(inputError) < 0)) || + ((difference > 0) && (getKI() * getIntegral().update(inputError) > 0)))) { + getIntegral().setY(integralYPrevious); + } + return output; + } private: BlockIntegralNoLimit _integral; BlockParamFloat _kFF; BlockParamFloat _kP; BlockParamFloat _kI; BlockParamFloat _offset; - BlockOutputLimiter _outputLimiter; +}; + +/* A combination of feed forward, P and I gain using the output limiter with the option to provide a special output limiter (for example for takeoff)*/ +class BlockFFPILimitedCustom: public BlockFFPILimited +{ +public: +// methods + BlockFFPILimitedCustom(SuperBlock *parent, const char *name, bool isAngularLimit = false) : + BlockFFPILimited(parent, name, isAngularLimit) + {}; + virtual ~BlockFFPILimitedCustom() {}; + float update(float inputValue, float inputError, BlockOutputLimiter *outputLimiter = NULL) { + return calcLimitedOutput(inputValue, inputError, outputLimiter == NULL ? _outputLimiter : *outputLimiter); + } }; /* A combination of P gain and output limiter */ diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c index f8db703041..bbd957273a 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c @@ -246,3 +246,42 @@ PARAM_DEFINE_FLOAT(MT_ACC_MAX, 40.0f); * @group mTECS */ PARAM_DEFINE_FLOAT(MT_AD_LP, 1.0f); + + +/** + * Minimal throttle during takeoff + * + * @min 0.0f + * @max 1.0f + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_TKF_THR_MIN, 1.0f); + +/** + * Maximal throttle during takeoff + * + * @min 0.0f + * @max 1.0f + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_TKF_THR_MAX, 1.0f); + +/** + * Minimal pitch during takeoff + * + * @min -90.0f + * @max 90.0f + * @unit deg + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_TKF_PIT_MIN, 0.0f); + +/** + * Maximal pitch during takeoff + * + * @min -90.0f + * @max 90.0f + * @unit deg + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_TKF_PIT_MAX, 45.0f); From 903012bcff60e3d59f706bec7a920ce3e3aab754 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 29 Mar 2014 14:14:58 +0100 Subject: [PATCH 288/381] Provide the wing-wing ESC an idle pulse to silence it --- ROMFS/px4fmu_common/init.d/3033_wingwing | 3 +++ 1 file changed, 3 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/3033_wingwing b/ROMFS/px4fmu_common/init.d/3033_wingwing index 6e1a531cfa..fff4b58df4 100644 --- a/ROMFS/px4fmu_common/init.d/3033_wingwing +++ b/ROMFS/px4fmu_common/init.d/3033_wingwing @@ -33,3 +33,6 @@ then fi set MIXER FMU_Q +# Provide ESC a constant 1000 us pulse +set PWM_OUTPUTS 4 +set PWM_DISARMED 1000 From 51e0ccb199fa73571ca0cd0f366e9453e0741bed Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 31 Mar 2014 10:10:50 +0400 Subject: [PATCH 289/381] rcS: removed unnecessary sleeps, minor code style fixes --- ROMFS/px4fmu_common/init.d/rcS | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 503dbb83e1..0cb6d281a2 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -294,6 +294,7 @@ then tone_alarm $TUNE_OUT_ERROR fi fi + if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == ardrone ] then echo "[init] Use FMU as primary output" @@ -317,6 +318,7 @@ then fi fi fi + if [ $OUTPUT_MODE == mkblctrl ] then echo "[init] Use MKBLCTRL as primary output" @@ -338,7 +340,8 @@ then tone_alarm $TUNE_OUT_ERROR fi - fi + fi + if [ $OUTPUT_MODE == hil ] then echo "[init] Use HIL as primary output" @@ -396,34 +399,29 @@ then # # MAVLink # - if [ $MAVLINK_FLAGS == default ] then if [ $HIL == yes ] then sleep 1 set MAVLINK_FLAGS "-r 10000 -d /dev/ttyACM0" - usleep 5000 else # Normal mode, use baudrate 57600 (default) and data rate 1000 bytes/s if [ $TTYS1_BUSY == yes ] then # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else set MAVLINK_FLAGS "-r 1000 -d /dev/ttyS0" - usleep 5000 # Exit from nsh to free port for mavlink set EXIT_ON_END yes else # Start MAVLink on default port: ttyS1 set MAVLINK_FLAGS "-r 1000" - usleep 5000 fi fi fi mavlink start $MAVLINK_FLAGS - usleep 5000 # # Start the datamanager From 848c8364317d5db5d2accbeb788dfc67b5e02efa Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Apr 2014 08:53:47 +0200 Subject: [PATCH 290/381] Robustify SF02/F parsing, adjust health checks and startup routine to known initialization time of the sensor --- src/drivers/sf0x/sf0x.cpp | 131 ++++++++++++++++++++++++-------------- 1 file changed, 83 insertions(+), 48 deletions(-) diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 70cd1ab1e5..07163035b5 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -276,34 +276,11 @@ SF0X::init() warnx("advert err"); } - /* attempt to get a measurement 5 times */ - while (ret != OK && i < 5) { - - if (measure()) { - ret = ERROR; - _sensor_ok = false; - } - - usleep(100000); - - if (collect()) { - ret = ERROR; - _sensor_ok = false; - - } else { - ret = OK; - /* sensor is ok, but we don't really know if it is within range */ - _sensor_ok = true; - } - - i++; - } - /* close the fd */ ::close(_fd); _fd = -1; out: - return ret; + return OK; } int @@ -376,6 +353,7 @@ SF0X::ioctl(struct file *filp, int cmd, unsigned long arg) /* adjust to a legal polling interval in Hz */ default: { + /* do we need to start internal polling? */ bool want_start = (_measure_ticks == 0); @@ -544,10 +522,16 @@ SF0X::collect() 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 */ + unsigned readlen = sizeof(_linebuf) - 1; + /* read from the sensor (uart buffer) */ - ret = ::read(_fd, &_linebuf[_linebuf_index], sizeof(_linebuf) - _linebuf_index); + ret = ::read(_fd, &_linebuf[_linebuf_index], readlen - _linebuf_index); if (ret < 0) { _linebuf[sizeof(_linebuf) - 1] = '\0'; @@ -562,19 +546,30 @@ SF0X::collect() } else { return -EAGAIN; } + } else if (ret == 0) { + return -EAGAIN; } - _linebuf_index += ret; - - if (_linebuf_index >= sizeof(_linebuf)) { - _linebuf_index = 0; - } + /* we did increment the index to the next position already, so just add the additional fields */ + _linebuf_index += (ret - 1); _last_read = hrt_absolute_time(); - if (_linebuf[_linebuf_index - 2] != '\r' || _linebuf[_linebuf_index - 1] != '\n') { - /* incomplete read, reschedule ourselves */ + if (_linebuf_index < 1) { + /* we need at least the two end bytes to make sense of this string */ return -EAGAIN; + + } else if (_linebuf[_linebuf_index - 1] != '\r' || _linebuf[_linebuf_index] != '\n') { + + if (_linebuf_index >= readlen - 1) { + /* 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; @@ -582,22 +577,56 @@ SF0X::collect() bool valid; /* enforce line ending */ - _linebuf[sizeof(_linebuf) - 1] = '\0'; + unsigned lend = (_linebuf_index < (sizeof(_linebuf) - 1)) ? _linebuf_index : (sizeof(_linebuf) - 1); + + _linebuf[lend] = '\0'; if (_linebuf[0] == '-' && _linebuf[1] == '-' && _linebuf[2] == '.') { si_units = -1.0f; valid = false; } else { - si_units = strtod(_linebuf, &end); - valid = true; + + /* 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 (int i = 0; i < (lend - 2); i++) { + if (_linebuf[i] == '\n') { + char buf[sizeof(_linebuf)]; + memcpy(buf, &_linebuf[i+1], (lend + 1) - (i + 1)); + memcpy(_linebuf, buf, (lend + 1) - (i + 1)); + } + + if (_linebuf[i] == '.') { + valid = true; + } + } + + if (valid) { + si_units = strtod(_linebuf, &end); + + /* we require at least 3 characters for a valid number */ + if (end > _linebuf + 3) { + valid = true; + } else { + si_units = -1.0f; + valid = false; + } + } } - debug("val (float): %8.4f, raw: %s\n", si_units, _linebuf); + debug("val (float): %8.4f, raw: %s, valid: %s\n", si_units, _linebuf, ((valid) ? "OK" : "NO")); - /* done with this chunk, resetting */ + /* done with this chunk, resetting - even if invalid */ _linebuf_index = 0; + /* if its invalid, there is no reason to forward the value */ + if (!valid) { + perf_count(_comms_errors); + return -EINVAL; + } + struct range_finder_report report; /* this should be fairly close to the end of the measurement, so the best approximation of the time */ @@ -689,7 +718,11 @@ SF0X::cycle() } if (OK != collect_ret) { - log("collection error"); + + /* we know the sensor needs about four seconds to initialize */ + if (hrt_absolute_time() > 5 * 1000 * 1000LL) { + log("collection error"); + } /* restart the measurement state machine */ start(); return; @@ -848,10 +881,10 @@ test() } warnx("single read"); - warnx("measurement: %0.2f m", (double)report.distance); - warnx("time: %lld", report.timestamp); + warnx("val: %0.2f m", (double)report.distance); + warnx("time: %lld", report.timestamp); - /* start the sensor polling at 2Hz */ + /* start the sensor polling at 2 Hz rate */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { errx(1, "failed to set 2Hz poll rate"); } @@ -866,24 +899,26 @@ test() int ret = poll(&fds, 1, 2000); if (ret != 1) { - errx(1, "timed out waiting for sensor data"); + warnx("timed out"); + break; } /* now go get it */ sz = read(fd, &report, sizeof(report)); if (sz != sizeof(report)) { - err(1, "periodic read failed"); + warnx("read failed: got %d vs exp. %d", sz, sizeof(report)); + break; } - warnx("periodic read %u", i); - warnx("measurement: %0.3f", (double)report.distance); - warnx("time: %lld", report.timestamp); + warnx("read #%u", i); + warnx("val: %0.3f m", (double)report.distance); + warnx("time: %lld", report.timestamp); } - /* reset the sensor polling to default rate */ + /* reset the sensor polling to the default rate */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { - errx(1, "failed to set default poll rate"); + errx(1, "ERR: DEF RATE"); } errx(0, "PASS"); From 0ed4dd6577dc842ef151afd7751bd61744e2c77f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Apr 2014 16:16:24 +0200 Subject: [PATCH 291/381] Fixed log format --- src/modules/sdlog2/sdlog2.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 3f07eea533..13981ac544 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1258,7 +1258,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_ESTM.states_nan = buf.estimator_status.states_nan; log_msg.body.log_ESTM.covariance_nan = buf.estimator_status.covariance_nan; log_msg.body.log_ESTM.kalman_gain_nan = buf.estimator_status.kalman_gain_nan; - LOGBUFFER_WRITE_AND_COUNT(DIST); + LOGBUFFER_WRITE_AND_COUNT(ESTM); } /* signal the other thread new data, but not yet unlock */ From fc757f9492a9495516e37abd6efc94c58904f1f0 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 2 Apr 2014 15:38:49 +0400 Subject: [PATCH 292/381] mavlink: is_published() fix --- src/modules/mavlink/mavlink_orb_subscription.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp index 4de7228324..d432edd2b4 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.cpp +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -88,6 +88,7 @@ MavlinkOrbSubscription::update(const hrt_abstime t) if (_updated) { orb_copy(_topic, _fd, _data); + _published = true; return true; } } From ed7b97c0203fd1a16768222cad6c6f49b0534bd2 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 3 Apr 2014 21:13:03 +0200 Subject: [PATCH 293/381] commander: don't beep if message is not understood --- src/modules/commander/commander.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index cf7ba757e2..58995fcec3 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -592,11 +592,11 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe answer_command(*cmd, VEHICLE_CMD_RESULT_UNSUPPORTED); break; } - - if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) { - /* already warned about unsupported commands in "default" case */ - answer_command(*cmd, result); - } + /* silently ignore unsupported commands, maybe they are passed on over mavlink */ +// if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) { +// /* already warned about unsupported commands in "default" case */ +// answer_command(*cmd, result); +// } /* send any requested ACKs */ if (cmd->confirmation > 0 && result != VEHICLE_CMD_RESULT_UNSUPPORTED) { From f17c0b133559dc440a7789caa45767b95de84e7b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 3 Apr 2014 21:15:47 +0200 Subject: [PATCH 294/381] mavlink: implemented multicasting between mavlink instances (two options: forwarding: forward received messages from self to other mavlink instances, passing: send out messages received from other mavlink intances over serial --- src/modules/mavlink/mavlink_main.cpp | 197 ++++++++++++++++++++++- src/modules/mavlink/mavlink_main.h | 34 +++- src/modules/mavlink/mavlink_messages.cpp | 6 +- src/modules/mavlink/mavlink_receiver.cpp | 5 + 4 files changed, 236 insertions(+), 6 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 18df577fea..c5055939ec 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -207,10 +207,15 @@ Mavlink::Mavlink() : _subscriptions(nullptr), _streams(nullptr), _mission_pub(-1), + _verbose(false), + _forwarding_on(false), + _passing_on(false), + _uart_fd(-1), _mavlink_param_queue_index(0), _subscribe_to_stream(nullptr), _subscribe_to_stream_rate(0.0f), _flow_control_enabled(true), + _message_buffer({}), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mavlink")) @@ -261,7 +266,6 @@ Mavlink::Mavlink() : errx(1, "instance ID is out of range"); break; } - } Mavlink::~Mavlink() @@ -394,6 +398,20 @@ Mavlink::instance_exists(const char *device_name, Mavlink *self) return false; } +void +Mavlink::forward_message(mavlink_message_t *msg, Mavlink *self) +{ + Mavlink *inst = ::_mavlink_instances; + + while (inst != nullptr) { + /* don't broadcast to itself */ + if (inst != self) { + inst->pass_message(msg); + } + inst = inst->next; + } +} + int Mavlink::get_uart_fd(unsigned index) { @@ -1616,6 +1634,125 @@ Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate) } } +int +Mavlink::message_buffer_init(int size) +{ + _message_buffer.size = size; + _message_buffer.write_ptr = 0; + _message_buffer.read_ptr = 0; + _message_buffer.data = (char*)malloc(_message_buffer.size); + return (_message_buffer.data == 0) ? ERROR : OK; +} + +void +Mavlink::message_buffer_destroy() +{ + _message_buffer.size = 0; + _message_buffer.write_ptr = 0; + _message_buffer.read_ptr = 0; + free(_message_buffer.data); +} + +int +Mavlink::message_buffer_count() +{ + int n = _message_buffer.write_ptr - _message_buffer.read_ptr; + + if (n < 0) { + n += _message_buffer.size; + } + + return n; +} + +int +Mavlink::message_buffer_is_empty() +{ + return _message_buffer.read_ptr == _message_buffer.write_ptr; +} + + +bool +Mavlink::message_buffer_write(void *ptr, int size) +{ + // bytes available to write + int available = _message_buffer.read_ptr - _message_buffer.write_ptr - 1; + + if (available < 0) { + available += _message_buffer.size; + } + + if (size > available) { + // buffer overflow + return false; + } + + char *c = (char *) ptr; + int n = _message_buffer.size - _message_buffer.write_ptr; // bytes to end of the buffer + + if (n < size) { + // message goes over end of the buffer + memcpy(&(_message_buffer.data[_message_buffer.write_ptr]), c, n); + _message_buffer.write_ptr = 0; + + } else { + n = 0; + } + + // now: n = bytes already written + int p = size - n; // number of bytes to write + memcpy(&(_message_buffer.data[_message_buffer.write_ptr]), &(c[n]), p); + _message_buffer.write_ptr = (_message_buffer.write_ptr + p) % _message_buffer.size; + return true; +} + +int +Mavlink::message_buffer_get_ptr(void **ptr, bool *is_part) +{ + // bytes available to read + int available = _message_buffer.write_ptr - _message_buffer.read_ptr; + + if (available == 0) { + return 0; // buffer is empty + } + + int n = 0; + + if (available > 0) { + // read pointer is before write pointer, all available bytes can be read + n = available; + *is_part = false; + + } else { + // read pointer is after write pointer, read bytes from read_ptr to end of the buffer + n = _message_buffer.size - _message_buffer.read_ptr; + *is_part = _message_buffer.write_ptr > 0; + } + + *ptr = &(_message_buffer.data[_message_buffer.read_ptr]); + return n; +} + +void +Mavlink::message_buffer_mark_read(int n) +{ + _message_buffer.read_ptr = (_message_buffer.read_ptr + n) % _message_buffer.size; +} + +void +Mavlink::pass_message(mavlink_message_t *msg) +{ + if (_passing_on) { + /* size is 8 bytes plus variable payload */ + int size = MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len; + pthread_mutex_lock(&_message_buffer_mutex); + message_buffer_write(msg, size); + pthread_mutex_unlock(&_message_buffer_mutex); + } +} + + + int Mavlink::task_main(int argc, char *argv[]) { @@ -1632,7 +1769,7 @@ Mavlink::task_main(int argc, char *argv[]) * set error flag instead */ bool err_flag = false; - while ((ch = getopt(argc, argv, "b:r:d:m:v")) != EOF) { + while ((ch = getopt(argc, argv, "b:r:d:m:fpv")) != EOF) { switch (ch) { case 'b': _baudrate = strtoul(optarg, NULL, 10); @@ -1672,6 +1809,14 @@ Mavlink::task_main(int argc, char *argv[]) break; + case 'f': + _forwarding_on = true; + break; + + case 'p': + _passing_on = true; + break; + case 'v': _verbose = true; break; @@ -1740,6 +1885,17 @@ Mavlink::task_main(int argc, char *argv[]) /* initialize mavlink text message buffering */ mavlink_logbuffer_init(&_logbuffer, 5); + /* if we are passing on mavlink messages, we need to prepare a buffer for this instance */ + if (_passing_on) { + /* initialize message buffer if multiplexing is on */ + if (OK != message_buffer_init(500)) { + errx(1, "can't allocate message buffer, exiting"); + } + + /* initialize message buffer mutex */ + pthread_mutex_init(&_message_buffer_mutex, NULL); + } + /* create the device node that's used for sending text log messages, etc. */ register_driver(MAVLINK_LOG_DEVICE, &fops, 0666, NULL); @@ -1884,6 +2040,37 @@ Mavlink::task_main(int argc, char *argv[]) } } + /* pass messages from other UARTs */ + if (_passing_on) { + + bool is_part; + void *read_ptr; + + /* guard get ptr by mutex */ + pthread_mutex_lock(&_message_buffer_mutex); + int available = message_buffer_get_ptr(&read_ptr, &is_part); + pthread_mutex_unlock(&_message_buffer_mutex); + + if (available > 0) { + /* write first part of buffer */ + _mavlink_resend_uart(_channel, (const mavlink_message_t*)read_ptr); + message_buffer_mark_read(available); + + /* write second part of buffer if there is some */ + if (is_part) { + /* guard get ptr by mutex */ + pthread_mutex_lock(&_message_buffer_mutex); + available = message_buffer_get_ptr(&read_ptr, &is_part); + pthread_mutex_unlock(&_message_buffer_mutex); + + _mavlink_resend_uart(_channel, (const mavlink_message_t*)read_ptr); + message_buffer_mark_read(available); + } + } + } + + + perf_end(_loop_perf); } @@ -1928,6 +2115,10 @@ Mavlink::task_main(int argc, char *argv[]) /* close mavlink logging device */ close(_mavlink_fd); + if (_passing_on) { + message_buffer_destroy(); + pthread_mutex_destroy(&_message_buffer_mutex); + } /* destroy log buffer */ mavlink_logbuffer_destroy(&_logbuffer); @@ -2067,7 +2258,7 @@ Mavlink::stream(int argc, char *argv[]) static void usage() { - warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate] [-r rate] [-m mode] [-s stream] [-v]"); + warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate] [-r rate] [-m mode] [-s stream] [-f] [-p] [-v]"); } int mavlink_main(int argc, char *argv[]) diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 5a118a0ad9..4f9a53a5ba 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -138,6 +138,8 @@ public: static bool instance_exists(const char *device_name, Mavlink *self); + static void forward_message(mavlink_message_t *msg, Mavlink *self); + static int get_uart_fd(unsigned index); int get_uart_fd(); @@ -153,10 +155,12 @@ public: void set_mode(enum MAVLINK_MODE); enum MAVLINK_MODE get_mode() { return _mode; } - bool get_hil_enabled() { return _hil_enabled; }; + bool get_hil_enabled() { return _hil_enabled; } bool get_flow_control_enabled() { return _flow_control_enabled; } + bool get_forwarding_on() { return _forwarding_on; } + /** * Handle waypoint related messages. */ @@ -234,6 +238,8 @@ private: mavlink_wpm_storage *_wpm; bool _verbose; + bool _forwarding_on; + bool _passing_on; int _uart_fd; int _baudrate; int _datarate; @@ -252,6 +258,16 @@ private: bool _flow_control_enabled; + struct mavlink_message_buffer { + int write_ptr; + int read_ptr; + int size; + char *data; + }; + mavlink_message_buffer _message_buffer; + + pthread_mutex_t _message_buffer_mutex; + /** * Send one parameter. * @@ -315,6 +331,22 @@ private: int configure_stream(const char *stream_name, const float rate); void configure_stream_threadsafe(const char *stream_name, const float rate); + int message_buffer_init(int size); + + void message_buffer_destroy(); + + int message_buffer_count(); + + int message_buffer_is_empty(); + + bool message_buffer_write(void *ptr, int size); + + int message_buffer_get_ptr(void **ptr, bool *is_part); + + void message_buffer_mark_read(int n); + + void pass_message(mavlink_message_t *msg); + static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); /** diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 4ca3840d47..2b5d650807 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1265,11 +1265,13 @@ protected: || status->arming_state == ARMING_STATE_ARMED_ERROR) { /* send camera capture on */ - mavlink_msg_command_long_send(_channel, 42, 30, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); + /* XXX TODO: get param for system ID */ + mavlink_msg_command_long_send(_channel, 42, 250, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); } else { /* send camera capture off */ - mavlink_msg_command_long_send(_channel, 42, 30, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0); + /* XXX TODO: get param for system ID */ + mavlink_msg_command_long_send(_channel, 42, 250, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0); } } }; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index d7e300670a..1581f30d37 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -913,6 +913,11 @@ MavlinkReceiver::receive_thread(void *arg) /* handle packet with parameter component */ _mavlink->mavlink_pm_message_handler(_mavlink->get_channel(), &msg); + + if (_mavlink->get_forwarding_on()) { + /* forward any messages to other mavlink instances */ + Mavlink::forward_message(&msg, _mavlink); + } } } } From e075d05f579091fb9c605c856650cbfd1587a044 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 4 Apr 2014 18:05:13 +0200 Subject: [PATCH 295/381] Move Pauls EKF into a class and instantiate only when / if needed. Checking for low memory conditions as we should. --- .../fw_att_pos_estimator/estimator.cpp | 189 +++------ src/modules/fw_att_pos_estimator/estimator.h | 237 +++++++---- .../fw_att_pos_estimator_main.cpp | 382 +++++++++--------- 3 files changed, 418 insertions(+), 390 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index 46e405526e..3ce1ce56ec 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -2,85 +2,6 @@ #include -// Global variables -float KH[n_states][n_states]; // intermediate result used for covariance updates -float KHP[n_states][n_states]; // intermediate result used for covariance updates -float P[n_states][n_states]; // covariance matrix -float Kfusion[n_states]; // Kalman gains -float states[n_states]; // state matrix -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) -Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad) -Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s) -float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2) -Vector3f earthRateNED; // earths angular rate vector in NED (rad/s) -Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s) -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) -uint8_t fusionModeGPS = 0; // 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 - -float velNED[3]; // North, East, Down velocity obs (m/s) -float posNE[2]; // North, East position obs (m) -float hgtMea; // measured height (m) -float posNED[3]; // North, East Down position (m) - -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 -float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time -float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time - -float innovMag[3]; // innovation output -float varInnovMag[3]; // innovation variance output -Vector3f magData; // magnetometer flux radings in X,Y,Z body axes -float innovVtas; // innovation output -float varInnovVtas; // innovation variance output -float VtasMeas; // true airspeed measurement (m/s) -float latRef; // WGS-84 latitude of reference point (rad) -float lonRef; // WGS-84 longitude of reference point (rad) -float hgtRef; // WGS-84 height of reference point (m) -Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes -uint8_t covSkipCount = 0; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction -float EAS2TAS = 1.0f; // ratio f true to equivalent airspeed - -// GPS input data variables -float gpsCourse; -float gpsVelD; -float gpsLat; -float gpsLon; -float gpsHgt; -uint8_t GPSstatus; - -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 - -// Baro input -float baroHgt; - -bool statesInitialised = false; - -bool fuseVelData = false; // this boolean causes the posNE and velNED obs to be fused -bool fusePosData = false; // this boolean causes the posNE and velNED obs to be fused -bool fuseHgtData = false; // this boolean causes the hgtMea obs to be fused -bool fuseMagData = false; // boolean true when magnetometer data is to be fused -bool fuseVtasData = false; // boolean true when airspeed data is to be fused - -bool onGround = true; ///< boolean true when the flight vehicle is on the ground (not flying) -bool staticMode = true; ///< boolean true if no position feedback is fused -bool useAirspeed = true; ///< boolean true if airspeed data is being used -bool useCompass = true; ///< boolean true if magnetometer data is being used - -struct ekf_status_report current_ekf_state; -struct ekf_status_report last_ekf_error; - -bool numericalProtection = true; - -static unsigned storeIndex = 0; - float Vector3f::length(void) const { return sqrt(x*x + y*y + z*z); @@ -185,7 +106,7 @@ void swap_var(float &d1, float &d2) d2 = tmp; } -void UpdateStrapdownEquationsNED() +void AttPosEKF::UpdateStrapdownEquationsNED() { Vector3f delVelNav; float q00; @@ -247,7 +168,7 @@ void UpdateStrapdownEquationsNED() qUpdated[3] = states[0]*deltaQuat[3] + states[3]*deltaQuat[0] + states[1]*deltaQuat[2] - states[2]*deltaQuat[1]; // Normalise the quaternions and update the quaternion states - quatMag = sqrt(sq(qUpdated[0]) + sq(qUpdated[1]) + sq(qUpdated[2]) + sq(qUpdated[3])); + quatMag = sqrtf(sq(qUpdated[0]) + sq(qUpdated[1]) + sq(qUpdated[2]) + sq(qUpdated[3])); if (quatMag > 1e-16f) { float quatMagInv = 1.0f/quatMag; @@ -312,7 +233,7 @@ void UpdateStrapdownEquationsNED() ConstrainStates(); } -void CovariancePrediction(float dt) +void AttPosEKF::CovariancePrediction(float dt) { // scalars float windVelSigma; @@ -953,7 +874,7 @@ void CovariancePrediction(float dt) ConstrainVariances(); } -void FuseVelposNED() +void AttPosEKF::FuseVelposNED() { // declare variables used by fault isolation logic @@ -999,8 +920,8 @@ void FuseVelposNED() observation[5] = -(hgtMea); // Estimate the GPS Velocity, GPS horiz position and height measurement variances. - velErr = 0.2*accNavMag; // additional error in GPS velocities caused by manoeuvring - posErr = 0.2*accNavMag; // additional error in GPS position caused by manoeuvring + 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] = 0.04f + sq(velErr); R_OBS[1] = R_OBS[0]; R_OBS[2] = 0.08f + sq(velErr); @@ -1026,7 +947,7 @@ void FuseVelposNED() varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS[i]; } // apply a 5-sigma threshold - current_ekf_state.velHealth = (sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) < 25.0*(varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2]); + current_ekf_state.velHealth = (sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) < 25.0f * (varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2]); current_ekf_state.velTimeout = (millis() - current_ekf_state.velFailTime) > horizRetryTime; if (current_ekf_state.velHealth || current_ekf_state.velTimeout) { @@ -1175,7 +1096,7 @@ void FuseVelposNED() //printf("velh: %s, posh: %s, hgth: %s\n", ((velHealth) ? "OK" : "FAIL"), ((posHealth) ? "OK" : "FAIL"), ((hgtHealth) ? "OK" : "FAIL")); } -void FuseMagnetometer() +void AttPosEKF::FuseMagnetometer() { uint8_t obsIndex; uint8_t indexLimit; @@ -1482,7 +1403,7 @@ void FuseMagnetometer() ConstrainVariances(); } -void FuseAirspeed() +void AttPosEKF::FuseAirspeed() { float vn; float ve; @@ -1503,14 +1424,14 @@ void FuseAirspeed() // Need to check that it is flying before fusing airspeed data // Calculate the predicted airspeed - VtasPred = sqrt((ve - vwe)*(ve - vwe) + (vn - vwn)*(vn - vwn) + vd*vd); + VtasPred = sqrtf((ve - vwe)*(ve - vwe) + (vn - vwn)*(vn - vwn) + vd*vd); // Perform fusion of True Airspeed measurement - if (useAirspeed && fuseVtasData && (VtasPred > 1.0) && (VtasMeas > 8.0)) + if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > 8.0f)) { // Calculate observation jacobians SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd))); - SH_TAS[1] = (SH_TAS[0]*(2*ve - 2*vwe))/2; - SH_TAS[2] = (SH_TAS[0]*(2*vn - 2*vwn))/2; + SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2*vwe))/2.0f; + SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2*vwn))/2.0f; float H_TAS[21]; for (uint8_t i=0; i<=20; i++) H_TAS[i] = 0.0f; @@ -1611,7 +1532,7 @@ void FuseAirspeed() ConstrainVariances(); } -void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) +void AttPosEKF::zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) { uint8_t row; uint8_t col; @@ -1624,7 +1545,7 @@ 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) +void AttPosEKF::zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) { uint8_t row; uint8_t col; @@ -1637,13 +1558,13 @@ void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) } } -float sq(float valIn) +float AttPosEKF::sq(float valIn) { return valIn*valIn; } // Store states in a history array along with time stamp -void StoreStates(uint64_t timestamp_ms) +void AttPosEKF::StoreStates(uint64_t timestamp_ms) { for (unsigned i=0; i max) ? max : ((val < min) ? min : val); } -void ConstrainVariances() +void AttPosEKF::ConstrainVariances() { if (!numericalProtection) { return; @@ -1914,7 +1835,7 @@ void ConstrainVariances() } -void ConstrainStates() +void AttPosEKF::ConstrainStates() { if (!numericalProtection) { return; @@ -1971,7 +1892,7 @@ void ConstrainStates() } -void ForceSymmetry() +void AttPosEKF::ForceSymmetry() { if (!numericalProtection) { return; @@ -1989,7 +1910,7 @@ void ForceSymmetry() } } -bool FilterHealthy() +bool AttPosEKF::FilterHealthy() { if (!statesInitialised) { return false; @@ -2012,7 +1933,7 @@ bool FilterHealthy() * This resets the position to the last GPS measurement * or to zero in case of static position. */ -void ResetPosition(void) +void AttPosEKF::ResetPosition(void) { if (staticMode) { states[7] = 0; @@ -2030,7 +1951,7 @@ void ResetPosition(void) * * This resets the height state with the last altitude measurements */ -void ResetHeight(void) +void AttPosEKF::ResetHeight(void) { // write to the state vector states[9] = -hgtMea; @@ -2039,7 +1960,7 @@ void ResetHeight(void) /** * Reset the velocity state. */ -void ResetVelocity(void) +void AttPosEKF::ResetVelocity(void) { if (staticMode) { states[4] = 0.0f; @@ -2054,7 +1975,7 @@ void ResetVelocity(void) } -void FillErrorReport(struct ekf_status_report *err) +void AttPosEKF::FillErrorReport(struct ekf_status_report *err) { for (int i = 0; i < n_states; i++) { @@ -2069,7 +1990,7 @@ void FillErrorReport(struct ekf_status_report *err) err->hgtTimeout = current_ekf_state.hgtTimeout; } -bool StatesNaN(struct ekf_status_report *err_report) { +bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { bool err = false; // check all states and covariance matrices @@ -2122,7 +2043,7 @@ bool StatesNaN(struct ekf_status_report *err_report) { * updated, but before any of the fusion steps are * executed. */ -int CheckAndBound() +int AttPosEKF::CheckAndBound() { // Store the old filter state @@ -2164,7 +2085,7 @@ int CheckAndBound() return 0; } -void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat) +void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat) { float initialRoll, initialPitch; float cosRoll, sinRoll, cosPitch, sinPitch; @@ -2200,7 +2121,7 @@ void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, fl initQuat[3] = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading; } -void InitializeDynamic(float (&initvelNED)[3]) +void AttPosEKF::InitializeDynamic(float (&initvelNED)[3]) { // Clear the init flag @@ -2254,7 +2175,7 @@ void InitializeDynamic(float (&initvelNED)[3]) summedDelVel.z = 0.0f; } -void InitialiseFilter(float (&initvelNED)[3]) +void AttPosEKF::InitialiseFilter(float (&initvelNED)[3]) { //store initial lat,long and height latRef = gpsLat; @@ -2266,7 +2187,7 @@ void InitialiseFilter(float (&initvelNED)[3]) InitializeDynamic(initvelNED); } -void ZeroVariables() +void AttPosEKF::ZeroVariables() { // Do the data structure init for (unsigned i = 0; i < n_states; i++) { @@ -2292,12 +2213,12 @@ void ZeroVariables() memset(¤t_ekf_state, 0, sizeof(current_ekf_state)); } -void GetFilterState(struct ekf_status_report *state) +void AttPosEKF::GetFilterState(struct ekf_status_report *state) { memcpy(state, ¤t_ekf_state, sizeof(state)); } -void GetLastErrorState(struct ekf_status_report *last_error) +void AttPosEKF::GetLastErrorState(struct ekf_status_report *last_error) { memcpy(last_error, &last_ekf_error, sizeof(last_error)); } diff --git a/src/modules/fw_att_pos_estimator/estimator.h b/src/modules/fw_att_pos_estimator/estimator.h index c5a5e9d8d9..f43f4931af 100644 --- a/src/modules/fw_att_pos_estimator/estimator.h +++ b/src/modules/fw_att_pos_estimator/estimator.h @@ -48,85 +48,85 @@ void swap_var(float &d1, float &d2); const unsigned int n_states = 21; const unsigned int data_buffer_size = 50; -extern uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored -extern Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad) -extern Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s) -extern Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad) -extern Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s) -extern float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2) -extern Vector3f earthRateNED; // earths angular rate vector in NED (rad/s) -extern Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s) -extern Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2) -extern Vector3f dVelIMU; -extern Vector3f dAngIMU; +// extern uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored +// extern Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad) +// extern Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s) +// extern Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad) +// extern Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s) +// extern float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2) +// extern Vector3f earthRateNED; // earths angular rate vector in NED (rad/s) +// extern Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s) +// extern Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2) +// extern Vector3f dVelIMU; +// extern Vector3f dAngIMU; -extern float P[n_states][n_states]; // covariance matrix -extern float Kfusion[n_states]; // Kalman gains -extern float states[n_states]; // state matrix -extern float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps +// extern float P[n_states][n_states]; // covariance matrix +// extern float Kfusion[n_states]; // Kalman gains +// extern float states[n_states]; // state matrix +// extern float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps -extern Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad) -extern Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s) -extern Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad) -extern Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s) +// extern Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad) +// extern Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s) +// extern Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad) +// extern Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s) -extern float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec) +// extern float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec) -extern bool onGround; // boolean true when the flight vehicle is on the ground (not flying) -extern bool useAirspeed; // boolean true if airspeed data is being used -extern bool useCompass; // boolean true if magnetometer data is being used -extern uint8_t fusionModeGPS ; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity -extern float innovVelPos[6]; // innovation output -extern float varInnovVelPos[6]; // innovation variance output +// extern bool onGround; // boolean true when the flight vehicle is on the ground (not flying) +// extern bool useAirspeed; // boolean true if airspeed data is being used +// extern bool useCompass; // boolean true if magnetometer data is being used +// extern uint8_t fusionModeGPS ; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity +// extern float innovVelPos[6]; // innovation output +// extern float varInnovVelPos[6]; // innovation variance output -extern bool fuseVelData; // this boolean causes the posNE and velNED obs to be fused -extern bool fusePosData; // this boolean causes the posNE and velNED obs to be fused -extern bool fuseHgtData; // this boolean causes the hgtMea obs to be fused -extern bool fuseMagData; // boolean true when magnetometer data is to be fused +// extern bool fuseVelData; // this boolean causes the posNE and velNED obs to be fused +// extern bool fusePosData; // this boolean causes the posNE and velNED obs to be fused +// extern bool fuseHgtData; // this boolean causes the hgtMea obs to be fused +// extern bool fuseMagData; // boolean true when magnetometer data is to be fused -extern float velNED[3]; // North, East, Down velocity obs (m/s) -extern float posNE[2]; // North, East position obs (m) -extern float hgtMea; // measured height (m) -extern float posNED[3]; // North, East Down position (m) +// extern float velNED[3]; // North, East, Down velocity obs (m/s) +// extern float posNE[2]; // North, East position obs (m) +// extern float hgtMea; // measured height (m) +// extern float posNED[3]; // North, East Down position (m) -extern float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements -extern float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements -extern float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement +// extern float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements +// extern float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements +// extern float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement -extern float innovMag[3]; // innovation output -extern float varInnovMag[3]; // innovation variance output -extern Vector3f magData; // magnetometer flux radings in X,Y,Z body axes -extern float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time -extern float innovVtas; // innovation output -extern float varInnovVtas; // innovation variance output -extern bool fuseVtasData; // boolean true when airspeed data is to be fused -extern float VtasMeas; // true airspeed measurement (m/s) -extern float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time -extern float latRef; // WGS-84 latitude of reference point (rad) -extern float lonRef; // WGS-84 longitude of reference point (rad) -extern float hgtRef; // WGS-84 height of reference point (m) -extern Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes -extern uint8_t covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction -extern float EAS2TAS; // ratio f true to equivalent airspeed +// extern float innovMag[3]; // innovation output +// extern float varInnovMag[3]; // innovation variance output +// extern Vector3f magData; // magnetometer flux radings in X,Y,Z body axes +// extern float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time +// extern float innovVtas; // innovation output +// extern float varInnovVtas; // innovation variance output +// extern bool fuseVtasData; // boolean true when airspeed data is to be fused +// extern float VtasMeas; // true airspeed measurement (m/s) +// extern float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time +// extern float latRef; // WGS-84 latitude of reference point (rad) +// extern float lonRef; // WGS-84 longitude of reference point (rad) +// extern float hgtRef; // WGS-84 height of reference point (m) +// extern Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes +// extern uint8_t covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction +// extern float EAS2TAS; // ratio f true to equivalent airspeed -// GPS input data variables -extern float gpsCourse; -extern float gpsVelD; -extern float gpsLat; -extern float gpsLon; -extern float gpsHgt; -extern uint8_t GPSstatus; +// // GPS input data variables +// extern float gpsCourse; +// extern float gpsVelD; +// extern float gpsLat; +// extern float gpsLon; +// extern float gpsHgt; +// extern uint8_t GPSstatus; -// Baro input -extern float baroHgt; +// // Baro input +// extern float baroHgt; -extern bool statesInitialised; -extern bool numericalProtection; +// extern bool statesInitialised; +// extern bool numericalProtection; const float covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions const float covDelAngMax = 0.02f; // maximum delta angle between covariance predictions -extern bool staticMode; +// extern bool staticMode; enum GPS_FIX { GPS_FIX_NOFIX = 0, @@ -150,6 +150,89 @@ struct ekf_status_report { bool kalmanGainsNaN; }; +class AttPosEKF { + +public: + // Global variables + float KH[n_states][n_states]; // intermediate result used for covariance updates + float KHP[n_states][n_states]; // intermediate result used for covariance updates + float P[n_states][n_states]; // covariance matrix + float Kfusion[n_states]; // Kalman gains + float states[n_states]; // state matrix + 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 + + 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 + float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time + float statesAtVtasMeasTime[n_states]; // filter states at the effective 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) + Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad) + Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s) + float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2) + Vector3f earthRateNED; // earths angular rate vector in NED (rad/s) + Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s) + 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) + uint8_t fusionModeGPS = 0; // 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 + + float velNED[3]; // North, East, Down velocity obs (m/s) + float posNE[2]; // North, East position obs (m) + float hgtMea; // measured height (m) + float posNED[3]; // North, East Down position (m) + + float innovMag[3]; // innovation output + float varInnovMag[3]; // innovation variance output + Vector3f magData; // magnetometer flux radings in X,Y,Z body axes + float innovVtas; // innovation output + float varInnovVtas; // innovation variance output + float VtasMeas; // true airspeed measurement (m/s) + float latRef; // WGS-84 latitude of reference point (rad) + float lonRef; // WGS-84 longitude of reference point (rad) + float hgtRef; // WGS-84 height of reference point (m) + Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes + uint8_t covSkipCount = 0; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction + float EAS2TAS = 1.0f; // ratio f true to equivalent airspeed + + // GPS input data variables + float gpsCourse; + float gpsVelD; + float gpsLat; + float gpsLon; + float gpsHgt; + uint8_t GPSstatus; + + // Baro input + float baroHgt; + + bool statesInitialised = false; + + bool fuseVelData = false; // this boolean causes the posNE and velNED obs to be fused + bool fusePosData = false; // this boolean causes the posNE and velNED obs to be fused + bool fuseHgtData = false; // this boolean causes the hgtMea obs to be fused + bool fuseMagData = false; // boolean true when magnetometer data is to be fused + bool fuseVtasData = false; // boolean true when airspeed data is to be fused + + bool onGround = true; ///< boolean true when the flight vehicle is on the ground (not flying) + bool staticMode = true; ///< boolean true if no position feedback is fused + bool useAirspeed = true; ///< boolean true if airspeed data is being used + bool useCompass = true; ///< boolean true if magnetometer data is being used + + struct ekf_status_report current_ekf_state; + struct ekf_status_report last_ekf_error; + + bool numericalProtection = true; + + unsigned storeIndex = 0; + + void UpdateStrapdownEquationsNED(); void CovariancePrediction(float dt); @@ -164,8 +247,6 @@ 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); -float sq(float valIn); - void quatNorm(float (&quatOut)[4], const float quatIn[4]); // store staes along with system time stamp in msces @@ -190,15 +271,19 @@ void quat2Tbn(Mat3f &Tbn, const float (&quat)[4]); void calcEarthRateNED(Vector3f &omega, float latitude); -void eul2quat(float (&quat)[4], const float (&eul)[3]); +static void eul2quat(float (&quat)[4], const float (&eul)[3]); -void quat2eul(float (&eul)[3], const float (&quat)[4]); +static void quat2eul(float (&eul)[3], const float (&quat)[4]); -void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD); +static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD); -void calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); +static void calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); -void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); +static void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); + +static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]); + +static float sq(float valIn); void OnGroundCheck(); @@ -231,5 +316,15 @@ void FillErrorReport(struct ekf_status_report *err); void InitializeDynamic(float (&initvelNED)[3]); +protected: + +bool FilterHealthy(); + +void ResetHeight(void); + +void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat); + +}; + uint32_t millis(); diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index c9d75bce49..20c5d37190 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -124,6 +124,16 @@ public: */ int start(); + /** + * Print the current status. + */ + void print_status(); + + /** + * Trip the filter by feeding it NaN values. + */ + int trip_nan(); + private: bool _task_should_exit; /**< if true, sensor task should exit */ @@ -199,6 +209,7 @@ private: param_t tas_delay_ms; } _parameter_handles; /**< handles for interesting parameters */ + AttPosEKF *_ekf; /** * Update our local parameter cache. @@ -280,7 +291,8 @@ FixedwingEstimator::FixedwingEstimator() : /* states */ _initialized(false), _gps_initialized(false), - _mavlink_fd(-1) + _mavlink_fd(-1), + _ekf(new AttPosEKF()) { _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); @@ -384,6 +396,10 @@ void FixedwingEstimator::task_main() { + if (!_ekf) { + errx(1, "failed allocating EKF filter - out of RAM!"); + } + /* * do subscriptions */ @@ -414,23 +430,23 @@ FixedwingEstimator::task_main() parameters_update(); /* set initial filter state */ - fuseVelData = false; - fusePosData = false; - fuseHgtData = false; - fuseMagData = false; - fuseVtasData = false; - statesInitialised = false; + _ekf->fuseVelData = false; + _ekf->fusePosData = false; + _ekf->fuseHgtData = false; + _ekf->fuseMagData = false; + _ekf->fuseVtasData = false; + _ekf->statesInitialised = false; /* initialize measurement data */ - VtasMeas = 0.0f; + _ekf->VtasMeas = 0.0f; Vector3f lastAngRate = {0.0f, 0.0f, 0.0f}; Vector3f lastAccel = {0.0f, 0.0f, -9.81f}; - dVelIMU.x = 0.0f; - dVelIMU.y = 0.0f; - dVelIMU.z = 0.0f; - dAngIMU.x = 0.0f; - dAngIMU.y = 0.0f; - dAngIMU.z = 0.0f; + _ekf->dVelIMU.x = 0.0f; + _ekf->dVelIMU.y = 0.0f; + _ekf->dVelIMU.z = 0.0f; + _ekf->dAngIMU.x = 0.0f; + _ekf->dAngIMU.y = 0.0f; + _ekf->dAngIMU.z = 0.0f; /* wakeup source(s) */ struct pollfd fds[2]; @@ -509,7 +525,7 @@ FixedwingEstimator::task_main() } last_sensor_timestamp = _gyro.timestamp; - IMUmsec = _gyro.timestamp / 1e3f; + _ekf.IMUmsec = _gyro.timestamp / 1e3f; float deltaT = (_gyro.timestamp - last_run) / 1e6f; last_run = _gyro.timestamp; @@ -521,20 +537,20 @@ FixedwingEstimator::task_main() // Always store data, independent of init status /* fill in last data set */ - dtIMU = deltaT; + _ekf->dtIMU = deltaT; - angRate.x = _gyro.x; - angRate.y = _gyro.y; - angRate.z = _gyro.z; + _ekf->angRate.x = _gyro.x; + _ekf->angRate.y = _gyro.y; + _ekf->angRate.z = _gyro.z; - accel.x = _accel.x; - accel.y = _accel.y; - accel.z = _accel.z; + _ekf->accel.x = _accel.x; + _ekf->accel.y = _accel.y; + _ekf->accel.z = _accel.z; - dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU; - lastAngRate = angRate; - dVelIMU = 0.5f * (accel + lastAccel) * dtIMU; - lastAccel = accel; + _ekf->dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU; + _ekf->lastAngRate = angRate; + _ekf->dVelIMU = 0.5f * (accel + lastAccel) * dtIMU; + _ekf->lastAccel = accel; #else @@ -563,20 +579,20 @@ FixedwingEstimator::task_main() // Always store data, independent of init status /* fill in last data set */ - dtIMU = deltaT; + _ekf->dtIMU = deltaT; - angRate.x = _sensor_combined.gyro_rad_s[0]; - angRate.y = _sensor_combined.gyro_rad_s[1]; - angRate.z = _sensor_combined.gyro_rad_s[2]; + _ekf->angRate.x = _sensor_combined.gyro_rad_s[0]; + _ekf->angRate.y = _sensor_combined.gyro_rad_s[1]; + _ekf->angRate.z = _sensor_combined.gyro_rad_s[2]; - accel.x = _sensor_combined.accelerometer_m_s2[0]; - accel.y = _sensor_combined.accelerometer_m_s2[1]; - accel.z = _sensor_combined.accelerometer_m_s2[2]; + _ekf->accel.x = _sensor_combined.accelerometer_m_s2[0]; + _ekf->accel.y = _sensor_combined.accelerometer_m_s2[1]; + _ekf->accel.z = _sensor_combined.accelerometer_m_s2[2]; - dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU; - lastAngRate = angRate; - dVelIMU = 0.5f * (accel + lastAccel) * dtIMU; - lastAccel = accel; + _ekf->dAngIMU = 0.5f * (_ekf->angRate + lastAngRate) * _ekf->dtIMU; + lastAngRate = _ekf->angRate; + _ekf->dVelIMU = 0.5f * (_ekf->accel + lastAccel) * _ekf->dtIMU; + lastAccel = _ekf->accel; if (last_mag != _sensor_combined.magnetometer_timestamp) { mag_updated = true; @@ -597,7 +613,7 @@ FixedwingEstimator::task_main() orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); perf_count(_perf_airspeed); - VtasMeas = _airspeed.true_airspeed_m_s; + _ekf->VtasMeas = _airspeed.true_airspeed_m_s; newAdsData = true; } else { @@ -622,24 +638,24 @@ FixedwingEstimator::task_main() /* check if we had a GPS outage for a long time */ if (hrt_elapsed_time(&last_gps) > 5 * 1000 * 1000) { - ResetPosition(); - ResetVelocity(); - ResetStoredStates(); + _ekf->ResetPosition(); + _ekf->ResetVelocity(); + _ekf->ResetStoredStates(); } /* fuse GPS updates */ //_gps.timestamp / 1e3; - GPSstatus = _gps.fix_type; - velNED[0] = _gps.vel_n_m_s; - velNED[1] = _gps.vel_e_m_s; - velNED[2] = _gps.vel_d_m_s; + _ekf->GPSstatus = _gps.fix_type; + _ekf->velNED[0] = _gps.vel_n_m_s; + _ekf->velNED[1] = _gps.vel_e_m_s; + _ekf->velNED[2] = _gps.vel_d_m_s; // warnx("GPS updated: status: %d, vel: %8.4f %8.4f %8.4f", (int)GPSstatus, velNED[0], velNED[1], velNED[2]); - gpsLat = math::radians(_gps.lat / (double)1e7); - gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI; - gpsHgt = _gps.alt / 1e3f; + _ekf->gpsLat = math::radians(_gps.lat / (double)1e7); + _ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI; + _ekf->gpsHgt = _gps.alt / 1e3f; newDataGps = true; } @@ -652,10 +668,10 @@ FixedwingEstimator::task_main() if (baro_updated) { orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); - baroHgt = _baro.altitude - _baro_ref; + _ekf->baroHgt = _baro.altitude - _baro_ref; // Could use a blend of GPS and baro alt data if desired - hgtMea = 1.0f * baroHgt + 0.0f * gpsHgt; + _ekf->hgtMea = 1.0f * _ekf->baroHgt + 0.0f * _ekf->gpsHgt; } #ifndef SENSOR_COMBINED_SUB @@ -671,27 +687,27 @@ FixedwingEstimator::task_main() // XXX we compensate the offsets upfront - should be close to zero. // 0.001f - magData.x = _mag.x; - magBias.x = 0.000001f; // _mag_offsets.x_offset + _ekf->magData.x = _mag.x; + _ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset - magData.y = _mag.y; - magBias.y = 0.000001f; // _mag_offsets.y_offset + _ekf->magData.y = _mag.y; + _ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset - magData.z = _mag.z; - magBias.z = 0.000001f; // _mag_offsets.y_offset + _ekf->magData.z = _mag.z; + _ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset #else // XXX we compensate the offsets upfront - should be close to zero. // 0.001f - magData.x = _sensor_combined.magnetometer_ga[0]; - magBias.x = 0.000001f; // _mag_offsets.x_offset + _ekf->magData.x = _sensor_combined.magnetometer_ga[0]; + _ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset - magData.y = _sensor_combined.magnetometer_ga[1]; - magBias.y = 0.000001f; // _mag_offsets.y_offset + _ekf->magData.y = _sensor_combined.magnetometer_ga[1]; + _ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset - magData.z = _sensor_combined.magnetometer_ga[2]; - magBias.z = 0.000001f; // _mag_offsets.y_offset + _ekf->magData.z = _sensor_combined.magnetometer_ga[2]; + _ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset #endif @@ -705,7 +721,7 @@ FixedwingEstimator::task_main() /** * CHECK IF THE INPUT DATA IS SANE */ - int check = CheckAndBound(); + int check = _ekf->CheckAndBound(); switch (check) { case 0: @@ -739,7 +755,7 @@ FixedwingEstimator::task_main() struct ekf_status_report ekf_report; - GetLastErrorState(&ekf_report); + _ekf->GetLastErrorState(&ekf_report); struct estimator_status_report rep; memset(&rep, 0, sizeof(rep)); @@ -779,16 +795,16 @@ FixedwingEstimator::task_main() if (hrt_elapsed_time(&start_time) > 100000) { - if (!_gps_initialized && (GPSstatus == 3)) { - velNED[0] = _gps.vel_n_m_s; - velNED[1] = _gps.vel_e_m_s; - velNED[2] = _gps.vel_d_m_s; + if (!_gps_initialized && (_ekf->GPSstatus == 3)) { + _ekf->velNED[0] = _gps.vel_n_m_s; + _ekf->velNED[1] = _gps.vel_e_m_s; + _ekf->velNED[2] = _gps.vel_d_m_s; double lat = _gps.lat * 1e-7; double lon = _gps.lon * 1e-7; float alt = _gps.alt * 1e-3; - InitialiseFilter(velNED); + _ekf->InitialiseFilter(_ekf->velNED); // Initialize projection _local_pos.ref_lat = _gps.lat; @@ -799,7 +815,7 @@ FixedwingEstimator::task_main() // Store orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); _baro_ref = _baro.altitude; - baroHgt = _baro.altitude - _baro_ref; + _ekf->baroHgt = _baro.altitude - _baro_ref; _baro_gps_offset = _baro_ref - _local_pos.ref_alt; // XXX this is not multithreading safe @@ -808,24 +824,24 @@ FixedwingEstimator::task_main() _gps_initialized = true; - } else if (!statesInitialised) { - velNED[0] = 0.0f; - velNED[1] = 0.0f; - velNED[2] = 0.0f; - posNED[0] = 0.0f; - posNED[1] = 0.0f; - posNED[2] = 0.0f; + } else if (!_ekf->statesInitialised) { + _ekf->velNED[0] = 0.0f; + _ekf->velNED[1] = 0.0f; + _ekf->velNED[2] = 0.0f; + _ekf->posNED[0] = 0.0f; + _ekf->posNED[1] = 0.0f; + _ekf->posNED[2] = 0.0f; - posNE[0] = posNED[0]; - posNE[1] = posNED[1]; - InitialiseFilter(velNED); + _ekf->posNE[0] = _ekf->posNED[0]; + _ekf->posNE[1] = _ekf->posNED[1]; + _ekf->InitialiseFilter(_ekf->velNED); } } // If valid IMU data and states initialised, predict states and covariances - if (statesInitialised) { + if (_ekf->statesInitialised) { // Run the strapdown INS equations every IMU update - UpdateStrapdownEquationsNED(); + _ekf->UpdateStrapdownEquationsNED(); #if 0 // debug code - could be tunred into a filter mnitoring/watchdog function float tempQuat[4]; @@ -842,20 +858,20 @@ FixedwingEstimator::task_main() #endif // store the predicted states for subsequent use by measurement fusion - StoreStates(IMUmsec); + _ekf->StoreStates(IMUmsec); // Check if on ground - status is used by covariance prediction - OnGroundCheck(); + _ekf->OnGroundCheck(); // sum delta angles and time used by covariance prediction - summedDelAng = summedDelAng + correctedDelAng; - summedDelVel = summedDelVel + dVelIMU; - dt += dtIMU; + _ekf->summedDelAng = _ekf->summedDelAng + _ekf->correctedDelAng; + _ekf->summedDelVel = _ekf->summedDelVel + _ekf->dVelIMU; + dt += _ekf->dtIMU; // perform a covariance prediction if the total delta angle has exceeded the limit // or the time limit will be exceeded at the next IMU update - if ((dt >= (covTimeStepMax - dtIMU)) || (summedDelAng.length() > covDelAngMax)) { - CovariancePrediction(dt); - summedDelAng = summedDelAng.zero(); - summedDelVel = summedDelVel.zero(); + if ((dt >= (covTimeStepMax - _ekf->dtIMU)) || (_ekf->summedDelAng.length() > covDelAngMax)) { + _ekf->CovariancePrediction(dt); + _ekf->summedDelAng = _ekf->summedDelAng.zero(); + _ekf->summedDelVel = _ekf->summedDelVel.zero(); dt = 0.0f; } @@ -865,79 +881,79 @@ FixedwingEstimator::task_main() // Fuse GPS Measurements if (newDataGps && _gps_initialized) { // Convert GPS measurements to Pos NE, hgt and Vel NED - velNED[0] = _gps.vel_n_m_s; - velNED[1] = _gps.vel_e_m_s; - velNED[2] = _gps.vel_d_m_s; - calcposNED(posNED, gpsLat, gpsLon, gpsHgt, latRef, lonRef, hgtRef); + _ekf->velNED[0] = _gps.vel_n_m_s; + _ekf->velNED[1] = _gps.vel_e_m_s; + _ekf->velNED[2] = _gps.vel_d_m_s; + _ekf->calcposNED(_ekf->posNED, _ekf->gpsLat, _ekf->gpsLon, _ekf->gpsHgt, _ekf->latRef, _ekf->lonRef, _ekf->hgtRef); - posNE[0] = posNED[0]; - posNE[1] = posNED[1]; + _ekf->posNE[0] = _ekf->posNED[0]; + _ekf->posNE[1] = _ekf->posNED[1]; // set fusion flags - fuseVelData = true; - fusePosData = true; + _ekf->fuseVelData = true; + _ekf->fusePosData = true; // recall states stored at time of measurement after adjusting for delays - RecallStates(statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); - RecallStates(statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); + _ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); + _ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); // run the fusion step - FuseVelposNED(); + _ekf->FuseVelposNED(); - } else if (statesInitialised) { + } else if (_ekf->statesInitialised) { // Convert GPS measurements to Pos NE, hgt and Vel NED - velNED[0] = 0.0f; - velNED[1] = 0.0f; - velNED[2] = 0.0f; - posNED[0] = 0.0f; - posNED[1] = 0.0f; - posNED[2] = 0.0f; + _ekf->velNED[0] = 0.0f; + _ekf->velNED[1] = 0.0f; + _ekf->velNED[2] = 0.0f; + _ekf->posNED[0] = 0.0f; + _ekf->posNED[1] = 0.0f; + _ekf->posNED[2] = 0.0f; - posNE[0] = posNED[0]; - posNE[1] = posNED[1]; + _ekf->posNE[0] = _ekf->posNED[0]; + _ekf->posNE[1] = _ekf->posNED[1]; // set fusion flags - fuseVelData = true; - fusePosData = true; + _ekf->fuseVelData = true; + _ekf->fusePosData = true; // recall states stored at time of measurement after adjusting for delays - RecallStates(statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); - RecallStates(statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); + _ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); + _ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); // run the fusion step - FuseVelposNED(); + _ekf->FuseVelposNED(); } else { - fuseVelData = false; - fusePosData = false; + _ekf->fuseVelData = false; + _ekf->fusePosData = false; } - if (newAdsData && statesInitialised) { + if (newAdsData && _ekf->statesInitialised) { // Could use a blend of GPS and baro alt data if desired - hgtMea = 1.0f * baroHgt + 0.0f * gpsHgt; - fuseHgtData = true; + _ekf->hgtMea = 1.0f * _ekf->baroHgt + 0.0f * _ekf->gpsHgt; + _ekf->fuseHgtData = true; // recall states stored at time of measurement after adjusting for delays - RecallStates(statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms)); + _ekf->RecallStates(_ekf->statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms)); // run the fusion step - FuseVelposNED(); + _ekf->FuseVelposNED(); } else { - fuseHgtData = false; + _ekf->fuseHgtData = false; } // Fuse Magnetometer Measurements - if (newDataMag && statesInitialised) { - fuseMagData = true; - RecallStates(statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data + if (newDataMag && _ekf->statesInitialised) { + _ekf->fuseMagData = true; + _ekf->RecallStates(_ekf->statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data } else { - fuseMagData = false; + _ekf->fuseMagData = false; } - if (statesInitialised) FuseMagnetometer(); + if (_ekf->statesInitialised) _ekf->FuseMagnetometer(); // Fuse Airspeed Measurements - if (newAdsData && statesInitialised && VtasMeas > 8.0f) { - fuseVtasData = true; - RecallStates(statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data - FuseAirspeed(); + if (newAdsData && _ekf->statesInitialised && _ekf->VtasMeas > 8.0f) { + _ekf->fuseVtasData = true; + _ekf->RecallStates(_ekf->statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data + _ekf->FuseAirspeed(); } else { - fuseVtasData = false; + _ekf->fuseVtasData = false; } // Publish results @@ -954,7 +970,7 @@ FixedwingEstimator::task_main() // 15-17: Earth Magnetic Field Vector - milligauss (North, East, Down) // 18-20: Body Magnetic Field Vector - milligauss (X,Y,Z) - math::Quaternion q(states[0], states[1], states[2], states[3]); + math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]); math::Matrix<3, 3> R = q.to_dcm(); math::Vector<3> euler = R.to_euler(); @@ -962,10 +978,10 @@ FixedwingEstimator::task_main() _att.R[i][j] = R(i, j); _att.timestamp = last_sensor_timestamp; - _att.q[0] = states[0]; - _att.q[1] = states[1]; - _att.q[2] = states[2]; - _att.q[3] = states[3]; + _att.q[0] = _ekf->states[0]; + _att.q[1] = _ekf->states[1]; + _att.q[2] = _ekf->states[2]; + _att.q[3] = _ekf->states[3]; _att.q_valid = true; _att.R_valid = true; @@ -974,13 +990,13 @@ FixedwingEstimator::task_main() _att.pitch = euler(1); _att.yaw = euler(2); - _att.rollspeed = angRate.x - states[10]; - _att.pitchspeed = angRate.y - states[11]; - _att.yawspeed = angRate.z - states[12]; + _att.rollspeed = _ekf->angRate.x - _ekf->states[10]; + _att.pitchspeed = _ekf->angRate.y - _ekf->states[11]; + _att.yawspeed = _ekf->angRate.z - _ekf->states[12]; // gyro offsets - _att.rate_offsets[0] = states[10]; - _att.rate_offsets[1] = states[11]; - _att.rate_offsets[2] = states[12]; + _att.rate_offsets[0] = _ekf->states[10]; + _att.rate_offsets[1] = _ekf->states[11]; + _att.rate_offsets[2] = _ekf->states[12]; /* lazily publish the attitude only once available */ if (_att_pub > 0) { @@ -993,20 +1009,15 @@ FixedwingEstimator::task_main() } } - if (!isfinite(states[0])) { - print_status(); - _exit(0); - } - if (_gps_initialized) { _local_pos.timestamp = last_sensor_timestamp; - _local_pos.x = states[7]; - _local_pos.y = states[8]; - _local_pos.z = states[9]; + _local_pos.x = _ekf->states[7]; + _local_pos.y = _ekf->states[8]; + _local_pos.z = _ekf->states[9]; - _local_pos.vx = states[4]; - _local_pos.vy = states[5]; - _local_pos.vz = states[6]; + _local_pos.vx = _ekf->states[4]; + _local_pos.vy = _ekf->states[5]; + _local_pos.vz = _ekf->states[6]; _local_pos.xy_valid = _gps_initialized; _local_pos.z_valid = true; @@ -1107,9 +1118,10 @@ FixedwingEstimator::start() return OK; } -void print_status() +void +FixedwingEstimator::print_status() { - math::Quaternion q(states[0], states[1], states[2], states[3]); + math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]); math::Matrix<3, 3> R = q.to_dcm(); math::Vector<3> euler = R.to_euler(); @@ -1125,30 +1137,30 @@ void print_status() // 15-17: Earth Magnetic Field Vector - gauss (North, East, Down) // 18-20: Body Magnetic Field Vector - gauss (X,Y,Z) - printf("dtIMU: %8.6f dt: %8.6f IMUmsec: %d\n", dtIMU, dt, (int)IMUmsec); - printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)dVelIMU.x, (double)dVelIMU.y, (double)dVelIMU.z, (double)accel.x, (double)accel.y, (double)accel.z); - printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)dAngIMU.x, (double)dAngIMU.y, (double)dAngIMU.z, (double)correctedDelAng.x, (double)correctedDelAng.y, (double)correctedDelAng.z); - printf("states (quat) [1-4]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)states[0], (double)states[1], (double)states[2], (double)states[3]); - printf("states (vel m/s) [5-7]: %8.4f, %8.4f, %8.4f\n", (double)states[4], (double)states[5], (double)states[6]); - printf("states (pos m) [8-10]: %8.4f, %8.4f, %8.4f\n", (double)states[7], (double)states[8], (double)states[9]); - printf("states (delta ang) [11-13]: %8.4f, %8.4f, %8.4f\n", (double)states[10], (double)states[11], (double)states[12]); - printf("states (wind) [14-15]: %8.4f, %8.4f\n", (double)states[13], (double)states[14]); - printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)states[15], (double)states[16], (double)states[17]); - printf("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f\n", (double)states[18], (double)states[19], (double)states[20]); + printf("dtIMU: %8.6f dt: %8.6f IMUmsec: %d\n", _ekf->dtIMU, dt, (int)IMUmsec); + printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z); + printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y, (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y, (double)_ekf->correctedDelAng.z); + printf("states (quat) [1-4]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[0], (double)_ekf->states[1], (double)_ekf->states[2], (double)_ekf->states[3]); + printf("states (vel m/s) [5-7]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[4], (double)_ekf->states[5], (double)_ekf->states[6]); + printf("states (pos m) [8-10]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[7], (double)_ekf->states[8], (double)_ekf->states[9]); + printf("states (delta ang) [11-13]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[10], (double)_ekf->states[11], (double)_ekf->states[12]); + printf("states (wind) [14-15]: %8.4f, %8.4f\n", (double)_ekf->states[13], (double)_ekf->states[14]); + printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[15], (double)_ekf->states[16], (double)_ekf->states[17]); + printf("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[18], (double)_ekf->states[19], (double)_ekf->states[20]); printf("states: %s %s %s %s %s %s %s %s %s %s\n", - (statesInitialised) ? "INITIALIZED" : "NON_INIT", - (onGround) ? "ON_GROUND" : "AIRBORNE", - (fuseVelData) ? "FUSE_VEL" : "INH_VEL", - (fusePosData) ? "FUSE_POS" : "INH_POS", - (fuseHgtData) ? "FUSE_HGT" : "INH_HGT", - (fuseMagData) ? "FUSE_MAG" : "INH_MAG", - (fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS", - (useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD", - (useCompass) ? "USE_COMPASS" : "IGN_COMPASS", - (staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE"); + (_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT", + (_ekf->onGround) ? "ON_GROUND" : "AIRBORNE", + (_ekf->fuseVelData) ? "FUSE_VEL" : "INH_VEL", + (_ekf->fusePosData) ? "FUSE_POS" : "INH_POS", + (_ekf->fuseHgtData) ? "FUSE_HGT" : "INH_HGT", + (_ekf->fuseMagData) ? "FUSE_MAG" : "INH_MAG", + (_ekf->fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS", + (_ekf->useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD", + (_ekf->useCompass) ? "USE_COMPASS" : "IGN_COMPASS", + (_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE"); } -int trip_nan() { +int FixedwingEstimator::trip_nan() { int ret = 0; @@ -1166,7 +1178,7 @@ int trip_nan() { float nan_val = 0.0f / 0.0f; warnx("system not armed, tripping state vector with NaN values"); - states[5] = nan_val; + _ekf->states[5] = nan_val; usleep(100000); // warnx("tripping covariance #1 with NaN values"); @@ -1178,15 +1190,15 @@ int trip_nan() { // usleep(100000); warnx("tripping covariance #3 with NaN values"); - P[3][3] = nan_val; // covariance matrix + _ekf->P[3][3] = nan_val; // covariance matrix usleep(100000); warnx("tripping Kalman gains with NaN values"); - Kfusion[0] = nan_val; // Kalman gains + _ekf->Kfusion[0] = nan_val; // Kalman gains usleep(100000); warnx("tripping stored states[0] with NaN values"); - storedStates[0][0] = nan_val; + _ekf->storedStates[0][0] = nan_val; usleep(100000); warnx("\nDONE - FILTER STATE:"); @@ -1234,7 +1246,7 @@ int fw_att_pos_estimator_main(int argc, char *argv[]) if (estimator::g_estimator) { warnx("running"); - print_status(); + estimator::g_estimator->print_status(); exit(0); @@ -1245,7 +1257,7 @@ int fw_att_pos_estimator_main(int argc, char *argv[]) if (!strcmp(argv[1], "trip")) { if (estimator::g_estimator) { - int ret = trip_nan(); + int ret = estimator::g_estimator->trip_nan(); exit(ret); From 2b6a9c5122008ca47cf7524b6887d7de9b0b8a5d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 4 Apr 2014 18:07:58 +0200 Subject: [PATCH 296/381] Removed a bunch of commented out things that we will not need any more. --- src/modules/fw_att_pos_estimator/estimator.h | 75 -------------------- 1 file changed, 75 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/estimator.h b/src/modules/fw_att_pos_estimator/estimator.h index f43f4931af..821392399a 100644 --- a/src/modules/fw_att_pos_estimator/estimator.h +++ b/src/modules/fw_att_pos_estimator/estimator.h @@ -48,81 +48,6 @@ void swap_var(float &d1, float &d2); const unsigned int n_states = 21; const unsigned int data_buffer_size = 50; -// extern uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored -// extern Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad) -// extern Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s) -// extern Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad) -// extern Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s) -// extern float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2) -// extern Vector3f earthRateNED; // earths angular rate vector in NED (rad/s) -// extern Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s) -// extern Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2) -// extern Vector3f dVelIMU; -// extern Vector3f dAngIMU; - -// extern float P[n_states][n_states]; // covariance matrix -// extern float Kfusion[n_states]; // Kalman gains -// extern float states[n_states]; // state matrix -// extern float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps - -// extern Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad) -// extern Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s) -// extern Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad) -// extern Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s) - -// extern float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec) - -// extern bool onGround; // boolean true when the flight vehicle is on the ground (not flying) -// extern bool useAirspeed; // boolean true if airspeed data is being used -// extern bool useCompass; // boolean true if magnetometer data is being used -// extern uint8_t fusionModeGPS ; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity -// extern float innovVelPos[6]; // innovation output -// extern float varInnovVelPos[6]; // innovation variance output - -// extern bool fuseVelData; // this boolean causes the posNE and velNED obs to be fused -// extern bool fusePosData; // this boolean causes the posNE and velNED obs to be fused -// extern bool fuseHgtData; // this boolean causes the hgtMea obs to be fused -// extern bool fuseMagData; // boolean true when magnetometer data is to be fused - -// extern float velNED[3]; // North, East, Down velocity obs (m/s) -// extern float posNE[2]; // North, East position obs (m) -// extern float hgtMea; // measured height (m) -// extern float posNED[3]; // North, East Down position (m) - -// extern float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements -// extern float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements -// extern float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement - -// extern float innovMag[3]; // innovation output -// extern float varInnovMag[3]; // innovation variance output -// extern Vector3f magData; // magnetometer flux radings in X,Y,Z body axes -// extern float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time -// extern float innovVtas; // innovation output -// extern float varInnovVtas; // innovation variance output -// extern bool fuseVtasData; // boolean true when airspeed data is to be fused -// extern float VtasMeas; // true airspeed measurement (m/s) -// extern float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time -// extern float latRef; // WGS-84 latitude of reference point (rad) -// extern float lonRef; // WGS-84 longitude of reference point (rad) -// extern float hgtRef; // WGS-84 height of reference point (m) -// extern Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes -// extern uint8_t covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction -// extern float EAS2TAS; // ratio f true to equivalent airspeed - -// // GPS input data variables -// extern float gpsCourse; -// extern float gpsVelD; -// extern float gpsLat; -// extern float gpsLon; -// extern float gpsHgt; -// extern uint8_t GPSstatus; - -// // Baro input -// extern float baroHgt; - -// extern bool statesInitialised; -// extern bool numericalProtection; - const float covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions const float covDelAngMax = 0.02f; // maximum delta angle between covariance predictions From fcd31b03686ae9a6e04f294044420d09e8e6a2cd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 4 Apr 2014 18:14:23 +0200 Subject: [PATCH 297/381] Reduced the number of states to 10 to avoid killing the logging system --- src/modules/sdlog2/sdlog2_messages.h | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 2b41755de9..22a6958720 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -302,12 +302,19 @@ struct log_PARM_s { /* --- ESTM - ESTIMATOR STATUS --- */ #define LOG_ESTM_MSG 132 struct log_ESTM_s { - float s[32]; + float s[10]; uint8_t n_states; uint8_t states_nan; uint8_t covariance_nan; uint8_t kalman_gain_nan; }; +// struct log_ESTM_s { +// float s[32]; +// uint8_t n_states; +// uint8_t states_nan; +// uint8_t covariance_nan; +// uint8_t kalman_gain_nan; +// }; #pragma pack(pop) @@ -341,7 +348,8 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(TIME, "Q", "StartTime"), LOG_FORMAT(VER, "NZ", "Arch,FwGit"), LOG_FORMAT(PARM, "Nf", "Name,Value"), - LOG_FORMAT(ESTM, "ffffffffffffffffffffffffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27,s28,s29,s30,s31,n_states,states_nan,cov_nan,kgain_nan"), + LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,n_states,states_nan,cov_nan,kgain_nan"), + //LOG_FORMAT(ESTM, "ffffffffffffffffffffffffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27,s28,s29,s30,s31,n_states,states_nan,cov_nan,kgain_nan"), }; static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s); From 88cf841f00b499792780195de63018b3bd49f683 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 4 Apr 2014 18:18:17 +0200 Subject: [PATCH 298/381] Bump RC timeout for all cases to half a second --- src/modules/commander/commander.cpp | 13 ++----------- 1 file changed, 2 insertions(+), 11 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index cf7ba757e2..3720b5a3b3 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -118,8 +118,7 @@ extern struct system_load_s system_load; #define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) #define POSITION_TIMEOUT 1000000 /**< consider the local or global position estimate invalid after 1s */ -#define RC_TIMEOUT 100000 -#define RC_TIMEOUT_HIL 500000 +#define RC_TIMEOUT 500000 #define DIFFPRESS_TIMEOUT 2000000 #define PRINT_INTERVAL 5000000 @@ -1109,16 +1108,8 @@ int commander_thread_main(int argc, char *argv[]) } } - - /* - * XXX workaround: - * Prevent RC loss in HIL when sensors.cpp is only publishing sp_man at a low rate (e.g. 30Hz) - * which can trigger RC loss if the computer/simulator lags. - */ - uint64_t rc_timeout = status.hil_state == HIL_STATE_ON ? RC_TIMEOUT_HIL : RC_TIMEOUT; - /* start 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 + RC_TIMEOUT) { /* handle the case where RC signal was regained */ if (!status.rc_signal_found_once) { status.rc_signal_found_once = true; From 1e25ceb085a8ca5cd53825a2eb30d9cf69c3a8d9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 4 Apr 2014 18:47:30 +0200 Subject: [PATCH 299/381] Create EKF object in right context --- src/modules/fw_att_pos_estimator/estimator.cpp | 11 ++++++++++- src/modules/fw_att_pos_estimator/estimator.h | 4 ++++ .../fw_att_pos_estimator_main.cpp | 4 +++- 3 files changed, 17 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index 3ce1ce56ec..7ab06e85d3 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -106,7 +106,16 @@ void swap_var(float &d1, float &d2) d2 = tmp; } -void AttPosEKF::UpdateStrapdownEquationsNED() +AttPosEKF::AttPosEKF() +{ + +} + +AttPosEKF::~AttPosEKF() +{ +} + +void AttPosEKF::UpdateStrapdownEquationsNED() { Vector3f delVelNav; float q00; diff --git a/src/modules/fw_att_pos_estimator/estimator.h b/src/modules/fw_att_pos_estimator/estimator.h index 821392399a..7edb3c714f 100644 --- a/src/modules/fw_att_pos_estimator/estimator.h +++ b/src/modules/fw_att_pos_estimator/estimator.h @@ -78,6 +78,10 @@ struct ekf_status_report { class AttPosEKF { public: + + AttPosEKF(); + ~AttPosEKF(); + // Global variables float KH[n_states][n_states]; // intermediate result used for covariance updates float KHP[n_states][n_states]; // intermediate result used for covariance updates diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 20c5d37190..840cd585ec 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -292,7 +292,7 @@ FixedwingEstimator::FixedwingEstimator() : _initialized(false), _gps_initialized(false), _mavlink_fd(-1), - _ekf(new AttPosEKF()) + _ekf(nullptr) { _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); @@ -396,6 +396,8 @@ void FixedwingEstimator::task_main() { + _ekf = new AttPosEKF(); + if (!_ekf) { errx(1, "failed allocating EKF filter - out of RAM!"); } From c6d98a32f83383e6204fd6cefbfcc1fd7e1cf159 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 10:27:43 +0200 Subject: [PATCH 300/381] Proper failsafe handling onboard, including throttle failsafe condition if enabled --- src/modules/px4iofirmware/controls.c | 36 ++++++++++++++++++---------- src/modules/px4iofirmware/protocol.h | 9 +++---- 2 files changed, 29 insertions(+), 16 deletions(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 609dd33126..b860fc587d 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -134,8 +134,6 @@ controls_tick() { perf_begin(c_gather_sbus); - bool sbus_status = (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS); - bool sbus_failsafe, sbus_frame_drop; bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, PX4IO_RC_INPUT_CHANNELS); @@ -261,8 +259,20 @@ controls_tick() { if (mapped < PX4IO_CONTROL_CHANNELS) { /* invert channel if pitch - pulling the lever down means pitching up by convention */ - if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */ + if (mapped == 1) { + /* roll, pitch, yaw, throttle, override is the standard order */ scaled = -scaled; + } + + if (mapped == 3 && (r_setup_features & PX4IO_P_SETUP_FEATURES_RC_FAIL_DETECT)) { + /* throttle failsafe detection */ + if (((raw < conf[PX4IO_P_RC_CONFIG_MIN]) && (raw < 800)) || + ((raw > conf[PX4IO_P_RC_CONFIG_MAX]) && (raw > 2200))) { + r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE; + } else { + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); + } + } r_rc_values[mapped] = SIGNED_TO_REG(scaled); assigned_channels |= (1 << mapped); @@ -312,6 +322,11 @@ controls_tick() { * Handle losing RC input */ + /* if we are in failsafe, clear the override flag */ + if (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) { + r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE); + } + /* this kicks in if the receiver is gone, but there is not on failsafe (indicated by separate flag) */ if (rc_input_lost) { /* Clear the RC input status flag, clear manual override flag */ @@ -322,27 +337,24 @@ controls_tick() { /* Mark all channels as invalid, as we just lost the RX */ r_rc_valid = 0; + /* Set raw channel count to zero */ + r_raw_rc_count = 0; + /* Set the RC_LOST alarm */ r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST; } - /* this kicks in if the receiver is completely gone */ - if (rc_input_lost) { - - /* Set channel count to zero */ - r_raw_rc_count = 0; - } - /* * Check for manual override. * * The PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK flag must be set, and we - * must have R/C input. + * must have R/C input (NO FAILSAFE!). * Override is enabled if either the hardcoded channel / value combination * is selected, or the AP has requested it. */ if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK)) { + (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && + !(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) { bool override = false; diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index d48c6c5297..b69f681070 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -164,10 +164,11 @@ /* setup page */ #define PX4IO_PAGE_SETUP 50 #define PX4IO_P_SETUP_FEATURES 0 -#define PX4IO_P_SETUP_FEATURES_SBUS1_OUT (1 << 0) /* enable S.Bus v1 output */ -#define PX4IO_P_SETUP_FEATURES_SBUS2_OUT (1 << 1) /* enable S.Bus v2 output */ -#define PX4IO_P_SETUP_FEATURES_PWM_RSSI (1 << 2) /* enable PWM RSSI parsing */ -#define PX4IO_P_SETUP_FEATURES_ADC_RSSI (1 << 3) /* enable ADC RSSI parsing */ +#define PX4IO_P_SETUP_FEATURES_SBUS1_OUT (1 << 0) /**< enable S.Bus v1 output */ +#define PX4IO_P_SETUP_FEATURES_SBUS2_OUT (1 << 1) /**< enable S.Bus v2 output */ +#define PX4IO_P_SETUP_FEATURES_PWM_RSSI (1 << 2) /**< enable PWM RSSI parsing */ +#define PX4IO_P_SETUP_FEATURES_ADC_RSSI (1 << 3) /**< enable ADC RSSI parsing */ +#define PX4IO_P_SETUP_FEATURES_RC_FAIL_DETECT (1 << 4) /**< enable RC fail detection based on channel value */ #define PX4IO_P_SETUP_ARMING 1 /* arming controls */ #define PX4IO_P_SETUP_ARMING_IO_ARM_OK (1 << 0) /* OK to arm the IO side */ From 64ffafb48ee45452070dfb37be8d0de6098915a8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 10:28:01 +0200 Subject: [PATCH 301/381] Only publish RC inputs if we have seen some valid inputs at some point --- src/drivers/px4io/px4io.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 2c00785038..8eee1cbca0 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1479,9 +1479,11 @@ PX4IO::io_publish_raw_rc() } else { rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN; - /* we do not know the RC input, but have to publish timestamp_published - * and rc_lost flag, so do not prematurely return here - */ + /* only keep publishing RC input if we ever got a valid input */ + if (_rc_last_valid == 0) { + /* we have never seen valid RC signals, abort */ + return OK; + } } /* lazily advertise on first publication */ From fb44ad8e22f2a0862f1d7bda66643a6336247024 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 10:29:17 +0200 Subject: [PATCH 302/381] Simplify the failsafe handling, reduce 3 params to one --- src/modules/sensors/sensor_params.c | 28 +++------------------------- src/modules/sensors/sensors.cpp | 26 +++++++------------------- 2 files changed, 10 insertions(+), 44 deletions(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 288c6e00a0..e4564aa25c 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -668,33 +668,11 @@ PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.6f); */ PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 2.0f); - -/** - * Failsafe channel mapping. - * - * @min 0 - * @max 18 - * @group Radio Calibration - */ -PARAM_DEFINE_INT32(RC_FS_CH, 0); - -/** - * Failsafe channel mode. - * - * 0 = too low means signal loss, - * 1 = too high means signal loss - * - * @min 0 - * @max 1 - * @group Radio Calibration - */ -PARAM_DEFINE_INT32(RC_FS_MODE, 0); - /** * Failsafe channel PWM threshold. * - * @min 0 - * @max 1 + * @min 800 + * @max 2200 * @group Radio Calibration */ -PARAM_DEFINE_FLOAT(RC_FS_THR, 800); +PARAM_DEFINE_FLOAT(RC_FS_THR, 0); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index f890c4c7f8..2442acd6b0 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -263,8 +263,6 @@ private: float rc_scale_yaw; float rc_scale_flaps; - int rc_fs_ch; - int rc_fs_mode; float rc_fs_thr; float battery_voltage_scaling; @@ -313,8 +311,6 @@ private: param_t rc_scale_yaw; param_t rc_scale_flaps; - param_t rc_fs_ch; - param_t rc_fs_mode; param_t rc_fs_thr; param_t battery_voltage_scaling; @@ -531,8 +527,6 @@ Sensors::Sensors() : _parameter_handles.rc_scale_flaps = param_find("RC_SCALE_FLAPS"); /* RC failsafe */ - _parameter_handles.rc_fs_ch = param_find("RC_FS_CH"); - _parameter_handles.rc_fs_mode = param_find("RC_FS_MODE"); _parameter_handles.rc_fs_thr = param_find("RC_FS_THR"); /* gyro offsets */ @@ -689,8 +683,6 @@ Sensors::parameters_update() param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch)); param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw)); param_get(_parameter_handles.rc_scale_flaps, &(_parameters.rc_scale_flaps)); - param_get(_parameter_handles.rc_fs_ch, &(_parameters.rc_fs_ch)); - param_get(_parameter_handles.rc_fs_mode, &(_parameters.rc_fs_mode)); param_get(_parameter_handles.rc_fs_thr, &(_parameters.rc_fs_thr)); /* update RC function mappings */ @@ -1312,19 +1304,15 @@ Sensors::rc_poll() manual_control.aux5 = NAN; /* require at least four channels to consider the signal valid */ - if (rc_input.channel_count < 4) + if (rc_input.channel_count < 4) { return; + } - /* failsafe check */ - if (_parameters.rc_fs_ch != 0) { - if (_parameters.rc_fs_mode == 0) { - if (rc_input.values[_parameters.rc_fs_ch - 1] < _parameters.rc_fs_thr) - return; - - } else if (_parameters.rc_fs_mode == 1) { - if (rc_input.values[_parameters.rc_fs_ch - 1] > _parameters.rc_fs_thr) - return; - } + /* check for failsafe */ + if (((rc_input.values[_rc.function[THROTTLE]] < _parameters.min[i]) && (rc_input.values[_rc.function[THROTTLE]] < _parameters.rc_fs_thr)) + || ((rc_input.values[_rc.function[THROTTLE]] > _parameters.max[i]) && (rc_input.values[_rc.function[THROTTLE]] > _parameters.rc_fs_thr))) { + /* do not publish manual control setpoints when there are none */ + return; } unsigned channel_limit = rc_input.channel_count; From 797698a7a114032c7eea62c5f48f2b229ca973b7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 10:34:35 +0200 Subject: [PATCH 303/381] Trigger failsafe action also on failsafe flag --- src/modules/sensors/sensors.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 2442acd6b0..4c34d853fc 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1309,7 +1309,7 @@ Sensors::rc_poll() } /* check for failsafe */ - if (((rc_input.values[_rc.function[THROTTLE]] < _parameters.min[i]) && (rc_input.values[_rc.function[THROTTLE]] < _parameters.rc_fs_thr)) + if (rc_input.rc_failsafe || ((rc_input.values[_rc.function[THROTTLE]] < _parameters.min[i]) && (rc_input.values[_rc.function[THROTTLE]] < _parameters.rc_fs_thr)) || ((rc_input.values[_rc.function[THROTTLE]] > _parameters.max[i]) && (rc_input.values[_rc.function[THROTTLE]] > _parameters.rc_fs_thr))) { /* do not publish manual control setpoints when there are none */ return; From 9123ebce8cbc618899ece31d31c97e022038beb2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 10:45:02 +0200 Subject: [PATCH 304/381] px4io: Allow RC failsafe detection as valid feature --- src/modules/px4iofirmware/registers.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 97d25bbfa8..42b863b539 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -175,7 +175,8 @@ volatile uint16_t r_page_setup[] = #define PX4IO_P_SETUP_FEATURES_VALID (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | \ PX4IO_P_SETUP_FEATURES_SBUS2_OUT | \ PX4IO_P_SETUP_FEATURES_ADC_RSSI | \ - PX4IO_P_SETUP_FEATURES_PWM_RSSI) + PX4IO_P_SETUP_FEATURES_PWM_RSSI | \ + PX4IO_P_SETUP_FEATURES_RC_FAIL_DETECT) #else #define PX4IO_P_SETUP_FEATURES_VALID 0 #endif From 9a0b2b7610d39f88b627046e0d90f66aada1e88f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 11:02:22 +0200 Subject: [PATCH 305/381] Make throttle failsafe depend on the failsafe threshold parameter. Make the parameter optional (no harm if not found). --- src/drivers/px4io/px4io.cpp | 17 ++++++++++++++++- src/modules/px4iofirmware/controls.c | 6 +++--- src/modules/px4iofirmware/protocol.h | 21 +++++++++++---------- src/modules/px4iofirmware/px4io.h | 1 + src/modules/px4iofirmware/registers.c | 4 ++-- 5 files changed, 33 insertions(+), 16 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 8eee1cbca0..f6125d273d 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -944,8 +944,23 @@ PX4IO::task_main() int pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE, &scaling, 1); if (pret != OK) { - log("voltage scaling upload failed"); + log("vscale upload failed"); } + + /* send RC throttle failsafe value to IO */ + float failsafe_param_val; + param_t failsafe_param = param_find("RC_FS_THR"); + + if (failsafe_param > 0) + + param_get(failsafe_param, &failsafe_param_val); + uint16_t failsafe_thr = failsafe_param_val; + pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RC_THR_FAILSAFE_US, &failsafe_thr, 1); + if (pret != OK) { + log("failsafe upload failed"); + } + } + } } diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index b860fc587d..356fe44cd5 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -264,10 +264,10 @@ controls_tick() { scaled = -scaled; } - if (mapped == 3 && (r_setup_features & PX4IO_P_SETUP_FEATURES_RC_FAIL_DETECT)) { + if (mapped == 3 && r_setup_rc_thr_failsafe) { /* throttle failsafe detection */ - if (((raw < conf[PX4IO_P_RC_CONFIG_MIN]) && (raw < 800)) || - ((raw > conf[PX4IO_P_RC_CONFIG_MAX]) && (raw > 2200))) { + if (((raw < conf[PX4IO_P_RC_CONFIG_MIN]) && (raw < r_setup_rc_thr_failsafe)) || + ((raw > conf[PX4IO_P_RC_CONFIG_MAX]) && (raw > r_setup_rc_thr_failsafe))) { r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE; } else { r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index b69f681070..a978d483a4 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -168,7 +168,6 @@ #define PX4IO_P_SETUP_FEATURES_SBUS2_OUT (1 << 1) /**< enable S.Bus v2 output */ #define PX4IO_P_SETUP_FEATURES_PWM_RSSI (1 << 2) /**< enable PWM RSSI parsing */ #define PX4IO_P_SETUP_FEATURES_ADC_RSSI (1 << 3) /**< enable ADC RSSI parsing */ -#define PX4IO_P_SETUP_FEATURES_RC_FAIL_DETECT (1 << 4) /**< enable RC fail detection based on channel value */ #define PX4IO_P_SETUP_ARMING 1 /* arming controls */ #define PX4IO_P_SETUP_ARMING_IO_ARM_OK (1 << 0) /* OK to arm the IO side */ @@ -202,13 +201,15 @@ enum { /* DSM bind states */ dsm_bind_send_pulses, dsm_bind_reinit_uart }; - /* 8 */ -#define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */ + /* 8 */ +#define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */ -#define PX4IO_P_SETUP_REBOOT_BL 10 /* reboot IO into bootloader */ -#define PX4IO_REBOOT_BL_MAGIC 14662 /* required argument for reboot (random) */ +#define PX4IO_P_SETUP_REBOOT_BL 10 /* reboot IO into bootloader */ +#define PX4IO_REBOOT_BL_MAGIC 14662 /* required argument for reboot (random) */ -#define PX4IO_P_SETUP_CRC 11 /* get CRC of IO firmware */ +#define PX4IO_P_SETUP_CRC 11 /* get CRC of IO firmware */ + /* 12 occupied by CRC */ +#define PX4IO_P_SETUP_RC_THR_FAILSAFE_US 13 /**< the throttle failsafe pulse length in microseconds */ /* autopilot control values, -10000..10000 */ #define PX4IO_PAGE_CONTROLS 51 /**< actuator control groups, one after the other, 8 wide */ @@ -218,10 +219,10 @@ enum { /* DSM bind states */ #define PX4IO_P_CONTROLS_GROUP_3 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 3) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */ #define PX4IO_P_CONTROLS_GROUP_VALID 64 -#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP0 (1 << 0) /* group 0 is valid / received */ -#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP1 (1 << 1) /* group 1 is valid / received */ -#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP2 (1 << 2) /* group 2 is valid / received */ -#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP3 (1 << 3) /* group 3 is valid / received */ +#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP0 (1 << 0) /**< group 0 is valid / received */ +#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP1 (1 << 1) /**< group 1 is valid / received */ +#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP2 (1 << 2) /**< group 2 is valid / received */ +#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP3 (1 << 3) /**< group 3 is valid / received */ /* raw text load to the mixer parser - ignores offset */ #define PX4IO_PAGE_MIXERLOAD 52 diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 54c5663a5b..88ad793980 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -108,6 +108,7 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */ #ifdef CONFIG_ARCH_BOARD_PX4IO_V1 #define r_setup_relays r_page_setup[PX4IO_P_SETUP_RELAYS] #endif +#define r_setup_rc_thr_failsafe r_page_setup[PX4IO_P_SETUP_RC_THR_FAILSAFE_US]; #define r_control_values (&r_page_controls[0]) diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 42b863b539..6752e7b4b3 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -169,14 +169,14 @@ volatile uint16_t r_page_setup[] = [PX4IO_P_SETUP_SET_DEBUG] = 0, [PX4IO_P_SETUP_REBOOT_BL] = 0, [PX4IO_P_SETUP_CRC ... (PX4IO_P_SETUP_CRC+1)] = 0, + [PX4IO_P_SETUP_RC_THR_FAILSAFE_US] = 0, }; #ifdef CONFIG_ARCH_BOARD_PX4IO_V2 #define PX4IO_P_SETUP_FEATURES_VALID (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | \ PX4IO_P_SETUP_FEATURES_SBUS2_OUT | \ PX4IO_P_SETUP_FEATURES_ADC_RSSI | \ - PX4IO_P_SETUP_FEATURES_PWM_RSSI | \ - PX4IO_P_SETUP_FEATURES_RC_FAIL_DETECT) + PX4IO_P_SETUP_FEATURES_PWM_RSSI) #else #define PX4IO_P_SETUP_FEATURES_VALID 0 #endif From 5e0d687b566022c12270f68facbf7ca35f62306c Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Tue, 25 Mar 2014 15:23:40 +0100 Subject: [PATCH 306/381] px4io driver: publish input_rc even if RC connection has been lost --- src/drivers/px4io/px4io.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 82f3ba044a..2c00785038 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1479,10 +1479,9 @@ PX4IO::io_publish_raw_rc() } else { rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN; - /* we do not know the RC input, only publish if RC OK flag is set */ - /* if no raw RC, just don't publish */ - if (!(_status & PX4IO_P_STATUS_FLAGS_RC_OK)) - return OK; + /* we do not know the RC input, but have to publish timestamp_published + * and rc_lost flag, so do not prematurely return here + */ } /* lazily advertise on first publication */ From 745ef4f4856af60aca5625860e5e3f25ea189dc9 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Tue, 25 Mar 2014 15:42:44 +0100 Subject: [PATCH 307/381] px4io: do not include failsafe condition into rc_lost flag --- src/modules/px4iofirmware/controls.c | 138 +++++++++++++-------------- 1 file changed, 67 insertions(+), 71 deletions(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 941500f0d9..609dd33126 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -201,94 +201,90 @@ controls_tick() { /* update RC-received timestamp */ system_state.rc_channels_timestamp_received = hrt_absolute_time(); - /* do not command anything in failsafe, kick in the RC loss counter */ - if (!(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) { + /* update RC-received timestamp */ + system_state.rc_channels_timestamp_valid = system_state.rc_channels_timestamp_received; - /* update RC-received timestamp */ - system_state.rc_channels_timestamp_valid = system_state.rc_channels_timestamp_received; + /* map raw inputs to mapped inputs */ + /* XXX mapping should be atomic relative to protocol */ + for (unsigned i = 0; i < r_raw_rc_count; i++) { - /* map raw inputs to mapped inputs */ - /* XXX mapping should be atomic relative to protocol */ - for (unsigned i = 0; i < r_raw_rc_count; i++) { + /* map the input channel */ + uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE]; - /* map the input channel */ - uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE]; + if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { - if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { + uint16_t raw = r_raw_rc_values[i]; - uint16_t raw = r_raw_rc_values[i]; + int16_t scaled; - int16_t scaled; + /* + * 1) Constrain to min/max values, as later processing depends on bounds. + */ + if (raw < conf[PX4IO_P_RC_CONFIG_MIN]) + raw = conf[PX4IO_P_RC_CONFIG_MIN]; + if (raw > conf[PX4IO_P_RC_CONFIG_MAX]) + raw = conf[PX4IO_P_RC_CONFIG_MAX]; - /* - * 1) Constrain to min/max values, as later processing depends on bounds. - */ - if (raw < conf[PX4IO_P_RC_CONFIG_MIN]) - raw = conf[PX4IO_P_RC_CONFIG_MIN]; - if (raw > conf[PX4IO_P_RC_CONFIG_MAX]) - raw = conf[PX4IO_P_RC_CONFIG_MAX]; + /* + * 2) Scale around the mid point differently for lower and upper range. + * + * This is necessary as they don't share the same endpoints and slope. + * + * First normalize to 0..1 range with correct sign (below or above center), + * then scale to 20000 range (if center is an actual center, -10000..10000, + * if parameters only support half range, scale to 10000 range, e.g. if + * center == min 0..10000, if center == max -10000..0). + * + * As the min and max bounds were enforced in step 1), division by zero + * cannot occur, as for the case of center == min or center == max the if + * statement is mutually exclusive with the arithmetic NaN case. + * + * DO NOT REMOVE OR ALTER STEP 1! + */ + if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) { + scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])); - /* - * 2) Scale around the mid point differently for lower and upper range. - * - * This is necessary as they don't share the same endpoints and slope. - * - * First normalize to 0..1 range with correct sign (below or above center), - * then scale to 20000 range (if center is an actual center, -10000..10000, - * if parameters only support half range, scale to 10000 range, e.g. if - * center == min 0..10000, if center == max -10000..0). - * - * As the min and max bounds were enforced in step 1), division by zero - * cannot occur, as for the case of center == min or center == max the if - * statement is mutually exclusive with the arithmetic NaN case. - * - * DO NOT REMOVE OR ALTER STEP 1! - */ - if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) { - scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])); + } else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) { + scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN])); - } else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) { - scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN])); + } else { + /* in the configured dead zone, output zero */ + scaled = 0; + } - } else { - /* in the configured dead zone, output zero */ - scaled = 0; - } + /* invert channel if requested */ + if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) + scaled = -scaled; - /* invert channel if requested */ - if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) + /* and update the scaled/mapped version */ + unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]; + if (mapped < PX4IO_CONTROL_CHANNELS) { + + /* invert channel if pitch - pulling the lever down means pitching up by convention */ + if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */ scaled = -scaled; - /* and update the scaled/mapped version */ - unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]; - if (mapped < PX4IO_CONTROL_CHANNELS) { + r_rc_values[mapped] = SIGNED_TO_REG(scaled); + assigned_channels |= (1 << mapped); - /* invert channel if pitch - pulling the lever down means pitching up by convention */ - if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */ - scaled = -scaled; - - r_rc_values[mapped] = SIGNED_TO_REG(scaled); - assigned_channels |= (1 << mapped); - - } } } + } - /* set un-assigned controls to zero */ - for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) { - if (!(assigned_channels & (1 << i))) - r_rc_values[i] = 0; - } + /* set un-assigned controls to zero */ + for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) { + if (!(assigned_channels & (1 << i))) + r_rc_values[i] = 0; + } - /* set RC OK flag, as we got an update */ - r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK; + /* set RC OK flag, as we got an update */ + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK; - /* if we have enough channels (5) to control the vehicle, the mapping is ok */ - if (assigned_channels > 4) { - r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK; - } else { - r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK); - } + /* if we have enough channels (5) to control the vehicle, the mapping is ok */ + if (assigned_channels > 4) { + r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK; + } else { + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK); } /* @@ -316,8 +312,8 @@ controls_tick() { * Handle losing RC input */ - /* this kicks in if the receiver is gone or the system went to failsafe */ - if (rc_input_lost || (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) { + /* this kicks in if the receiver is gone, but there is not on failsafe (indicated by separate flag) */ + if (rc_input_lost) { /* Clear the RC input status flag, clear manual override flag */ r_status_flags &= ~( PX4IO_P_STATUS_FLAGS_OVERRIDE | From 09d106432749ec02866241863eb912f05b903e64 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 11:06:07 +0200 Subject: [PATCH 308/381] px4io: Remove unused variable --- src/modules/px4iofirmware/controls.c | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 609dd33126..f56f630bce 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -134,8 +134,6 @@ controls_tick() { perf_begin(c_gather_sbus); - bool sbus_status = (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS); - bool sbus_failsafe, sbus_frame_drop; bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, PX4IO_RC_INPUT_CHANNELS); From 3e4841b6fe2d8d6d06b167be49cbe76ab7e04a46 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 11:10:41 +0200 Subject: [PATCH 309/381] px4io: Guard against the RC failsafe value of channel 5 causing a manual override action if set to manual in failsafe --- src/modules/px4iofirmware/controls.c | 24 +++++++++++++----------- 1 file changed, 13 insertions(+), 11 deletions(-) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index f56f630bce..aae7bb951a 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -310,6 +310,11 @@ controls_tick() { * Handle losing RC input */ + /* if we are in failsafe, clear the override flag */ + if (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) { + r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE); + } + /* this kicks in if the receiver is gone, but there is not on failsafe (indicated by separate flag) */ if (rc_input_lost) { /* Clear the RC input status flag, clear manual override flag */ @@ -320,27 +325,24 @@ controls_tick() { /* Mark all channels as invalid, as we just lost the RX */ r_rc_valid = 0; - /* Set the RC_LOST alarm */ - r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST; - } - - /* this kicks in if the receiver is completely gone */ - if (rc_input_lost) { - /* Set channel count to zero */ r_raw_rc_count = 0; + + /* Set the RC_LOST alarm */ + r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST; } /* * Check for manual override. * * The PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK flag must be set, and we - * must have R/C input. + * must have R/C input (NO FAILSAFE!). * Override is enabled if either the hardcoded channel / value combination * is selected, or the AP has requested it. */ if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK)) { + (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && + !(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) { bool override = false; @@ -363,10 +365,10 @@ controls_tick() { mixer_tick(); } else { - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE; + r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE); } } else { - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE; + r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE); } } From 73d04f7a37ba52ce1891e740db554557bd71940a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 11:15:22 +0200 Subject: [PATCH 310/381] px4io driver: Only publish RC signal if it was at least once valid. --- src/drivers/px4io/px4io.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 2c00785038..8eee1cbca0 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1479,9 +1479,11 @@ PX4IO::io_publish_raw_rc() } else { rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN; - /* we do not know the RC input, but have to publish timestamp_published - * and rc_lost flag, so do not prematurely return here - */ + /* only keep publishing RC input if we ever got a valid input */ + if (_rc_last_valid == 0) { + /* we have never seen valid RC signals, abort */ + return OK; + } } /* lazily advertise on first publication */ From 3b5e6f98338fded2cbe7be1c301dc2698f7239aa Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 11:28:07 +0200 Subject: [PATCH 311/381] sensors and px4io driver: Guard against failsafe trigger for inverted remotes --- src/drivers/px4io/px4io.cpp | 4 ++-- src/modules/sensors/sensor_params.c | 2 +- src/modules/sensors/sensors.cpp | 8 ++++---- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index f6125d273d..a30bfb2dec 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -948,8 +948,8 @@ PX4IO::task_main() } /* send RC throttle failsafe value to IO */ - float failsafe_param_val; - param_t failsafe_param = param_find("RC_FS_THR"); + int32_t failsafe_param_val; + param_t failsafe_param = param_find("RC_FAILS_THR"); if (failsafe_param > 0) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index e4564aa25c..14f7ac812f 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -675,4 +675,4 @@ PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 2.0f); * @max 2200 * @group Radio Calibration */ -PARAM_DEFINE_FLOAT(RC_FS_THR, 0); +PARAM_DEFINE_INT32(RC_FAILS_THR, 0); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 4c34d853fc..44a91ca67a 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -263,7 +263,7 @@ private: float rc_scale_yaw; float rc_scale_flaps; - float rc_fs_thr; + int32_t rc_fs_thr; float battery_voltage_scaling; float battery_current_scaling; @@ -527,7 +527,7 @@ Sensors::Sensors() : _parameter_handles.rc_scale_flaps = param_find("RC_SCALE_FLAPS"); /* RC failsafe */ - _parameter_handles.rc_fs_thr = param_find("RC_FS_THR"); + _parameter_handles.rc_fs_thr = param_find("RC_FAILS_THR"); /* gyro offsets */ _parameter_handles.gyro_offset[0] = param_find("SENS_GYRO_XOFF"); @@ -1309,8 +1309,8 @@ Sensors::rc_poll() } /* check for failsafe */ - if (rc_input.rc_failsafe || ((rc_input.values[_rc.function[THROTTLE]] < _parameters.min[i]) && (rc_input.values[_rc.function[THROTTLE]] < _parameters.rc_fs_thr)) - || ((rc_input.values[_rc.function[THROTTLE]] > _parameters.max[i]) && (rc_input.values[_rc.function[THROTTLE]] > _parameters.rc_fs_thr))) { + if (_parameters.rc_fs_thr && (rc_input.rc_failsafe || ((rc_input.values[_rc.function[THROTTLE]] < _parameters.min[i]) && (rc_input.values[_rc.function[THROTTLE]] < _parameters.rc_fs_thr)) + || ((rc_input.values[_rc.function[THROTTLE]] > _parameters.max[i]) && (rc_input.values[_rc.function[THROTTLE]] > _parameters.rc_fs_thr)))) { /* do not publish manual control setpoints when there are none */ return; } From ec2197fd1b228d8eb4855c49d5f9b1365685d01c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 4 Apr 2014 13:02:44 +1100 Subject: [PATCH 312/381] uploader: ignore bad character encodings for older bootloaders this prevents the uploader from throwing an exception with older bootloaders --- Tools/px_uploader.py | 28 ++++++++++++++++------------ 1 file changed, 16 insertions(+), 12 deletions(-) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index e4a8b3c054..985e6ffd9d 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -389,18 +389,22 @@ class uploader(object): self.otp_pid = self.otp[12:8:-1] self.otp_coa = self.otp[32:160] # show user: - print("type: " + self.otp_id.decode('Latin-1')) - print("idtype: " + binascii.b2a_qp(self.otp_idtype).decode('Latin-1')) - print("vid: " + binascii.hexlify(self.otp_vid).decode('Latin-1')) - print("pid: "+ binascii.hexlify(self.otp_pid).decode('Latin-1')) - print("coa: "+ binascii.b2a_base64(self.otp_coa).decode('Latin-1')) - print("sn: ", end='') - for byte in range(0,12,4): - x = self.__getSN(byte) - x = x[::-1] # reverse the bytes - self.sn = self.sn + x - print(binascii.hexlify(x).decode('Latin-1'), end='') # show user - print('') + try: + print("type: " + self.otp_id.decode('Latin-1')) + print("idtype: " + binascii.b2a_qp(self.otp_idtype).decode('Latin-1')) + print("vid: " + binascii.hexlify(self.otp_vid).decode('Latin-1')) + print("pid: "+ binascii.hexlify(self.otp_pid).decode('Latin-1')) + print("coa: "+ binascii.b2a_base64(self.otp_coa).decode('Latin-1')) + print("sn: ", end='') + for byte in range(0,12,4): + x = self.__getSN(byte) + x = x[::-1] # reverse the bytes + self.sn = self.sn + x + print(binascii.hexlify(x).decode('Latin-1'), end='') # show user + print('') + except Exception: + # ignore bad character encodings + pass print("erase...") self.__erase() From ea5279389f2b13110735083f511afb630ef5e3d8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 12 Feb 2014 12:16:51 +1100 Subject: [PATCH 313/381] uORB: added an ORB topic for system_power holds power supply state and 5V rail voltage on FMUv2 --- src/modules/uORB/objects_common.cpp | 3 ++ src/modules/uORB/topics/system_power.h | 71 ++++++++++++++++++++++++++ 2 files changed, 74 insertions(+) create mode 100644 src/modules/uORB/topics/system_power.h diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 4b31cc8a49..c8a589bb7c 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -90,6 +90,9 @@ ORB_DEFINE(battery_status, struct battery_status_s); #include "topics/servorail_status.h" ORB_DEFINE(servorail_status, struct servorail_status_s); +#include "topics/system_power.h" +ORB_DEFINE(system_power, struct system_power_s); + #include "topics/vehicle_global_position.h" ORB_DEFINE(vehicle_global_position, struct vehicle_global_position_s); diff --git a/src/modules/uORB/topics/system_power.h b/src/modules/uORB/topics/system_power.h new file mode 100644 index 0000000000..7763b60043 --- /dev/null +++ b/src/modules/uORB/topics/system_power.h @@ -0,0 +1,71 @@ +/**************************************************************************** + * + * 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 system_power.h + * + * Definition of the system_power voltage and power status uORB topic. + */ + +#ifndef SYSTEM_POWER_H_ +#define SYSTEM_POWER_H_ + +#include "../uORB.h" +#include + +/** + * @addtogroup topics + * @{ + */ + +/** + * voltage and power supply status + */ +struct system_power_s { + uint64_t timestamp; /**< microseconds since system boot */ + float voltage5V_v; /**< peripheral 5V rail voltage */ + uint8_t usb_connected:1; /**< USB is connected when 1 */ + uint8_t brick_valid:1; /**< brick power is good when 1 */ + uint8_t servo_valid:1; /**< servo power is good when 1 */ + uint8_t periph_5V_OC:1; /**< peripheral overcurrent when 1 */ + uint8_t hipower_5V_OC:1; /**< hi power peripheral overcurrent when 1 */ +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(system_power); + +#endif From 6ea22c8c079db8633d663cbf8ca39b81a434a040 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 12 Feb 2014 12:17:25 +1100 Subject: [PATCH 314/381] adc: publish system_power ORB topic from ADC --- src/drivers/stm32/adc/adc.cpp | 47 ++++++++++++++++++++++++++++++++++- 1 file changed, 46 insertions(+), 1 deletion(-) diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp index 0b8a275e6a..3a60d2cae1 100644 --- a/src/drivers/stm32/adc/adc.cpp +++ b/src/drivers/stm32/adc/adc.cpp @@ -41,6 +41,7 @@ */ #include +#include #include #include @@ -64,6 +65,8 @@ #include #include +#include + /* * Register accessors. * For now, no reason not to just use ADC1. @@ -119,6 +122,8 @@ private: unsigned _channel_count; adc_msg_s *_samples; /**< sample buffer */ + orb_advert_t _to_system_power; + /** work trampoline */ static void _tick_trampoline(void *arg); @@ -134,13 +139,16 @@ private: */ uint16_t _sample(unsigned channel); + // update system_power ORB topic, only on FMUv2 + void update_system_power(void); }; ADC::ADC(uint32_t channels) : CDev("adc", ADC_DEVICE_PATH), _sample_perf(perf_alloc(PC_ELAPSED, "ADC samples")), _channel_count(0), - _samples(nullptr) + _samples(nullptr), + _to_system_power(0) { _debug_enabled = true; @@ -290,6 +298,43 @@ ADC::_tick() /* scan the channel set and sample each */ for (unsigned i = 0; i < _channel_count; i++) _samples[i].am_data = _sample(_samples[i].am_channel); + update_system_power(); +} + +void +ADC::update_system_power(void) +{ +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + system_power_s system_power; + system_power.timestamp = hrt_absolute_time(); + + system_power.voltage5V_v = 0; + for (unsigned i = 0; i < _channel_count; i++) { + if (_samples[i].am_channel == 4) { + // it is 2:1 scaled + system_power.voltage5V_v = _samples[i].am_data * (6.6f / 4096); + } + } + + // these are not ADC related, but it is convenient to + // publish these to the same topic + system_power.usb_connected = stm32_gpioread(GPIO_OTGFS_VBUS); + + // note that the valid pins are active low + system_power.brick_valid = !stm32_gpioread(GPIO_VDD_BRICK_VALID); + system_power.servo_valid = !stm32_gpioread(GPIO_VDD_SERVO_VALID); + + // OC pins are active low + system_power.periph_5V_OC = !stm32_gpioread(GPIO_VDD_5V_PERIPH_OC); + system_power.hipower_5V_OC = !stm32_gpioread(GPIO_VDD_5V_HIPOWER_OC); + + /* lazily publish */ + if (_to_system_power > 0) { + orb_publish(ORB_ID(system_power), _to_system_power, &system_power); + } else { + _to_system_power = orb_advertise(ORB_ID(system_power), &system_power); + } +#endif // CONFIG_ARCH_BOARD_PX4FMU_V2 } uint16_t From 30d1ce3a51caafe959fc048e4d7ca6147bd2ccd6 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 11 Mar 2014 12:42:46 +1100 Subject: [PATCH 315/381] hmc5883: properly reset mag to normal state on calibration fail and add current output in "hmc5883 info" --- src/drivers/hmc5883/hmc5883.cpp | 36 +++++++++++++++++---------------- 1 file changed, 19 insertions(+), 17 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 4c85c0cdaa..cb97354ecc 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -169,6 +169,8 @@ private: int _bus; /**< the bus the device is connected to */ + struct mag_report _last_report; /**< used for info() */ + /** * Test whether the device supported by the driver is present at a * specific address. @@ -870,6 +872,8 @@ HMC5883::collect() } } + _last_report = new_report; + /* post a report to the ring */ if (_reports->force(&new_report)) { perf_count(_buffer_overflows); @@ -1042,32 +1046,29 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) warnx("axes scaling: %.6f %.6f %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[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"); - goto out; - } - - if (OK != ::ioctl(fd, MAGIOCEXSTRAP, 0)) { - warnx("failed to disable sensor calibration mode"); - goto out; - } - /* set scaling in device */ mscale_previous.x_scale = scaling[0]; mscale_previous.y_scale = scaling[1]; mscale_previous.z_scale = scaling[2]; - if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) { - warn("WARNING: failed to set new scale / offsets for mag"); - goto out; - } - ret = OK; out: + if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) { + warn("WARNING: failed to set new scale / offsets for mag"); + } + + /* set back to normal mode */ + /* Set to 1.1 Gauss */ + if (OK != ::ioctl(fd, MAGIOCSRANGE, 1)) { + warnx("failed to set 1.1 Ga range"); + } + + if (OK != ::ioctl(fd, MAGIOCEXSTRAP, 0)) { + warnx("failed to disable sensor calibration mode"); + } + if (ret == OK) { if (!check_scale()) { warnx("mag scale calibration successfully finished."); @@ -1221,6 +1222,7 @@ HMC5883::print_info() perf_print_counter(_comms_errors); perf_print_counter(_buffer_overflows); printf("poll interval: %u ticks\n", _measure_ticks); + printf("output (%.2f %.2f %.2f)\n", (double)_last_report.x, (double)_last_report.y, (double)_last_report.z); printf("offsets (%.2f %.2f %.2f)\n", (double)_scale.x_offset, (double)_scale.y_offset, (double)_scale.z_offset); printf("scaling (%.2f %.2f %.2f) 1/range_scale %.2f range_ga %.2f\n", (double)_scale.x_scale, (double)_scale.y_scale, (double)_scale.z_scale, From f1f9f452c0225b7ebf65007505e6d74e57216cc1 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Sat, 22 Mar 2014 16:57:32 -0700 Subject: [PATCH 316/381] tone_alarm: Added arming failure tone --- src/drivers/drv_tone_alarm.h | 1 + src/drivers/stm32/tone_alarm/tone_alarm.cpp | 2 ++ 2 files changed, 3 insertions(+) diff --git a/src/drivers/drv_tone_alarm.h b/src/drivers/drv_tone_alarm.h index cb5fd53a5f..147d7123a4 100644 --- a/src/drivers/drv_tone_alarm.h +++ b/src/drivers/drv_tone_alarm.h @@ -147,6 +147,7 @@ enum { TONE_BATTERY_WARNING_SLOW_TUNE, TONE_BATTERY_WARNING_FAST_TUNE, TONE_GPS_WARNING_TUNE, + TONE_ARMING_FAILURE_TUNE, TONE_NUMBER_OF_TUNES }; diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index f36f2091e3..8ed0de58c6 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -334,6 +334,7 @@ ToneAlarm::ToneAlarm() : _default_tunes[TONE_BATTERY_WARNING_SLOW_TUNE] = "MBNT100a8"; //battery warning slow _default_tunes[TONE_BATTERY_WARNING_FAST_TUNE] = "MBNT255a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8"; //battery warning fast _default_tunes[TONE_GPS_WARNING_TUNE] = "MFT255L4AAAL1F#"; //gps warning slow + _default_tunes[TONE_ARMING_FAILURE_TUNE] = "MFT255L4<< Date: Sat, 5 Apr 2014 12:42:32 +0200 Subject: [PATCH 317/381] sdlog2: Add system power to logging --- src/modules/sdlog2/sdlog2.c | 25 +++++++++++++++++++++++++ src/modules/sdlog2/sdlog2_messages.h | 14 ++++++++++++++ 2 files changed, 39 insertions(+) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 13981ac544..7a984821f2 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -796,6 +796,8 @@ int sdlog2_thread_main(int argc, char *argv[]) struct telemetry_status_s telemetry; struct range_finder_report range_finder; struct estimator_status_report estimator_status; + struct system_power_s system_power; + struct servorail_status_s servorail_status; } buf; memset(&buf, 0, sizeof(buf)); @@ -828,6 +830,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_DIST_s log_DIST; struct log_TELE_s log_TELE; struct log_ESTM_s log_ESTM; + struct log_PWR_s log_PWR; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -859,6 +862,8 @@ int sdlog2_thread_main(int argc, char *argv[]) int telemetry_sub; int range_finder_sub; int estimator_status_sub; + int system_power_sub; + int servorail_status_sub; } subs; subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); @@ -884,6 +889,8 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.telemetry_sub = orb_subscribe(ORB_ID(telemetry_status)); subs.range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder)); subs.estimator_status_sub = orb_subscribe(ORB_ID(estimator_status)); + subs.system_power_sub = orb_subscribe(ORB_ID(system_power)); + subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status)); thread_running = true; @@ -1226,6 +1233,24 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(BATT); } + /* --- SYSTEM POWER RAILS --- */ + if (copy_if_updated(ORB_ID(system_power), subs.system_power_sub, &buf.system_power)) { + log_msg.msg_type = LOG_PWR_MSG; + log_msg.body.log_PWR.5v_peripherals = buf.system_power.voltage_5V_v; + log_msg.body.log_PWR.usb_ok = buf.system_power.usb_connected; + log_msg.body.log_PWR.brick_ok = buf.system_power.brick_valid; + log_msg.body.log_PWR.servo_ok = buf.system_power.servo_valid; + log_msg.body.log_PWR.low_power_rail_overcurrent = buf.system_power.periph_5V_OC; + log_msg.body.log_PWR.high_power_rail_overcurrent = buf.system_power.hipower_5V_OC; + + /* copy servo rail status topic here too */ + orb_copy(ORB_ID(servorail_status), subs.servorail_status_sub, &buf.servorail_status); + log_msg.body.log_PWR.5v_servo_rail = buf.servorail_status.voltage_v; + log_msg.body.log_PWR.5v_servo_rssi = buf.servorail_status.rssi_v; + + LOGBUFFER_WRITE_AND_COUNT(PWR); + } + /* --- TELEMETRY --- */ if (copy_if_updated(ORB_ID(telemetry_status), subs.telemetry_sub, &buf.telemetry)) { log_msg.msg_type = LOG_TELE_MSG; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 22a6958720..93a150dbea 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -316,6 +316,19 @@ struct log_ESTM_s { // uint8_t kalman_gain_nan; // }; +/* --- PWR - ONBOARD POWER SYSTEM --- */ +#define LOG_PWR_MSG 133 +struct log_PWR_s { + float 5v_peripherals; + float 5v_servo_rail; + float 5v_servo_rssi; + uint8_t usb_ok; + uint8_t brick_ok; + uint8_t servo_ok; + uint8_t low_power_rail_overcurrent; + uint8_t high_power_rail_overcurrent; +}; + #pragma pack(pop) /* construct list of all message formats */ @@ -349,6 +362,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(VER, "NZ", "Arch,FwGit"), LOG_FORMAT(PARM, "Nf", "Name,Value"), LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,n_states,states_nan,cov_nan,kgain_nan"), + LOG_FORMAT(PWR, "fffBBBBB", "5V_PERIPH, 5V_SRV,USB_OK,BRICK_OK,SRV_OK,PERIPH_OC,HIPWR_OC"); //LOG_FORMAT(ESTM, "ffffffffffffffffffffffffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27,s28,s29,s30,s31,n_states,states_nan,cov_nan,kgain_nan"), }; From f21ce7e50cf4e9f77fb7d26508c989ed2ff1f300 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 12:56:15 +0200 Subject: [PATCH 318/381] Compile hotfix for master --- .../fw_att_pos_estimator/estimator.cpp | 17 ++++++++++- src/modules/fw_att_pos_estimator/estimator.h | 30 +++++++++---------- 2 files changed, 31 insertions(+), 16 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp index 7ab06e85d3..c312173938 100644 --- a/src/modules/fw_att_pos_estimator/estimator.cpp +++ b/src/modules/fw_att_pos_estimator/estimator.cpp @@ -106,7 +106,22 @@ void swap_var(float &d1, float &d2) d2 = tmp; } -AttPosEKF::AttPosEKF() +AttPosEKF::AttPosEKF() : + fusionModeGPS(0), + covSkipCount(0), + EAS2TAS(1.0f), + statesInitialised(false), + fuseVelData(false), + fusePosData(false), + fuseHgtData(false), + fuseMagData(false), + fuseVtasData(false), + onGround(true), + staticMode(true), + useAirspeed(true), + useCompass(true), + numericalProtection(true), + storeIndex(0) { } diff --git a/src/modules/fw_att_pos_estimator/estimator.h b/src/modules/fw_att_pos_estimator/estimator.h index 7edb3c714f..e62f1a98a5 100644 --- a/src/modules/fw_att_pos_estimator/estimator.h +++ b/src/modules/fw_att_pos_estimator/estimator.h @@ -108,7 +108,7 @@ public: Vector3f dVelIMU; Vector3f dAngIMU; float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec) - uint8_t fusionModeGPS = 0; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity + 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 @@ -127,8 +127,8 @@ public: float lonRef; // WGS-84 longitude of reference point (rad) float hgtRef; // WGS-84 height of reference point (m) Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes - uint8_t covSkipCount = 0; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction - float EAS2TAS = 1.0f; // ratio f true to equivalent airspeed + uint8_t covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction + float EAS2TAS; // ratio f true to equivalent airspeed // GPS input data variables float gpsCourse; @@ -141,25 +141,25 @@ public: // Baro input float baroHgt; - bool statesInitialised = false; + bool statesInitialised; - bool fuseVelData = false; // this boolean causes the posNE and velNED obs to be fused - bool fusePosData = false; // this boolean causes the posNE and velNED obs to be fused - bool fuseHgtData = false; // this boolean causes the hgtMea obs to be fused - bool fuseMagData = false; // boolean true when magnetometer data is to be fused - bool fuseVtasData = false; // boolean true when airspeed data is to be fused + bool fuseVelData; // this boolean causes the posNE and velNED obs to be fused + bool fusePosData; // this boolean causes the posNE and velNED obs to be fused + bool fuseHgtData; // this boolean causes the hgtMea obs to be fused + bool fuseMagData; // boolean true when magnetometer data is to be fused + bool fuseVtasData; // boolean true when airspeed data is to be fused - bool onGround = true; ///< boolean true when the flight vehicle is on the ground (not flying) - bool staticMode = true; ///< boolean true if no position feedback is fused - bool useAirspeed = true; ///< boolean true if airspeed data is being used - bool useCompass = true; ///< boolean true if magnetometer data is being used + bool onGround; ///< boolean true when the flight vehicle is on the ground (not flying) + bool staticMode; ///< boolean true if no position feedback is fused + bool useAirspeed; ///< boolean true if airspeed data is being used + bool useCompass; ///< boolean true if magnetometer data is being used struct ekf_status_report current_ekf_state; struct ekf_status_report last_ekf_error; - bool numericalProtection = true; + bool numericalProtection; - unsigned storeIndex = 0; + unsigned storeIndex; void UpdateStrapdownEquationsNED(); From 06d5c6ad285a46cfb59ce07bc3cd49507305a997 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 13:03:52 +0200 Subject: [PATCH 319/381] sdlog2: Compile fixes for system power logging --- src/modules/sdlog2/sdlog2.c | 8 +++++--- src/modules/sdlog2/sdlog2_messages.h | 8 ++++---- 2 files changed, 9 insertions(+), 7 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 7a984821f2..9f2fca4465 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -84,6 +84,8 @@ #include #include #include +#include +#include #include #include @@ -1236,7 +1238,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- SYSTEM POWER RAILS --- */ if (copy_if_updated(ORB_ID(system_power), subs.system_power_sub, &buf.system_power)) { log_msg.msg_type = LOG_PWR_MSG; - log_msg.body.log_PWR.5v_peripherals = buf.system_power.voltage_5V_v; + log_msg.body.log_PWR.peripherals_5v = buf.system_power.voltage5V_v; log_msg.body.log_PWR.usb_ok = buf.system_power.usb_connected; log_msg.body.log_PWR.brick_ok = buf.system_power.brick_valid; log_msg.body.log_PWR.servo_ok = buf.system_power.servo_valid; @@ -1245,8 +1247,8 @@ int sdlog2_thread_main(int argc, char *argv[]) /* copy servo rail status topic here too */ orb_copy(ORB_ID(servorail_status), subs.servorail_status_sub, &buf.servorail_status); - log_msg.body.log_PWR.5v_servo_rail = buf.servorail_status.voltage_v; - log_msg.body.log_PWR.5v_servo_rssi = buf.servorail_status.rssi_v; + log_msg.body.log_PWR.servo_rail_5v = buf.servorail_status.voltage_v; + log_msg.body.log_PWR.servo_rssi_5v = buf.servorail_status.rssi_v; LOGBUFFER_WRITE_AND_COUNT(PWR); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 93a150dbea..d629259c0c 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -319,9 +319,9 @@ struct log_ESTM_s { /* --- PWR - ONBOARD POWER SYSTEM --- */ #define LOG_PWR_MSG 133 struct log_PWR_s { - float 5v_peripherals; - float 5v_servo_rail; - float 5v_servo_rssi; + float peripherals_5v; + float servo_rail_5v; + float servo_rssi_5v; uint8_t usb_ok; uint8_t brick_ok; uint8_t servo_ok; @@ -362,7 +362,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(VER, "NZ", "Arch,FwGit"), LOG_FORMAT(PARM, "Nf", "Name,Value"), LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,n_states,states_nan,cov_nan,kgain_nan"), - LOG_FORMAT(PWR, "fffBBBBB", "5V_PERIPH, 5V_SRV,USB_OK,BRICK_OK,SRV_OK,PERIPH_OC,HIPWR_OC"); + LOG_FORMAT(PWR, "fffBBBBB", "5V_PERIPH, 5V_SRV,USB_OK,BRICK_OK,SRV_OK,PERIPH_OC,HIPWR_OC"), //LOG_FORMAT(ESTM, "ffffffffffffffffffffffffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27,s28,s29,s30,s31,n_states,states_nan,cov_nan,kgain_nan"), }; From 00c82f0836388ffa1e1c4a5827eefe28c0521df8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 12 Feb 2014 12:18:20 +1100 Subject: [PATCH 320/381] Merged airspeed changes --- src/drivers/meas_airspeed/meas_airspeed.cpp | 62 ++++++++++++++++++++- 1 file changed, 59 insertions(+), 3 deletions(-) diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 2c3efdc35f..2324475ceb 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -87,6 +87,7 @@ #include #include #include +#include #include @@ -121,6 +122,14 @@ protected: virtual int collect(); math::LowPassFilter2p _filter; + + /** + * Correct for 5V rail voltage variations + */ + void voltage_correction(float &diff_pres_pa); + + int _t_system_power; + struct system_power_s system_power; }; /* @@ -129,10 +138,11 @@ protected: extern "C" __EXPORT int meas_airspeed_main(int argc, char *argv[]); MEASAirspeed::MEASAirspeed(int bus, int address, const char *path) : Airspeed(bus, address, - CONVERSION_INTERVAL, path), - _filter(MEAS_RATE, MEAS_DRIVER_FILTER_FREQ) + CONVERSION_INTERVAL, path), + _filter(MEAS_RATE, MEAS_DRIVER_FILTER_FREQ), + _t_system_power(-1) { - + memset(&system_power, 0, sizeof(system_power)); } int @@ -207,6 +217,9 @@ MEASAirspeed::collect() diff_press_pa = 0.0f; } + // correct for 5V rail voltage if possible + voltage_correction(diff_press_pa); + struct differential_pressure_s report; /* track maximum differential pressure measured (so we can work out top speed). */ @@ -287,6 +300,49 @@ MEASAirspeed::cycle() USEC2TICK(CONVERSION_INTERVAL)); } +/** + 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 + */ +void +MEASAirspeed::voltage_correction(float &diff_press_pa) +{ +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + if (_t_system_power == -1) { + _t_system_power = orb_subscribe(ORB_ID(system_power)); + } + if (_t_system_power == -1) { + // not available + return; + } + bool updated = false; + orb_check(_t_system_power, &updated); + if (updated) { + orb_copy(ORB_ID(system_power), _t_system_power, &system_power); + } + if (system_power.voltage5V_v < 3.0f || system_power.voltage5V_v > 6.0f) { + // not valid, skip correction + return; + } + + const float slope = 70.0f; + /* + apply a piecewise linear correction, flattening at 0.5V from 5V + */ + float voltage_diff = system_power.voltage5V_v - 5.0f; + if (voltage_diff > 0.5f) { + voltage_diff = 0.5f; + } + if (voltage_diff < -0.5f) { + voltage_diff = -0.5f; + } + diff_press_pa -= voltage_diff * slope; +#endif // CONFIG_ARCH_BOARD_PX4FMU_V2 +} + /** * Local functions in support of the shell command. */ From 9719af623d92ceab0c5eebe21906b4d5cf515682 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 12 Feb 2014 13:48:37 +1100 Subject: [PATCH 321/381] Merged voltage compensation --- src/drivers/meas_airspeed/meas_airspeed.cpp | 21 +++++++++++++++++---- 1 file changed, 17 insertions(+), 4 deletions(-) diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 2324475ceb..0b7986383f 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -126,7 +126,7 @@ protected: /** * Correct for 5V rail voltage variations */ - void voltage_correction(float &diff_pres_pa); + void voltage_correction(float &diff_pres_pa, float &temperature); int _t_system_power; struct system_power_s system_power; @@ -204,7 +204,7 @@ MEASAirspeed::collect() dp_raw = 0x3FFF & dp_raw; dT_raw = (val[2] << 8) + val[3]; dT_raw = (0xFFE0 & dT_raw) >> 5; - float temperature = ((200 * dT_raw) / 2047) - 50; + float temperature = ((200.0f * dT_raw) / 2047) - 50; /* calculate differential pressure. As its centered around 8000 * and can go positive or negative, enforce absolute value @@ -218,7 +218,7 @@ MEASAirspeed::collect() } // correct for 5V rail voltage if possible - voltage_correction(diff_press_pa); + voltage_correction(diff_press_pa, temperature); struct differential_pressure_s report; @@ -308,7 +308,7 @@ MEASAirspeed::cycle() offset versus voltage for 3 sensors */ void -MEASAirspeed::voltage_correction(float &diff_press_pa) +MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature) { #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 if (_t_system_power == -1) { @@ -340,6 +340,19 @@ MEASAirspeed::voltage_correction(float &diff_press_pa) voltage_diff = -0.5f; } diff_press_pa -= voltage_diff * slope; + + /* + the temperature masurement varies as well + */ + const float temp_slope = 0.887f; + voltage_diff = system_power.voltage5V_v - 5.0f; + if (voltage_diff > 0.5f) { + voltage_diff = 0.5f; + } + if (voltage_diff < -1.0f) { + voltage_diff = -1.0f; + } + temperature -= voltage_diff * temp_slope; #endif // CONFIG_ARCH_BOARD_PX4FMU_V2 } From cdec5e1d052b587a2e2a5d9d3dfef0bc507cc5b9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 13 Feb 2014 14:25:52 +1100 Subject: [PATCH 322/381] Included raw differential pressure field --- src/drivers/meas_airspeed/meas_airspeed.cpp | 49 ++++++++++++++----- .../uORB/topics/differential_pressure.h | 9 ++-- 2 files changed, 43 insertions(+), 15 deletions(-) diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 0b7986383f..58b128948f 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -206,20 +206,46 @@ MEASAirspeed::collect() dT_raw = (0xFFE0 & dT_raw) >> 5; float temperature = ((200.0f * dT_raw) / 2047) - 50; - /* calculate differential pressure. As its centered around 8000 - * and can go positive or negative, enforce absolute value - */ + // Calculate differential pressure. As its centered around 8000 + // and can go positive or negative const float P_min = -1.0f; const float P_max = 1.0f; - float diff_press_pa = fabsf((((float)dp_raw - 0.1f * 16383.0f) * (P_max - P_min) / (0.8f * 16383.0f) + P_min) * 6894.8f) - _diff_pres_offset; + const float PSI_to_Pa = 6894.757f; + /* + this equation is an inversion of the equation in the + pressure transfer function figure on page 4 of the datasheet - if (diff_press_pa < 0.0f) { - diff_press_pa = 0.0f; - } + We negate the result so that positive differential pressures + are generated when the bottom port is used as the static + port on the pitot and top port is used as the dynamic port + */ + float diff_press_PSI = -((dp_raw - 0.1f*16383) * (P_max-P_min)/(0.8f*16383) + P_min); + float diff_press_pa_raw = diff_press_PSI * PSI_to_Pa; // correct for 5V rail voltage if possible - voltage_correction(diff_press_pa, temperature); + voltage_correction(diff_press_pa_raw, temperature); + float diff_press_pa = fabsf(diff_press_pa_raw - _diff_pres_offset); + + /* + 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 + + 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). */ @@ -232,6 +258,7 @@ MEASAirspeed::collect() report.temperature = temperature; report.differential_pressure_pa = diff_press_pa; report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa); + report.differential_pressure_raw_pa = diff_press_pa_raw; report.voltage = 0; report.max_differential_pressure_pa = _max_differential_pressure_pa; @@ -328,7 +355,7 @@ MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature) return; } - const float slope = 70.0f; + const float slope = 65.0f; /* apply a piecewise linear correction, flattening at 0.5V from 5V */ @@ -478,7 +505,7 @@ test() } warnx("single read"); - warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa); + warnx("diff pressure: %d pa", (int)report.differential_pressure_pa); /* start the sensor polling at 2Hz */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { @@ -506,7 +533,7 @@ test() } warnx("periodic read %u", i); - warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa); + warnx("diff pressure: %d pa", (int)report.differential_pressure_pa); warnx("temperature: %d C (0x%02x)", (int)report.temperature, (unsigned) report.temperature); } diff --git a/src/modules/uORB/topics/differential_pressure.h b/src/modules/uORB/topics/differential_pressure.h index ff88b04c6e..01e14cda9e 100644 --- a/src/modules/uORB/topics/differential_pressure.h +++ b/src/modules/uORB/topics/differential_pressure.h @@ -52,13 +52,14 @@ * Differential pressure. */ struct differential_pressure_s { - uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ - uint64_t error_count; + 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 */ - float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */ - float temperature; /**< Temperature provided by sensor, -1000.0f if unknown */ + float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */ + float temperature; /**< Temperature provided by sensor, -1000.0f if unknown */ }; From 3da219c3db638e0a57d18e892575df13d8c11f47 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 14:14:03 +0200 Subject: [PATCH 323/381] Update airspeed calibration routine to account for new signedness options --- src/modules/commander/airspeed_calibration.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 6039d92a76..c8c7a42e79 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 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 @@ -64,9 +64,9 @@ int do_airspeed_calibration(int mavlink_fd) { /* give directions */ mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); - mavlink_log_info(mavlink_fd, "don't move system"); + mavlink_log_info(mavlink_fd, "ensure airspeed sensor is not registering wind"); - const int calibration_count = 2500; + const int calibration_count = 2000; int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); struct differential_pressure_s diff_pres; @@ -85,13 +85,15 @@ int do_airspeed_calibration(int mavlink_fd) if (fd > 0) { if (OK == ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { paramreset_successful = true; + } else { + mavlink_log_critical(mavlink_fd, "airspeed offset zero failed"); } close(fd); } if (!paramreset_successful) { - warn("WARNING: failed to set scale / offsets for airspeed sensor"); - mavlink_log_critical(mavlink_fd, "could not reset dpress sensor"); + warn("FAILED to set scale / offsets for airspeed"); + mavlink_log_critical(mavlink_fd, "dpress reset failed"); mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); return ERROR; } @@ -107,7 +109,7 @@ int do_airspeed_calibration(int mavlink_fd) if (poll_ret) { orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); - diff_pres_offset += diff_pres.differential_pressure_pa; + diff_pres_offset += diff_pres.differential_pressure_raw_pa; calibration_counter++; if (calibration_counter % (calibration_count / 20) == 0) From be38372be17925e14fa864ad26e8f487a402d46f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 14:28:44 +0200 Subject: [PATCH 324/381] sf0x driver: Stop emitting error messages once there is no hope this sensor will recover - continue to output error messages if the errors are just intermittent --- src/drivers/sf0x/sf0x.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 07163035b5..a0cf98340a 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -124,6 +124,8 @@ private: orb_advert_t _range_finder_topic; + unsigned _consecutive_fail_count; + perf_counter_t _sample_perf; perf_counter_t _comms_errors; perf_counter_t _buffer_overflows; @@ -186,6 +188,7 @@ SF0X::SF0X(const char *port) : _linebuf_index(0), _last_read(0), _range_finder_topic(-1), + _consecutive_fail_count(0), _sample_perf(perf_alloc(PC_ELAPSED, "sf0x_read")), _comms_errors(perf_alloc(PC_COUNT, "sf0x_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "sf0x_buffer_overflows")) @@ -720,12 +723,17 @@ SF0X::cycle() if (OK != collect_ret) { /* we know the sensor needs about four seconds to initialize */ - if (hrt_absolute_time() > 5 * 1000 * 1000LL) { - log("collection error"); + if (hrt_absolute_time() > 5 * 1000 * 1000LL && _consecutive_fail_count < 5) { + log("collection error #%u", _consecutive_fail_count); } + _consecutive_fail_count++; + /* restart the measurement state machine */ start(); return; + } else { + /* apparently success */ + _consecutive_fail_count = 0; } /* next phase is measurement */ From 671d35f67ccae5c35ef4fcb573f5fa52395601c4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 16:05:58 +0200 Subject: [PATCH 325/381] Fix logic for S.Bus failsafe detection --- src/modules/sensors/sensors.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 44a91ca67a..c910062620 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1309,8 +1309,8 @@ Sensors::rc_poll() } /* check for failsafe */ - if (_parameters.rc_fs_thr && (rc_input.rc_failsafe || ((rc_input.values[_rc.function[THROTTLE]] < _parameters.min[i]) && (rc_input.values[_rc.function[THROTTLE]] < _parameters.rc_fs_thr)) - || ((rc_input.values[_rc.function[THROTTLE]] > _parameters.max[i]) && (rc_input.values[_rc.function[THROTTLE]] > _parameters.rc_fs_thr)))) { + if ((rc_input.rc_failsafe) || ((_parameters.rc_fs_thr != 0) && (((rc_input.values[_rc.function[THROTTLE]] < _parameters.min[i]) && (rc_input.values[_rc.function[THROTTLE]] < _parameters.rc_fs_thr)) + || ((rc_input.values[_rc.function[THROTTLE]] > _parameters.max[i]) && (rc_input.values[_rc.function[THROTTLE]] > _parameters.rc_fs_thr))))) { /* do not publish manual control setpoints when there are none */ return; } From de7a6b057f5ced2cf118a919912327f878492cdb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 16:08:28 +0200 Subject: [PATCH 326/381] Cleanups on system power logging format --- src/modules/sdlog2/sdlog2.c | 2 +- src/modules/sdlog2/sdlog2_messages.h | 64 ++++++++++++---------------- 2 files changed, 29 insertions(+), 37 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 9f2fca4465..d2cf6d662b 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1248,7 +1248,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* copy servo rail status topic here too */ orb_copy(ORB_ID(servorail_status), subs.servorail_status_sub, &buf.servorail_status); log_msg.body.log_PWR.servo_rail_5v = buf.servorail_status.voltage_v; - log_msg.body.log_PWR.servo_rssi_5v = buf.servorail_status.rssi_v; + log_msg.body.log_PWR.servo_rssi = buf.servorail_status.rssi_v; LOGBUFFER_WRITE_AND_COUNT(PWR); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index d629259c0c..354bb08e8d 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -277,6 +277,29 @@ struct log_TELE_s { uint8_t txbuf; }; +/* --- ESTM - ESTIMATOR STATUS --- */ +#define LOG_ESTM_MSG 23 +struct log_ESTM_s { + float s[10]; + uint8_t n_states; + uint8_t states_nan; + uint8_t covariance_nan; + uint8_t kalman_gain_nan; +}; + +/* --- PWR - ONBOARD POWER SYSTEM --- */ +#define LOG_PWR_MSG 24 +struct log_PWR_s { + float peripherals_5v; + float servo_rail_5v; + float servo_rssi; + uint8_t usb_ok; + uint8_t brick_ok; + uint8_t servo_ok; + uint8_t low_power_rail_overcurrent; + uint8_t high_power_rail_overcurrent; +}; + /********** SYSTEM MESSAGES, ID > 0x80 **********/ /* --- TIME - TIME STAMP --- */ @@ -299,36 +322,6 @@ struct log_PARM_s { float value; }; -/* --- ESTM - ESTIMATOR STATUS --- */ -#define LOG_ESTM_MSG 132 -struct log_ESTM_s { - float s[10]; - uint8_t n_states; - uint8_t states_nan; - uint8_t covariance_nan; - uint8_t kalman_gain_nan; -}; -// struct log_ESTM_s { -// float s[32]; -// uint8_t n_states; -// uint8_t states_nan; -// uint8_t covariance_nan; -// uint8_t kalman_gain_nan; -// }; - -/* --- PWR - ONBOARD POWER SYSTEM --- */ -#define LOG_PWR_MSG 133 -struct log_PWR_s { - float peripherals_5v; - float servo_rail_5v; - float servo_rssi_5v; - uint8_t usb_ok; - uint8_t brick_ok; - uint8_t servo_ok; - uint8_t low_power_rail_overcurrent; - uint8_t high_power_rail_overcurrent; -}; - #pragma pack(pop) /* construct list of all message formats */ @@ -355,17 +348,16 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"), LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"), LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"), + LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,n_states,states_nan,cov_nan,kgain_nan"), + LOG_FORMAT(PWR, "fffBBBBB", "Periph_5V,Servo_5V,RSSI,USB_OK,BRICK_OK,SRV_OK,PERIPH_OC,HIPWR_OC"), /* system-level messages, ID >= 0x80 */ - // FMT: don't write format of format message, it's useless + /* FMT: don't write format of format message, it's useless */ LOG_FORMAT(TIME, "Q", "StartTime"), LOG_FORMAT(VER, "NZ", "Arch,FwGit"), - LOG_FORMAT(PARM, "Nf", "Name,Value"), - LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,n_states,states_nan,cov_nan,kgain_nan"), - LOG_FORMAT(PWR, "fffBBBBB", "5V_PERIPH, 5V_SRV,USB_OK,BRICK_OK,SRV_OK,PERIPH_OC,HIPWR_OC"), - //LOG_FORMAT(ESTM, "ffffffffffffffffffffffffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27,s28,s29,s30,s31,n_states,states_nan,cov_nan,kgain_nan"), + LOG_FORMAT(PARM, "Nf", "Name,Value") }; -static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s); +static const unsigned log_formats_num = sizeof(log_formats) / sizeof(log_formats[0]); #endif /* SDLOG2_MESSAGES_H_ */ From 7b95d36405cb63b53fd1fea2c25e29aedca5a3a2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 16:45:23 +0200 Subject: [PATCH 327/381] navigator hotfix: Increase acceptance range for yaw setpoints. --- 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 c45cafc1ba..5a94b66717 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1539,7 +1539,7 @@ Navigator::check_mission_item_reached() /* check yaw if defined only for rotary wing except takeoff */ float yaw_err = _wrap_pi(_mission_item.yaw - _global_pos.yaw); - if (fabsf(yaw_err) < 0.05f) { /* XXX get rid of magic number */ + if (fabsf(yaw_err) < 0.2f) { /* XXX get rid of magic number */ _waypoint_yaw_reached = true; } From fc39af08a1d6673aa727a84b17afd6c4485dff19 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 17:02:37 +0200 Subject: [PATCH 328/381] airspeed: Prevent the filter from overshooting into the negative airspeed range --- src/drivers/meas_airspeed/meas_airspeed.cpp | 6 ++++++ .../mathlib/math/filter/LowPassFilter2p.cpp | 7 ++++++- .../mathlib/math/filter/LowPassFilter2p.hpp | 20 +++++++++++++++---- 3 files changed, 28 insertions(+), 5 deletions(-) diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 58b128948f..1ad383ee0b 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -258,6 +258,12 @@ MEASAirspeed::collect() report.temperature = temperature; 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.voltage = 0; report.max_differential_pressure_pa = _max_differential_pressure_pa; diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp index 3699d9bce3..6f640c9f9a 100644 --- a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp +++ b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp @@ -69,7 +69,7 @@ float LowPassFilter2p::apply(float sample) // do the filtering float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2; if (isnan(delay_element_0) || isinf(delay_element_0)) { - // don't allow bad values to propogate via the filter + // don't allow bad values to propagate via the filter delay_element_0 = sample; } float output = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2; @@ -81,5 +81,10 @@ float LowPassFilter2p::apply(float sample) return output; } +float LowPassFilter2p::reset(float sample) { + _delay_element_1 = _delay_element_2 = sample; + return apply(sample); +} + } // namespace math diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.hpp b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp index 208ec98d4e..74cd5d78c0 100644 --- a/src/lib/mathlib/math/filter/LowPassFilter2p.hpp +++ b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp @@ -52,18 +52,30 @@ public: _delay_element_1 = _delay_element_2 = 0; } - // change parameters + /** + * Change filter parameters + */ void set_cutoff_frequency(float sample_freq, float cutoff_freq); - // apply - Add a new raw value to the filter - // and retrieve the filtered result + /** + * Add a new raw value to the filter + * + * @return retrieve the filtered result + */ float apply(float sample); - // return the cutoff frequency + /** + * Return the cutoff frequency + */ float get_cutoff_freq(void) const { return _cutoff_freq; } + /** + * Reset the filter state to this value + */ + float reset(float sample); + private: float _cutoff_freq; float _a1; From 024de1fec431fbd065aeb31035245e7851450a0b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 17:04:36 +0200 Subject: [PATCH 329/381] Remove unwanted colon --- src/modules/px4iofirmware/px4io.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 88ad793980..4db9484848 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -108,7 +108,7 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */ #ifdef CONFIG_ARCH_BOARD_PX4IO_V1 #define r_setup_relays r_page_setup[PX4IO_P_SETUP_RELAYS] #endif -#define r_setup_rc_thr_failsafe r_page_setup[PX4IO_P_SETUP_RC_THR_FAILSAFE_US]; +#define r_setup_rc_thr_failsafe r_page_setup[PX4IO_P_SETUP_RC_THR_FAILSAFE_US] #define r_control_values (&r_page_controls[0]) From 262485a5e87ccdc8ff645ba45992bdbe13363fab Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 17:09:48 +0200 Subject: [PATCH 330/381] px4io: Typo fixed --- 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 a30bfb2dec..e318e206ad 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -951,7 +951,7 @@ PX4IO::task_main() int32_t failsafe_param_val; param_t failsafe_param = param_find("RC_FAILS_THR"); - if (failsafe_param > 0) + if (failsafe_param > 0) { param_get(failsafe_param, &failsafe_param_val); uint16_t failsafe_thr = failsafe_param_val; From e6d48c4f3276af252315118286556b3a000274b6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 17:12:21 +0200 Subject: [PATCH 331/381] Fix failsafe assignment in sensors app --- src/modules/sensors/sensors.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index c910062620..b171b995d2 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1309,8 +1309,8 @@ Sensors::rc_poll() } /* check for failsafe */ - if ((rc_input.rc_failsafe) || ((_parameters.rc_fs_thr != 0) && (((rc_input.values[_rc.function[THROTTLE]] < _parameters.min[i]) && (rc_input.values[_rc.function[THROTTLE]] < _parameters.rc_fs_thr)) - || ((rc_input.values[_rc.function[THROTTLE]] > _parameters.max[i]) && (rc_input.values[_rc.function[THROTTLE]] > _parameters.rc_fs_thr))))) { + if ((rc_input.rc_failsafe) || ((_parameters.rc_fs_thr != 0) && (((rc_input.values[_rc.function[THROTTLE]] < _parameters.min[_rc.function[THROTTLE]]) && (rc_input.values[_rc.function[THROTTLE]] < _parameters.rc_fs_thr)) + || ((rc_input.values[_rc.function[THROTTLE]] > _parameters.max[_rc.function[THROTTLE]]) && (rc_input.values[_rc.function[THROTTLE]] > _parameters.rc_fs_thr))))) { /* do not publish manual control setpoints when there are none */ return; } From 6319ec2036c52f39a6fded6480836fe79e3ba35f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 17:23:34 +0200 Subject: [PATCH 332/381] Add celsius air temperature field to airspeed --- src/modules/uORB/topics/airspeed.h | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/uORB/topics/airspeed.h b/src/modules/uORB/topics/airspeed.h index a3da3758fd..d2ee754cdf 100644 --- a/src/modules/uORB/topics/airspeed.h +++ b/src/modules/uORB/topics/airspeed.h @@ -52,9 +52,10 @@ * Airspeed */ struct airspeed_s { - uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ + uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ float indicated_airspeed_m_s; /**< indicated airspeed in meters per second, -1 if unknown */ - float true_airspeed_m_s; /**< true airspeed in meters per second, -1 if unknown */ + float true_airspeed_m_s; /**< true airspeed in meters per second, -1 if unknown */ + float air_temperature_celsius; /**< air temperature in degrees celsius, -1000 if unknown */ }; /** From c17201afbff06f2be278e202d23eb136f38d1ae1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 17:23:54 +0200 Subject: [PATCH 333/381] Log air temperature --- src/modules/sdlog2/sdlog2.c | 1 + src/modules/sdlog2/sdlog2_messages.h | 3 ++- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index d2cf6d662b..e62b0fafc6 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1193,6 +1193,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.msg_type = LOG_AIRS_MSG; log_msg.body.log_AIRS.indicated_airspeed = buf.airspeed.indicated_airspeed_m_s; log_msg.body.log_AIRS.true_airspeed = buf.airspeed.true_airspeed_m_s; + log_msg.body.log_AIRS.air_temperature_celsius = buf.airspeed.air_temperature_celsius; LOGBUFFER_WRITE_AND_COUNT(AIRS); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 354bb08e8d..a1a5856bc0 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -174,6 +174,7 @@ struct log_OUT0_s { struct log_AIRS_s { float indicated_airspeed; float true_airspeed; + float air_temperature_celsius; }; /* --- ARSP - ATTITUDE RATE SET POINT --- */ @@ -338,7 +339,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(STAT, "BBfBB", "MainState,ArmState,BatRem,BatWarn,Landed"), LOG_FORMAT(RC, "ffffffffB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), - LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"), + LOG_FORMAT(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, "LLfffffB", "Lat,Lon,Alt,VelN,VelE,VelD,BaroAlt,Flags"), From a1a4013d02ca7ec3f62c6c3f2e4b95181f365c35 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 17:24:11 +0200 Subject: [PATCH 334/381] Populate air temperature field --- src/modules/sensors/sensors.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index b171b995d2..4083b8b4f5 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1025,12 +1025,13 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) raw.differential_pressure_timestamp = _diff_pres.timestamp; raw.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa; - float air_temperature_celcius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature : (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG); + 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_celcius); + raw.baro_pres_mbar * 1e2f, air_temperature_celsius); + _airspeed.air_temperature_celsius = air_temperature_celsius; /* announce the airspeed if needed, just publish else */ if (_airspeed_pub > 0) { From 4a0c6600887e900932f6888f1b8948816a1f00b4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 21:06:21 +0200 Subject: [PATCH 335/381] Do not make minimum airspeed assumptions, as we can trust our digital sensor a bit. A blocked pitot also most likely will result in more than just 6.5 m/s airspeed and so the check is based on a bunch of weak assumptions --- src/modules/fw_att_control/fw_att_control_main.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index f139c25f48..522e8c3e6d 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -660,14 +660,13 @@ FixedwingAttitudeControl::task_main() float airspeed; - /* if airspeed is smaller than min, the sensor is not giving good readings */ - if ((_airspeed.indicated_airspeed_m_s < 0.5f * _parameters.airspeed_min) || - !isfinite(_airspeed.indicated_airspeed_m_s) || + /* if airspeed is not updating, we assume the normal average speed */ + if (!isfinite(_airspeed.true_airspeed_m_s) || hrt_elapsed_time(&_airspeed.timestamp) > 1e6) { airspeed = _parameters.airspeed_trim; } else { - airspeed = _airspeed.indicated_airspeed_m_s; + airspeed = _airspeed.true_airspeed_m_s; } float airspeed_scaling = _parameters.airspeed_trim / airspeed; From ab60b13b6dbcf636b1889d2150d96aff8b26cfc9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 21:26:17 +0200 Subject: [PATCH 336/381] fw_att_controller: Forcing actuator scaling to at least minimum speed --- src/modules/fw_att_control/fw_att_control_main.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 522e8c3e6d..2f84dc963b 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -669,8 +669,15 @@ FixedwingAttitudeControl::task_main() airspeed = _airspeed.true_airspeed_m_s; } - float airspeed_scaling = _parameters.airspeed_trim / airspeed; - //warnx("aspd scale: %6.2f act scale: %6.2f", airspeed_scaling, actuator_scaling); + /* + * For scaling our actuators using anything less than the min (close to stall) + * speed doesn't make any sense - its the strongest reasonable deflection we + * want to do in flight and its the baseline a human pilot would choose. + * + * Forcing the scaling to this value allows reasonable handheld tests. + */ + + float airspeed_scaling = _parameters.airspeed_trim / ((airspeed < _parameters.airspeed_min) ? _parameters.airspeed_min : airspeed); float roll_sp = _parameters.rollsp_offset_rad; float pitch_sp = _parameters.pitchsp_offset_rad; From 45cef9eed4a9f1c6761f41a187672cc4ba82cd39 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 6 Apr 2014 11:11:58 +0200 Subject: [PATCH 337/381] mtecs: better (but not final) default parameters for mtecs --- .../fw_pos_control_l1/mtecs/mTecs_params.c | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c index f8db703041..194dfa1508 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c @@ -55,7 +55,7 @@ * @max 1 * @group mTECS */ -PARAM_DEFINE_INT32(MT_ENABLED, 0); +PARAM_DEFINE_INT32(MT_ENABLED, 1); /** * Total Energy Rate Control FF @@ -64,7 +64,7 @@ PARAM_DEFINE_INT32(MT_ENABLED, 0); * @max 10.0 * @group mTECS */ -PARAM_DEFINE_FLOAT(MT_THR_FF, 1.0f); +PARAM_DEFINE_FLOAT(MT_THR_FF, 0.2f); /** * Total Energy Rate Control P @@ -73,7 +73,7 @@ PARAM_DEFINE_FLOAT(MT_THR_FF, 1.0f); * @max 10.0 * @group mTECS */ -PARAM_DEFINE_FLOAT(MT_THR_P, 0.1f); +PARAM_DEFINE_FLOAT(MT_THR_P, 0.03f); /** * Total Energy Rate Control I @@ -82,7 +82,7 @@ PARAM_DEFINE_FLOAT(MT_THR_P, 0.1f); * @max 10.0 * @group mTECS */ -PARAM_DEFINE_FLOAT(MT_THR_I, 0.0f); +PARAM_DEFINE_FLOAT(MT_THR_I, 0.1f); /** * Total Energy Rate Control OFF (Cruise throttle) @@ -100,7 +100,7 @@ PARAM_DEFINE_FLOAT(MT_THR_OFF, 0.7f); * @max 10.0 * @group mTECS */ -PARAM_DEFINE_FLOAT(MT_PIT_FF, 1.0f); +PARAM_DEFINE_FLOAT(MT_PIT_FF, 0.1f); /** * Energy Distribution Rate Control P @@ -109,7 +109,7 @@ PARAM_DEFINE_FLOAT(MT_PIT_FF, 1.0f); * @max 10.0 * @group mTECS */ -PARAM_DEFINE_FLOAT(MT_PIT_P, 0.1f); +PARAM_DEFINE_FLOAT(MT_PIT_P, 0.03f); /** * Energy Distribution Rate Control I @@ -118,7 +118,7 @@ PARAM_DEFINE_FLOAT(MT_PIT_P, 0.1f); * @max 10.0 * @group mTECS */ -PARAM_DEFINE_FLOAT(MT_PIT_I, 0.0f); +PARAM_DEFINE_FLOAT(MT_PIT_I, 0.03f); /** @@ -166,7 +166,7 @@ PARAM_DEFINE_FLOAT(MT_PIT_MIN, -45.0f); * @unit deg * @group mTECS */ -PARAM_DEFINE_FLOAT(MT_PIT_MAX, 45.0f); +PARAM_DEFINE_FLOAT(MT_PIT_MAX, 20.0f); /** * P gain for the altitude control @@ -175,7 +175,7 @@ PARAM_DEFINE_FLOAT(MT_PIT_MAX, 45.0f); * @max 10.0f * @group mTECS */ -PARAM_DEFINE_FLOAT(MT_FPA_P, 0.1f); +PARAM_DEFINE_FLOAT(MT_FPA_P, 0.2f); /** * D gain for the altitude control @@ -202,7 +202,7 @@ PARAM_DEFINE_FLOAT(MT_FPA_D_LP, 1.0f); * @unit deg * @group mTECS */ -PARAM_DEFINE_FLOAT(MT_FPA_MIN, -30.0f); +PARAM_DEFINE_FLOAT(MT_FPA_MIN, -10.0f); /** * Maximal flight path angle setpoint @@ -222,7 +222,7 @@ PARAM_DEFINE_FLOAT(MT_FPA_MAX, 30.0f); * @max 10.0f * @group mTECS */ -PARAM_DEFINE_FLOAT(MT_ACC_P, 0.1f); +PARAM_DEFINE_FLOAT(MT_ACC_P, 1.5f); /** * Minimal acceleration (air) From 7c4f1c90dc441e57d9af55beb33bb6d91ece0c90 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 6 Apr 2014 14:57:45 +0400 Subject: [PATCH 338/381] mavlink_receiver: fixed bug in manual control publication, minor refactoring --- src/modules/mavlink/mavlink_receiver.cpp | 60 ++++++++++++------------ src/modules/mavlink/mavlink_receiver.h | 1 - 2 files changed, 29 insertions(+), 32 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index d7e300670a..3ec40ee0a9 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -222,12 +222,10 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) vcmd.source_component = msg->compid; vcmd.confirmation = cmd_mavlink.confirmation; - /* check if topic is advertised */ - if (_cmd_pub <= 0) { + if (_cmd_pub < 0) { _cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); } else { - /* publish */ orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd); } } @@ -253,7 +251,7 @@ MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg) f.quality = flow.quality; f.sensor_id = flow.sensor_id; - if (_flow_pub <= 0) { + if (_flow_pub < 0) { _flow_pub = orb_advertise(ORB_ID(optical_flow), &f); } else { @@ -287,7 +285,7 @@ MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg) vcmd.source_component = msg->compid; vcmd.confirmation = 1; - if (_cmd_pub <= 0) { + if (_cmd_pub < 0) { _cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); } else { @@ -312,7 +310,7 @@ MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg) vicon_position.pitch = pos.pitch; vicon_position.yaw = pos.yaw; - if (_vicon_position_pub <= 0) { + if (_vicon_position_pub < 0) { _vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position); } else { @@ -373,7 +371,7 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message offboard_control_sp.timestamp = hrt_absolute_time(); - if (_offboard_control_sp_pub <= 0) { + if (_offboard_control_sp_pub < 0) { _offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); } else { @@ -401,7 +399,7 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) tstatus.rxerrors = rstatus.rxerrors; tstatus.fixed = rstatus.fixed; - if (_telemetry_status_pub <= 0) { + if (_telemetry_status_pub < 0) { _telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus); } else { @@ -428,7 +426,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) rc.chan[2].scaled = man.r / 1000.0f; rc.chan[3].scaled = man.z / 1000.0f; - if (_rc_pub == 0) { + if (_rc_pub < 0) { _rc_pub = orb_advertise(ORB_ID(rc_channels), &rc); } else { @@ -450,7 +448,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) manual.yaw = man.r / 1000.0f; manual.throttle = man.z / 1000.0f; - if (_manual_pub == 0) { + if (_manual_pub < 0) { _manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual); } else { @@ -619,11 +617,11 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) hil_sensors.differential_pressure_timestamp = timestamp; /* publish combined sensor topic */ - if (_sensors_pub > 0) { - orb_publish(ORB_ID(sensor_combined), _sensors_pub, &hil_sensors); + if (_sensors_pub < 0) { + _sensors_pub = orb_advertise(ORB_ID(sensor_combined), &hil_sensors); } else { - _sensors_pub = orb_advertise(ORB_ID(sensor_combined), &hil_sensors); + orb_publish(ORB_ID(sensor_combined), _sensors_pub, &hil_sensors); } } @@ -638,11 +636,11 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) hil_battery_status.current_a = 10.0f; hil_battery_status.discharged_mah = -1.0f; - if (_battery_pub > 0) { - orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); + if (_battery_pub < 0) { + _battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); } else { - _battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); + orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); } } @@ -694,11 +692,11 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg) hil_gps.fix_type = gps.fix_type; hil_gps.satellites_visible = gps.satellites_visible; - if (_gps_pub > 0) { - orb_publish(ORB_ID(vehicle_gps_position), _gps_pub, &hil_gps); + if (_gps_pub < 0) { + _gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps); } else { - _gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps); + orb_publish(ORB_ID(vehicle_gps_position), _gps_pub, &hil_gps); } } @@ -752,11 +750,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) hil_attitude.pitchspeed = hil_state.pitchspeed; hil_attitude.yawspeed = hil_state.yawspeed; - if (_attitude_pub > 0) { - orb_publish(ORB_ID(vehicle_attitude), _attitude_pub, &hil_attitude); + if (_attitude_pub < 0) { + _attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude); } else { - _attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude); + orb_publish(ORB_ID(vehicle_attitude), _attitude_pub, &hil_attitude); } } @@ -775,11 +773,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) hil_global_pos.vel_d = hil_state.vz / 100.0f; hil_global_pos.yaw = hil_attitude.yaw; - if (_global_pos_pub > 0) { - orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &hil_global_pos); + if (_global_pos_pub < 0) { + _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); } else { - _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); + orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &hil_global_pos); } } @@ -816,11 +814,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) bool landed = (float)(hil_state.alt) / 1000.0f < (_hil_local_alt0 + 0.1f); // XXX improve? hil_local_pos.landed = landed; - if (_local_pos_pub > 0) { - orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &hil_local_pos); + if (_local_pos_pub < 0) { + _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos); } else { - _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos); + orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &hil_local_pos); } } @@ -857,11 +855,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) hil_battery_status.current_a = 10.0f; hil_battery_status.discharged_mah = -1.0f; - if (_battery_pub > 0) { - orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); + if (_battery_pub < 0) { + _battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); } else { - _battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status); + orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status); } } } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 0a5a1b5c7a..72ce4560fb 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -120,7 +120,6 @@ private: mavlink_status_t status; struct vehicle_local_position_s hil_local_pos; - int _manual_sub; orb_advert_t _global_pos_pub; orb_advert_t _local_pos_pub; orb_advert_t _attitude_pub; From 8fcdff15a480399541b9ecccf8dc8260d437ec7f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 6 Apr 2014 14:58:31 +0200 Subject: [PATCH 339/381] mtecs: add comment about setting the standard limits --- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index 33bfd49622..63e60995e8 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -134,8 +134,8 @@ void mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float fligh } /* Set special ouput limiters if we are not in TECS_MODE_NORMAL */ - BlockOutputLimiter *outputLimiterThrottle = NULL; - BlockOutputLimiter *outputLimiterPitch = NULL; + BlockOutputLimiter *outputLimiterThrottle = NULL; // NULL --> use standard inflight limits + BlockOutputLimiter *outputLimiterPitch = NULL; // NULL --> use standard inflight limits if (mode == TECS_MODE_TAKEOFF) { outputLimiterThrottle = &BlockOutputLimiterTakeoffThrottle; //XXX: accept prelaunch values from launchdetector outputLimiterPitch = &BlockOutputLimiterTakeoffPitch; From e4cfdb4f9f30e0eef36f9e18e8d656d57994141e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 6 Apr 2014 19:34:32 +0200 Subject: [PATCH 340/381] mavlink: Add manual SP subscription --- src/modules/mavlink/mavlink_receiver.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 72ce4560fb..8ccb2a0354 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -138,6 +138,7 @@ private: orb_advert_t _telemetry_status_pub; orb_advert_t _rc_pub; orb_advert_t _manual_pub; + int _manual_sub; int _hil_frames; uint64_t _old_timestamp; bool _hil_local_proj_inited; From 536ff50fe1ddc438283f5f3c66ec9dc3d79d7d83 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 7 Apr 2014 14:38:16 +0200 Subject: [PATCH 341/381] mavlink: in normal mode transmit position setpoint and roll-pitch-yaw-thrust setpoint --- src/modules/mavlink/mavlink_main.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 18df577fea..16c27baa9a 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1783,6 +1783,8 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("LOCAL_POSITION_NED", 3.0f * rate_mult); configure_stream("RC_CHANNELS_RAW", 1.0f * rate_mult); configure_stream("NAMED_VALUE_FLOAT", 1.0f * rate_mult); + configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f * rate_mult); + configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f * rate_mult); break; case MAVLINK_MODE_CAMERA: From de3efc0975f1b84cf556bae11bbebe95609dbcdc Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 7 Apr 2014 17:17:56 +0400 Subject: [PATCH 342/381] mavlink: publish SYS_STATUS at constant rate, don't look at update() result --- src/modules/mavlink/mavlink_messages.cpp | 31 ++++++++++++------------ 1 file changed, 15 insertions(+), 16 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 4ca3840d47..c89031fccf 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -262,22 +262,21 @@ protected: void send(const hrt_abstime t) { - if (status_sub->update(t)) { - mavlink_msg_sys_status_send(_channel, - status->onboard_control_sensors_present, - status->onboard_control_sensors_enabled, - status->onboard_control_sensors_health, - status->load * 1000.0f, - status->battery_voltage * 1000.0f, - status->battery_current * 1000.0f, - status->battery_remaining, - status->drop_rate_comm, - status->errors_comm, - status->errors_count1, - status->errors_count2, - status->errors_count3, - status->errors_count4); - } + status_sub->update(t); + mavlink_msg_sys_status_send(_channel, + status->onboard_control_sensors_present, + status->onboard_control_sensors_enabled, + status->onboard_control_sensors_health, + status->load * 1000.0f, + status->battery_voltage * 1000.0f, + status->battery_current * 1000.0f, + status->battery_remaining, + status->drop_rate_comm, + status->errors_comm, + status->errors_count1, + status->errors_count2, + status->errors_count3, + status->errors_count4); } }; From a4a12dab337f132bfe51db2ad482273ecfdf0ce6 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 7 Apr 2014 21:42:19 +0200 Subject: [PATCH 343/381] commander: put unsupported warning back in place --- src/modules/commander/commander.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 58995fcec3..06f7ae6cc3 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -588,15 +588,15 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe break; default: - /* warn about unsupported commands */ + /* Warn about unsupported commands, this makes sense because only commands + * to this component ID (or all) are passed by mavlink. */ answer_command(*cmd, VEHICLE_CMD_RESULT_UNSUPPORTED); break; } - /* silently ignore unsupported commands, maybe they are passed on over mavlink */ -// if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) { -// /* already warned about unsupported commands in "default" case */ -// answer_command(*cmd, result); -// } + if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) { + /* already warned about unsupported commands in "default" case */ + answer_command(*cmd, result); + } /* send any requested ACKs */ if (cmd->confirmation > 0 && result != VEHICLE_CMD_RESULT_UNSUPPORTED) { From bb3792bcdd8edeb24cad7ef0170c76a552f943d5 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 7 Apr 2014 21:42:48 +0200 Subject: [PATCH 344/381] mavlink: use LL_FOREACH --- src/modules/mavlink/mavlink_main.cpp | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index c5055939ec..58393e013f 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -401,14 +401,12 @@ Mavlink::instance_exists(const char *device_name, Mavlink *self) void Mavlink::forward_message(mavlink_message_t *msg, Mavlink *self) { - Mavlink *inst = ::_mavlink_instances; - - while (inst != nullptr) { - /* don't broadcast to itself */ + + Mavlink *inst; + LL_FOREACH(_mavlink_instances, inst) { if (inst != self) { inst->pass_message(msg); } - inst = inst->next; } } From 38c3e68976c8dc167b4d1e5d24792401fc7cc7d3 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 7 Apr 2014 21:44:01 +0200 Subject: [PATCH 345/381] Send camera command to all, use own sysid --- src/modules/mavlink/mavlink_messages.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 2b5d650807..1e6bd6f931 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1265,13 +1265,11 @@ protected: || status->arming_state == ARMING_STATE_ARMED_ERROR) { /* send camera capture on */ - /* XXX TODO: get param for system ID */ - mavlink_msg_command_long_send(_channel, 42, 250, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); + mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); } else { /* send camera capture off */ - /* XXX TODO: get param for system ID */ - mavlink_msg_command_long_send(_channel, 42, 250, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0); + mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0); } } }; From 814d3c385cc0132ec9bbdc22f87f35301bbc8f44 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 8 Apr 2014 11:03:23 +0200 Subject: [PATCH 346/381] gps driver: fake mode: lower eph and epv values in order to convince the commander that the gps signal is valid --- src/drivers/gps/gps.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index c72f692d73..a902bdf2f5 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -282,8 +282,8 @@ GPS::task_main() _report.p_variance_m = 10.0f; _report.c_variance_rad = 0.1f; _report.fix_type = 3; - _report.eph_m = 10.0f; - _report.epv_m = 10.0f; + _report.eph_m = 3.0f; + _report.epv_m = 7.0f; _report.timestamp_velocity = hrt_absolute_time(); _report.vel_n_m_s = 0.0f; _report.vel_e_m_s = 0.0f; From 134633c7f0ef6ec668ba9c122a52b024c393e51a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 8 Apr 2014 15:15:06 +0200 Subject: [PATCH 347/381] mavlink: accessor for _mavlink_fd --- src/modules/mavlink/mavlink_main.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 5a118a0ad9..4e6d585ab9 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -196,6 +196,8 @@ public: bool _task_should_exit; /**< if true, mavlink task should exit */ + int get_mavlink_fd() { return _mavlink_fd; } + protected: Mavlink *next; From a6215b7bda47616018d29b2f7de630deb4984be9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 8 Apr 2014 17:06:52 +0200 Subject: [PATCH 348/381] commander: handle_command: filter commands by sysid and componentid --- 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 69a45a02f3..89b8e684df 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -395,6 +395,11 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe enum VEHICLE_CMD_RESULT result = VEHICLE_CMD_RESULT_UNSUPPORTED; bool ret = false; + /* only handle commands that are meant to be handled by this system and component */ + if (cmd->target_system != status->system_id || cmd->target_component != status->component_id) { + return false; + } + /* only handle high-priority commands here */ /* request to set different system mode */ From f0d043c06849a2f2f5e07d6f3bdfafc0e4e6f041 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 8 Apr 2014 17:08:35 +0200 Subject: [PATCH 349/381] mavlink: COMMAND_LONG stream: listen to vehicle_command uorb topic and send mavlink_msg_command_long --- src/modules/mavlink/mavlink_messages.cpp | 46 ++++++++++++++++++++++++ 1 file changed, 46 insertions(+) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 45b86a497b..4d7b9160d3 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1273,6 +1273,51 @@ protected: } }; +class MavlinkStreamCommandLong : public MavlinkStream +{ +public: + const char *get_name() + { + return "COMMAND_LONG"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamCommandLong(); + } + +private: + MavlinkOrbSubscription *vehicle_command_sub; + struct vehicle_command_s *vehicle_command; + +protected: + void subscribe(Mavlink *mavlink) + { + vehicle_command_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_command)); + vehicle_command = (struct vehicle_command_s *)vehicle_command_sub->get_data(); + } + + void send(const hrt_abstime t) + { + if (vehicle_command_sub->update(t)) { + if (!((vehicle_command->target_system == mavlink_system.sysid) && (vehicle_command->target_component == mavlink_system.compid))) { + mavlink_msg_command_long_send(_channel, + vehicle_command->target_system, + vehicle_command->target_component, + vehicle_command->command, + vehicle_command->confirmation, + vehicle_command->param1, + vehicle_command->param2, + vehicle_command->param3, + vehicle_command->param4, + vehicle_command->param5, + vehicle_command->param6, + vehicle_command->param7); + } + } + } +}; + MavlinkStream *streams_list[] = { new MavlinkStreamHeartbeat(), new MavlinkStreamSysStatus(), @@ -1299,5 +1344,6 @@ MavlinkStream *streams_list[] = { new MavlinkStreamAttitudeControls(), new MavlinkStreamNamedValueFloat(), new MavlinkStreamCameraCapture(), + new MavlinkStreamCommandLong(), nullptr }; From bccdbde45eb70271bd946ca86173cd8add20cd3d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 8 Apr 2014 17:29:11 +0200 Subject: [PATCH 350/381] commander: handle_command: do not filter command if componentid == 0 --- 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 89b8e684df..ce6de88ef9 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -396,7 +396,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe bool ret = false; /* only handle commands that are meant to be handled by this system and component */ - if (cmd->target_system != status->system_id || cmd->target_component != status->component_id) { + if (cmd->target_system != status->system_id || ((cmd->target_component != status->component_id) && (cmd->target_component != 0))) { // component_id 0: valid for all components return false; } From 523606668f8c940357cd9c46b0995035eced7659 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 8 Apr 2014 22:23:18 +0400 Subject: [PATCH 351/381] position_estimator_inav: make land detector more sensitive to LANDED -> IN AIR transitions --- .../position_estimator_inav/position_estimator_inav_main.c | 2 +- .../position_estimator_inav/position_estimator_inav_params.c | 2 +- 2 files changed, 2 insertions(+), 2 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 368fa6ee25..763b875635 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -763,7 +763,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float thrust = armed.armed ? actuator.control[3] : 0.0f; if (landed) { - if (alt_disp2 > land_disp2 && thrust > params.land_thr) { + if (alt_disp2 > land_disp2 || thrust > params.land_thr) { landed = false; landed_time = 0; } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index b71f9472f5..dcad5c03b6 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -56,7 +56,7 @@ PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.05f); PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f); PARAM_DEFINE_FLOAT(INAV_LAND_T, 3.0f); PARAM_DEFINE_FLOAT(INAV_LAND_DISP, 0.7f); -PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.3f); +PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.2f); int parameters_init(struct position_estimator_inav_param_handles *h) { From 09093b17daf8f02f87ea689dc73bd35b6cac1542 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 8 Apr 2014 23:28:52 +0400 Subject: [PATCH 352/381] mavlink: commands stream implemented --- src/modules/mavlink/mavlink_commands.cpp | 73 ++++++++++++++++++++++++ src/modules/mavlink/mavlink_commands.h | 65 +++++++++++++++++++++ src/modules/mavlink/mavlink_main.cpp | 6 ++ src/modules/mavlink/module.mk | 3 +- 4 files changed, 146 insertions(+), 1 deletion(-) create mode 100644 src/modules/mavlink/mavlink_commands.cpp create mode 100644 src/modules/mavlink/mavlink_commands.h diff --git a/src/modules/mavlink/mavlink_commands.cpp b/src/modules/mavlink/mavlink_commands.cpp new file mode 100644 index 0000000000..1c1e097a43 --- /dev/null +++ b/src/modules/mavlink/mavlink_commands.cpp @@ -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. + * + ****************************************************************************/ + +/** + * @file mavlink_commands.cpp + * Mavlink commands stream implementation. + * + * @author Anton Babushkin + */ + +#include "mavlink_commands.h" + +MavlinkCommandsStream::MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel) : _channel(channel) +{ + _cmd_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_command)); + _cmd = (struct vehicle_command_s *)_cmd_sub->get_data(); +} + +MavlinkCommandsStream::~MavlinkCommandsStream() +{ +} + +void +MavlinkCommandsStream::update(const hrt_abstime t) +{ + if (_cmd_sub->update(t)) { + /* only send commands for other systems/components */ + if (_cmd->target_system != mavlink_system.sysid || _cmd->target_component != mavlink_system.compid) { + mavlink_msg_command_long_send(_channel, + _cmd->target_system, + _cmd->target_component, + _cmd->command, + _cmd->confirmation, + _cmd->param1, + _cmd->param2, + _cmd->param3, + _cmd->param4, + _cmd->param5, + _cmd->param6, + _cmd->param7); + } + } +} diff --git a/src/modules/mavlink/mavlink_commands.h b/src/modules/mavlink/mavlink_commands.h new file mode 100644 index 0000000000..6255d65dff --- /dev/null +++ b/src/modules/mavlink/mavlink_commands.h @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * 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_commands.h + * Mavlink commands stream definition. + * + * @author Anton Babushkin + */ + +#ifndef MAVLINK_COMMANDS_H_ +#define MAVLINK_COMMANDS_H_ + +#include +#include + +class Mavlink; +class MavlinkCommansStream; + +#include "mavlink_main.h" + +class MavlinkCommandsStream +{ +private: + MavlinkOrbSubscription *_cmd_sub; + struct vehicle_command_s *_cmd; + mavlink_channel_t _channel; + +public: + MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel); + ~MavlinkCommandsStream(); + void update(const hrt_abstime t); +}; + +#endif /* MAVLINK_COMMANDS_H_ */ diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 3d897431a1..1ed3f40014 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -81,6 +81,7 @@ #include "mavlink_messages.h" #include "mavlink_receiver.h" #include "mavlink_rate_limiter.h" +#include "mavlink_commands.h" /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -1920,6 +1921,8 @@ Mavlink::task_main(int argc, char *argv[]) struct vehicle_status_s *status = (struct vehicle_status_s *) status_sub->get_data(); + MavlinkCommandsStream commands_stream(this, _channel); + /* add default streams depending on mode and intervals depending on datarate */ float rate_mult = _datarate / 1000.0f; @@ -1982,6 +1985,9 @@ Mavlink::task_main(int argc, char *argv[]) set_hil_enabled(status->hil_state == HIL_STATE_ON); } + /* update commands stream */ + commands_stream.update(t); + /* check for requested subscriptions */ if (_subscribe_to_stream != nullptr) { if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) { diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index f09efa2e6c..515fbfadce 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -42,6 +42,7 @@ SRCS += mavlink_main.cpp \ mavlink_orb_subscription.cpp \ mavlink_messages.cpp \ mavlink_stream.cpp \ - mavlink_rate_limiter.cpp + mavlink_rate_limiter.cpp \ + mavlink_commands.cpp INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink From d55e64d1e54542762510387a22897f504c68a5a6 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 8 Apr 2014 23:29:40 +0400 Subject: [PATCH 353/381] mavlink: minor comments and formatting fixes --- src/modules/mavlink/mavlink_messages.cpp | 2 -- src/modules/mavlink/mavlink_stream.h | 2 +- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 45b86a497b..47603b3903 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1252,8 +1252,6 @@ protected: { status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status)); status = (struct vehicle_status_s *)status_sub->get_data(); - - } void send(const hrt_abstime t) diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index 135e1bce08..def40d9adc 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file mavlink_stream.cpp + * @file mavlink_stream.h * Mavlink messages stream definition. * * @author Anton Babushkin From 27884e49bedfdbd0e259bb9b90e757b45bf7fd44 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 8 Apr 2014 21:40:05 +0200 Subject: [PATCH 354/381] Revert "Merge pull request #816 from PX4/mavlink_commandlongstream" This reverts commit 00ef10f307d3c4a262a15ab5747d854eb4c568d5, reversing changes made to d55e64d1e54542762510387a22897f504c68a5a6. --- src/modules/mavlink/mavlink_messages.cpp | 46 ------------------------ 1 file changed, 46 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index bb8bba0370..47603b3903 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1271,51 +1271,6 @@ protected: } }; -class MavlinkStreamCommandLong : public MavlinkStream -{ -public: - const char *get_name() - { - return "COMMAND_LONG"; - } - - MavlinkStream *new_instance() - { - return new MavlinkStreamCommandLong(); - } - -private: - MavlinkOrbSubscription *vehicle_command_sub; - struct vehicle_command_s *vehicle_command; - -protected: - void subscribe(Mavlink *mavlink) - { - vehicle_command_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_command)); - vehicle_command = (struct vehicle_command_s *)vehicle_command_sub->get_data(); - } - - void send(const hrt_abstime t) - { - if (vehicle_command_sub->update(t)) { - if (!((vehicle_command->target_system == mavlink_system.sysid) && (vehicle_command->target_component == mavlink_system.compid))) { - mavlink_msg_command_long_send(_channel, - vehicle_command->target_system, - vehicle_command->target_component, - vehicle_command->command, - vehicle_command->confirmation, - vehicle_command->param1, - vehicle_command->param2, - vehicle_command->param3, - vehicle_command->param4, - vehicle_command->param5, - vehicle_command->param6, - vehicle_command->param7); - } - } - } -}; - MavlinkStream *streams_list[] = { new MavlinkStreamHeartbeat(), new MavlinkStreamSysStatus(), @@ -1342,6 +1297,5 @@ MavlinkStream *streams_list[] = { new MavlinkStreamAttitudeControls(), new MavlinkStreamNamedValueFloat(), new MavlinkStreamCameraCapture(), - new MavlinkStreamCommandLong(), nullptr }; From 63dae04c22009512abe9a0450480698de2bba9db Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 9 Apr 2014 17:06:06 +0200 Subject: [PATCH 355/381] pauls estimator: Added NaN guard before publishing --- src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 840cd585ec..8ea6118022 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -959,7 +959,7 @@ FixedwingEstimator::task_main() } // Publish results - if (_initialized) { + if (_initialized && (check == OK)) { From 863cd2e838a5e30efd817520c3fceb3847dd18f6 Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Wed, 9 Apr 2014 17:11:27 +0200 Subject: [PATCH 356/381] Added VICON logging, finally. --- src/modules/sdlog2/sdlog2.c | 30 ++++++++++++++++++---------- src/modules/sdlog2/sdlog2_messages.h | 12 +++++++++++ 2 files changed, 31 insertions(+), 11 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index e62b0fafc6..83ee934084 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -226,11 +226,11 @@ sdlog2_usage(const char *reason) } errx(1, "usage: sdlog2 {start|stop|status} [-r ] [-b ] -e -a -t\n" - "\t-r\tLog rate in Hz, 0 means unlimited rate\n" - "\t-b\tLog buffer size in KiB, default is 8\n" - "\t-e\tEnable logging by default (if not, can be started by command)\n" - "\t-a\tLog only when armed (can be still overriden by command)\n" - "\t-t\tUse date/time for naming log directories and files\n"); + "\t-r\tLog rate in Hz, 0 means unlimited rate\n" + "\t-b\tLog buffer size in KiB, default is 8\n" + "\t-e\tEnable logging by default (if not, can be started by command)\n" + "\t-a\tLog only when armed (can be still overriden by command)\n" + "\t-t\tUse date/time for naming log directories and files\n"); } /** @@ -257,11 +257,11 @@ int sdlog2_main(int argc, char *argv[]) main_thread_should_exit = false; deamon_task = task_spawn_cmd("sdlog2", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT - 30, - 3000, - sdlog2_thread_main, - (const char **)argv); + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT - 30, + 3000, + sdlog2_thread_main, + (const char **)argv); exit(0); } @@ -833,6 +833,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_TELE_s log_TELE; struct log_ESTM_s log_ESTM; struct log_PWR_s log_PWR; + struct log_VICON_s log_VICON; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -1163,7 +1164,14 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- VICON POSITION --- */ if (copy_if_updated(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos)) { - // TODO not implemented yet + log_msg.msg_type = LOG_VICON_MSG; + log_msg.body.log_VICON.x = buf.vicon_pos.x; + log_msg.body.log_VICON.y = buf.vicon_pos.y; + log_msg.body.log_VICON.z = buf.vicon_pos.z; + log_msg.body.log_VICON.pitch = buf.vicon_pos.pitch; + log_msg.body.log_VICON.roll = buf.vicon_pos.roll; + log_msg.body.log_VICON.yaw = buf.vicon_pos.yaw; + LOGBUFFER_WRITE_AND_COUNT(VICON); } /* --- FLOW --- */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index a1a5856bc0..eee5ae590b 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -301,6 +301,17 @@ struct log_PWR_s { uint8_t high_power_rail_overcurrent; }; +/* --- VICON - VICON POSITION --- */ +#define LOG_VICON_MSG 25 +struct log_VICON_s { + float x; + float y; + float z; + float roll; + float pitch; + float yaw; +}; + /********** SYSTEM MESSAGES, ID > 0x80 **********/ /* --- TIME - TIME STAMP --- */ @@ -351,6 +362,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"), LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,n_states,states_nan,cov_nan,kgain_nan"), LOG_FORMAT(PWR, "fffBBBBB", "Periph_5V,Servo_5V,RSSI,USB_OK,BRICK_OK,SRV_OK,PERIPH_OC,HIPWR_OC"), + LOG_FORMAT(VICON, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"), /* system-level messages, ID >= 0x80 */ /* FMT: don't write format of format message, it's useless */ From 284218174cad1817b5dc542ecb1527f6ec58bb2d Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Wed, 9 Apr 2014 17:21:07 +0200 Subject: [PATCH 357/381] Tabs tabs tabs --- 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 83ee934084..8dfe172284 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1164,14 +1164,14 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- VICON POSITION --- */ if (copy_if_updated(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos)) { - log_msg.msg_type = LOG_VICON_MSG; - log_msg.body.log_VICON.x = buf.vicon_pos.x; - log_msg.body.log_VICON.y = buf.vicon_pos.y; - log_msg.body.log_VICON.z = buf.vicon_pos.z; - log_msg.body.log_VICON.pitch = buf.vicon_pos.pitch; - log_msg.body.log_VICON.roll = buf.vicon_pos.roll; - log_msg.body.log_VICON.yaw = buf.vicon_pos.yaw; - LOGBUFFER_WRITE_AND_COUNT(VICON); + log_msg.msg_type = LOG_VICON_MSG; + log_msg.body.log_VICON.x = buf.vicon_pos.x; + log_msg.body.log_VICON.y = buf.vicon_pos.y; + log_msg.body.log_VICON.z = buf.vicon_pos.z; + log_msg.body.log_VICON.pitch = buf.vicon_pos.pitch; + log_msg.body.log_VICON.roll = buf.vicon_pos.roll; + log_msg.body.log_VICON.yaw = buf.vicon_pos.yaw; + LOGBUFFER_WRITE_AND_COUNT(VICON); } /* --- FLOW --- */ From 5b090a7095a25f31dd040935defa2363a60b3107 Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Wed, 9 Apr 2014 17:25:33 +0200 Subject: [PATCH 358/381] VICON -> VICN --- src/modules/sdlog2/sdlog2_messages.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index eee5ae590b..affd512ccf 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -362,7 +362,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"), LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,n_states,states_nan,cov_nan,kgain_nan"), LOG_FORMAT(PWR, "fffBBBBB", "Periph_5V,Servo_5V,RSSI,USB_OK,BRICK_OK,SRV_OK,PERIPH_OC,HIPWR_OC"), - LOG_FORMAT(VICON, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"), + LOG_FORMAT(VICN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"), /* system-level messages, ID >= 0x80 */ /* FMT: don't write format of format message, it's useless */ From a39382dc174d646c5ecda8c15c6e0db9c1c4e703 Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Wed, 9 Apr 2014 17:28:00 +0200 Subject: [PATCH 359/381] VICON -> VICN really this time. --- src/modules/sdlog2/sdlog2.c | 18 +++++++++--------- src/modules/sdlog2/sdlog2_messages.h | 6 +++--- 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 8dfe172284..1bf6f1de3e 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -833,7 +833,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_TELE_s log_TELE; struct log_ESTM_s log_ESTM; struct log_PWR_s log_PWR; - struct log_VICON_s log_VICON; + struct log_VICN_s log_VICN; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -1164,14 +1164,14 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- VICON POSITION --- */ if (copy_if_updated(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos)) { - log_msg.msg_type = LOG_VICON_MSG; - log_msg.body.log_VICON.x = buf.vicon_pos.x; - log_msg.body.log_VICON.y = buf.vicon_pos.y; - log_msg.body.log_VICON.z = buf.vicon_pos.z; - log_msg.body.log_VICON.pitch = buf.vicon_pos.pitch; - log_msg.body.log_VICON.roll = buf.vicon_pos.roll; - log_msg.body.log_VICON.yaw = buf.vicon_pos.yaw; - LOGBUFFER_WRITE_AND_COUNT(VICON); + log_msg.msg_type = LOG_VICN_MSG; + log_msg.body.log_VICN.x = buf.vicon_pos.x; + log_msg.body.log_VICN.y = buf.vicon_pos.y; + log_msg.body.log_VICN.z = buf.vicon_pos.z; + log_msg.body.log_VICN.pitch = buf.vicon_pos.pitch; + log_msg.body.log_VICN.roll = buf.vicon_pos.roll; + log_msg.body.log_VICN.yaw = buf.vicon_pos.yaw; + LOGBUFFER_WRITE_AND_COUNT(VICN); } /* --- FLOW --- */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index affd512ccf..d0585df394 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -301,9 +301,9 @@ struct log_PWR_s { uint8_t high_power_rail_overcurrent; }; -/* --- VICON - VICON POSITION --- */ -#define LOG_VICON_MSG 25 -struct log_VICON_s { +/* --- VICN - VICON POSITION --- */ +#define LOG_VICN_MSG 25 +struct log_VICN_s { float x; float y; float z; From b1e4b34a56bfaaa98adb253195d9751f960d5a94 Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Wed, 9 Apr 2014 17:58:18 +0200 Subject: [PATCH 360/381] Logging changes, baud rate changes. --- ROMFS/px4fmu_common/init.d/rc.logging | 2 +- ROMFS/px4fmu_common/init.d/rcS | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging index c5aef8273c..d6fbe8c4fa 100644 --- a/ROMFS/px4fmu_common/init.d/rc.logging +++ b/ROMFS/px4fmu_common/init.d/rc.logging @@ -8,7 +8,7 @@ then if hw_ver compare PX4FMU_V1 then echo "Start sdlog2 at 50Hz" - sdlog2 start -r 50 -a -b 8 -t + sdlog2 start -r 50 -a -b 1 -t -e else echo "Start sdlog2 at 200Hz" sdlog2 start -r 200 -a -b 16 -t diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 0cb6d281a2..a62f67718b 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -410,7 +410,7 @@ then if [ $TTYS1_BUSY == yes ] then # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else - set MAVLINK_FLAGS "-r 1000 -d /dev/ttyS0" + set MAVLINK_FLAGS "-r 1000 -d /dev/ttyS0 -b 115200" # Exit from nsh to free port for mavlink set EXIT_ON_END yes From e99291d825cfc89c67cadd8487e568d3b21d218f Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Thu, 10 Apr 2014 09:08:37 +0200 Subject: [PATCH 361/381] Added vicon stream. --- src/modules/mavlink/mavlink_messages.cpp | 42 ++++++++++++++++++++++++ 1 file changed, 42 insertions(+) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 47603b3903..37929edac6 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -640,6 +640,47 @@ protected: }; + +class MavlinkStreamViconPositionEstimate : public MavlinkStream +{ +public: + const char *get_name() + { + return "VICON_POSITION_ESTIMATE"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamViconPositionEstimate(); + } + +private: + MavlinkOrbSubscription *pos_sub; + struct vehicle_vicon_position_s *pos; + +protected: + void subscribe(Mavlink *mavlink) + { + pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position)); + pos = (struct vehicle_vicon_position_s *)pos_sub->get_data(); + } + + void send(const hrt_abstime t) + { + if (pos_sub->update(t)) { + mavlink_msg_vicon_position_estimate_send(_channel, + pos->timestamp / 1000, + pos->x, + pos->y, + pos->z, + pos->roll, + pos->pitch, + pos->yaw); + } + } +}; + + class MavlinkStreamGPSGlobalOrigin : public MavlinkStream { public: @@ -1297,5 +1338,6 @@ MavlinkStream *streams_list[] = { new MavlinkStreamAttitudeControls(), new MavlinkStreamNamedValueFloat(), new MavlinkStreamCameraCapture(), + new MavlinkStreamViconPositionEstimate(), nullptr }; From 7b85c455558db7971c97c4e8b4d356ebc28b30a1 Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Thu, 10 Apr 2014 09:08:47 +0200 Subject: [PATCH 362/381] Set startup script to do mavlinks. --- ROMFS/px4fmu_common/init.d/rcS | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index a62f67718b..f0bdf32f86 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -410,7 +410,7 @@ then if [ $TTYS1_BUSY == yes ] then # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else - set MAVLINK_FLAGS "-r 1000 -d /dev/ttyS0 -b 115200" + set MAVLINK_FLAGS "-r 1000 -d /dev/ttyS0 -b 57600" # Exit from nsh to free port for mavlink set EXIT_ON_END yes @@ -421,8 +421,16 @@ then fi fi + # Main mavlink mavlink start $MAVLINK_FLAGS - + # Optical Flow + mavlink start -d /dev/ttyS3 -m custom -b 115200 + # Android board + #mavlink start -d /dev/ttyS2 -m custom -b 115200 + #mavlink stream -d /dev/ttyS2 -s LOCAL_POSITION_NED -r 5 + #mavlink stream -d /dev/ttyS2 -s ATTITUDE -r 50 + #mavlink stream -d /dev/ttyS2 -s VICON_POSITION_ESTIMATE -r 50 + # # Start the datamanager # From e6542653b9d013d5cb1b1c0f01ee9af7de4abe5b Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Thu, 10 Apr 2014 10:26:15 +0200 Subject: [PATCH 363/381] Finished adding a '-w' option. --- src/modules/mavlink/mavlink_main.cpp | 37 +++++++++++++++--------- src/modules/mavlink/mavlink_main.h | 12 ++++++++ src/modules/mavlink/mavlink_receiver.cpp | 4 +++ 3 files changed, 40 insertions(+), 13 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 1ed3f40014..b9e0663b71 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -167,12 +167,12 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length int buf_free = 0; if (instance->get_flow_control_enabled() - && ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) { + && ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) { if (buf_free == 0) { if (last_write_times[(unsigned)channel] != 0 && - hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500 * 1000UL) { + hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500 * 1000UL) { warnx("DISABLING HARDWARE FLOW CONTROL"); instance->enable_flow_control(false); @@ -186,12 +186,17 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length } } - ssize_t ret = write(uart, ch, desired); - - if (ret != desired) { - // XXX do something here, but change to using FIONWRITE and OS buf size for detection + /* If the wait until transmit flag is on, only transmit after we've received messages. + Otherwise, transmit all the time. */ + if (instance->should_transmit()) { + ssize_t ret = write(uart, ch, desired); + if (ret != desired) { + // XXX do something here, but change to using FIONWRITE and OS buf size for detection + } } + + } static void usage(void); @@ -204,6 +209,8 @@ Mavlink::Mavlink() : _task_running(false), _hil_enabled(false), _is_usb_uart(false), + _wait_to_transmit(false), + _received_messages(false), _main_loop_delay(1000), _subscriptions(nullptr), _streams(nullptr), @@ -1768,7 +1775,7 @@ Mavlink::task_main(int argc, char *argv[]) * set error flag instead */ bool err_flag = false; - while ((ch = getopt(argc, argv, "b:r:d:m:fpv")) != EOF) { + while ((ch = getopt(argc, argv, "b:r:d:m:fpvw")) != EOF) { switch (ch) { case 'b': _baudrate = strtoul(optarg, NULL, 10); @@ -1820,6 +1827,10 @@ Mavlink::task_main(int argc, char *argv[]) _verbose = true; break; + case 'w': + _wait_to_transmit = true; + break; + default: err_flag = true; break; @@ -2164,11 +2175,11 @@ Mavlink::start(int argc, char *argv[]) // task - start_helper() only returns // when the started task exits. task_spawn_cmd(buf, - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 2048, - (main_t)&Mavlink::start_helper, - (const char **)argv); + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2048, + (main_t)&Mavlink::start_helper, + (const char **)argv); // Ensure that this shell command // does not return before the instance @@ -2264,7 +2275,7 @@ Mavlink::stream(int argc, char *argv[]) static void usage() { - warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate] [-r rate] [-m mode] [-s stream] [-f] [-p] [-v]"); + warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate] [-r rate] [-m mode] [-s stream] [-f] [-p] [-v] [-w]"); } int mavlink_main(int argc, char *argv[]) diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 9941a5f993..2c18261399 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -202,6 +202,14 @@ public: int get_mavlink_fd() { return _mavlink_fd; } + + /* Functions for waiting to start transmission until message received. */ + void set_has_received_messages(bool received_messages) { _received_messages = received_messages; } + bool get_has_received_messages() { return _received_messages; } + void set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; } + bool get_wait_to_transmit() { return _wait_to_transmit; } + bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); } + protected: Mavlink *next; @@ -216,6 +224,8 @@ private: /* states */ bool _hil_enabled; /**< Hardware In the Loop 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. */ unsigned _main_loop_delay; /**< mainloop delay, depends on data rate */ @@ -270,6 +280,8 @@ private: pthread_mutex_t _message_buffer_mutex; + + /** * Send one parameter. * diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index c66350f5ba..61ef2b043e 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -181,6 +181,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) break; } } + + /* If we've received a valid message, mark the flag indicating so. + This is used in the '-w' command-line flag. */ + _mavlink->set_has_received_messages(true); } void From 241137d2b9ecc960af3b722d1b0a1a5d9c74e52d Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Thu, 10 Apr 2014 10:26:33 +0200 Subject: [PATCH 364/381] Fixed final parameters in rcS --- ROMFS/px4fmu_common/init.d/rcS | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index f0bdf32f86..e37cab04c6 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -422,14 +422,14 @@ then fi # Main mavlink - mavlink start $MAVLINK_FLAGS + mavlink start -d /dev/ttyS0 -b 57600 + mavlink stream -d /dev/ttyS0 -s OPTICAL_FLOW -r 10 # Optical Flow mavlink start -d /dev/ttyS3 -m custom -b 115200 # Android board - #mavlink start -d /dev/ttyS2 -m custom -b 115200 - #mavlink stream -d /dev/ttyS2 -s LOCAL_POSITION_NED -r 5 - #mavlink stream -d /dev/ttyS2 -s ATTITUDE -r 50 - #mavlink stream -d /dev/ttyS2 -s VICON_POSITION_ESTIMATE -r 50 + mavlink start -d /dev/ttyS2 -m custom -b 115200 -w + mavlink stream -d /dev/ttyS2 -s ATTITUDE -r 50 + #mavlink stream -d /dev/ttyS2 -s VICON_POSITION_ESTIMATE -r 10 # # Start the datamanager @@ -453,7 +453,7 @@ then sh /etc/init.d/rc.logging echo "[init] Start GPS" - gps start + #gps start fi # From 58f471af17adf706ee8e094e9129de43533fb008 Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Thu, 10 Apr 2014 10:27:35 +0200 Subject: [PATCH 365/381] Reverted rcS --- ROMFS/px4fmu_common/init.d/rcS | 16 ++++------------ 1 file changed, 4 insertions(+), 12 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index e37cab04c6..0cb6d281a2 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -410,7 +410,7 @@ then if [ $TTYS1_BUSY == yes ] then # Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else - set MAVLINK_FLAGS "-r 1000 -d /dev/ttyS0 -b 57600" + set MAVLINK_FLAGS "-r 1000 -d /dev/ttyS0" # Exit from nsh to free port for mavlink set EXIT_ON_END yes @@ -421,16 +421,8 @@ then fi fi - # Main mavlink - mavlink start -d /dev/ttyS0 -b 57600 - mavlink stream -d /dev/ttyS0 -s OPTICAL_FLOW -r 10 - # Optical Flow - mavlink start -d /dev/ttyS3 -m custom -b 115200 - # Android board - mavlink start -d /dev/ttyS2 -m custom -b 115200 -w - mavlink stream -d /dev/ttyS2 -s ATTITUDE -r 50 - #mavlink stream -d /dev/ttyS2 -s VICON_POSITION_ESTIMATE -r 10 - + mavlink start $MAVLINK_FLAGS + # # Start the datamanager # @@ -453,7 +445,7 @@ then sh /etc/init.d/rc.logging echo "[init] Start GPS" - #gps start + gps start fi # From 4f91fdb98ca03f27ea130a53636b4b7990fba183 Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Thu, 10 Apr 2014 10:30:00 +0200 Subject: [PATCH 366/381] Indentation. --- src/modules/mavlink/mavlink_main.h | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 2c18261399..66d82b4718 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -203,12 +203,12 @@ public: int get_mavlink_fd() { return _mavlink_fd; } - /* Functions for waiting to start transmission until message received. */ - void set_has_received_messages(bool received_messages) { _received_messages = received_messages; } - bool get_has_received_messages() { return _received_messages; } - void set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; } - bool get_wait_to_transmit() { return _wait_to_transmit; } - bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); } + /* Functions for waiting to start transmission until message received. */ + void set_has_received_messages(bool received_messages) { _received_messages = received_messages; } + bool get_has_received_messages() { return _received_messages; } + void set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; } + bool get_wait_to_transmit() { return _wait_to_transmit; } + bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); } protected: Mavlink *next; @@ -224,8 +224,8 @@ private: /* states */ bool _hil_enabled; /**< Hardware In the Loop 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. */ + bool _wait_to_transmit; /**< Wait to transmit until received messages. */ + bool _received_messages; /**< Whether we've received valid mavlink messages. */ unsigned _main_loop_delay; /**< mainloop delay, depends on data rate */ From dd88e319ee774ac9c8b7726d2fb4dbcc5b04078b Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Thu, 10 Apr 2014 10:33:10 +0200 Subject: [PATCH 367/381] Reverted logging. --- ROMFS/px4fmu_common/init.d/rc.logging | 2 +- src/modules/mavlink/mavlink_main.cpp | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging index d6fbe8c4fa..c5aef8273c 100644 --- a/ROMFS/px4fmu_common/init.d/rc.logging +++ b/ROMFS/px4fmu_common/init.d/rc.logging @@ -8,7 +8,7 @@ then if hw_ver compare PX4FMU_V1 then echo "Start sdlog2 at 50Hz" - sdlog2 start -r 50 -a -b 1 -t -e + sdlog2 start -r 50 -a -b 8 -t else echo "Start sdlog2 at 200Hz" sdlog2 start -r 200 -a -b 16 -t diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index b9e0663b71..0186cddea2 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -2175,11 +2175,11 @@ Mavlink::start(int argc, char *argv[]) // task - start_helper() only returns // when the started task exits. task_spawn_cmd(buf, - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 2048, - (main_t)&Mavlink::start_helper, - (const char **)argv); + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2048, + (main_t)&Mavlink::start_helper, + (const char **)argv); // Ensure that this shell command // does not return before the instance From e5105f6d9124dd12a86bec445a8b6f483adfbc56 Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Thu, 10 Apr 2014 10:35:30 +0200 Subject: [PATCH 368/381] Whtespace. --- src/modules/mavlink/mavlink_main.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 0186cddea2..f4a4ce86c6 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -2175,11 +2175,11 @@ Mavlink::start(int argc, char *argv[]) // task - start_helper() only returns // when the started task exits. task_spawn_cmd(buf, - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 2048, - (main_t)&Mavlink::start_helper, - (const char **)argv); + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2048, + (main_t)&Mavlink::start_helper, + (const char **)argv); // Ensure that this shell command // does not return before the instance From 8a946f0320c4bd4a61927a12b7ba4c0c96c77d7d Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Thu, 10 Apr 2014 10:37:58 +0200 Subject: [PATCH 369/381] More whitespace all the time. --- src/modules/mavlink/mavlink_messages.cpp | 178 +++++++++++------------ src/modules/mavlink/mavlink_receiver.cpp | 6 +- 2 files changed, 92 insertions(+), 92 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 37929edac6..2d1d92243c 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -94,7 +94,7 @@ cm_uint16_from_m_float(float m) } void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, - uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode) + uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode) { *mavlink_state = 0; *mavlink_base_mode = 0; @@ -107,7 +107,7 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set /* arming state */ if (status->arming_state == ARMING_STATE_ARMED - || status->arming_state == ARMING_STATE_ARMED_ERROR) { + || status->arming_state == ARMING_STATE_ARMED_ERROR) { *mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } @@ -163,8 +163,8 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set /* set system state */ if (status->arming_state == ARMING_STATE_INIT - || status->arming_state == ARMING_STATE_IN_AIR_RESTORE - || status->arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review + || status->arming_state == ARMING_STATE_IN_AIR_RESTORE + || status->arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review *mavlink_state = MAV_STATE_UNINIT; } else if (status->arming_state == ARMING_STATE_ARMED) { @@ -344,13 +344,13 @@ protected: } mavlink_msg_highres_imu_send(_channel, - sensor->timestamp, - sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2], - sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2], - sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2], - sensor->baro_pres_mbar, sensor->differential_pressure_pa, - sensor->baro_alt_meter, sensor->baro_temp_celcius, - fields_updated); + sensor->timestamp, + sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2], + sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2], + sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2], + sensor->baro_pres_mbar, sensor->differential_pressure_pa, + sensor->baro_alt_meter, sensor->baro_temp_celcius, + fields_updated); } } }; @@ -420,14 +420,14 @@ protected: { if (att_sub->update(t)) { mavlink_msg_attitude_quaternion_send(_channel, - att->timestamp / 1000, - att->q[0], - att->q[1], - att->q[2], - att->q[3], - att->rollspeed, - att->pitchspeed, - att->yawspeed); + att->timestamp / 1000, + att->q[0], + att->q[1], + att->q[2], + att->q[3], + att->rollspeed, + att->pitchspeed, + att->yawspeed); } } }; @@ -534,16 +534,16 @@ protected: { if (gps_sub->update(t)) { mavlink_msg_gps_raw_int_send(_channel, - gps->timestamp_position, - gps->fix_type, - gps->lat, - gps->lon, - gps->alt, - cm_uint16_from_m_float(gps->eph_m), - cm_uint16_from_m_float(gps->epv_m), - gps->vel_m_s * 100.0f, - _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f, - gps->satellites_visible); + gps->timestamp_position, + gps->fix_type, + gps->lat, + gps->lon, + gps->alt, + cm_uint16_from_m_float(gps->eph_m), + cm_uint16_from_m_float(gps->epv_m), + gps->vel_m_s * 100.0f, + _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f, + gps->satellites_visible); } } }; @@ -586,15 +586,15 @@ protected: if (updated) { mavlink_msg_global_position_int_send(_channel, - pos->timestamp / 1000, - pos->lat * 1e7, - pos->lon * 1e7, - pos->alt * 1000.0f, - (pos->alt - home->alt) * 1000.0f, - pos->vel_n * 100.0f, - pos->vel_e * 100.0f, - pos->vel_d * 100.0f, - _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f); + pos->timestamp / 1000, + pos->lat * 1e7, + pos->lon * 1e7, + pos->alt * 1000.0f, + (pos->alt - home->alt) * 1000.0f, + pos->vel_n * 100.0f, + pos->vel_e * 100.0f, + pos->vel_d * 100.0f, + _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f); } } }; @@ -628,13 +628,13 @@ protected: { if (pos_sub->update(t)) { mavlink_msg_local_position_ned_send(_channel, - pos->timestamp / 1000, - pos->x, - pos->y, - pos->z, - pos->vx, - pos->vy, - pos->vz); + pos->timestamp / 1000, + pos->x, + pos->y, + pos->z, + pos->vx, + pos->vy, + pos->vz); } } }; @@ -644,40 +644,40 @@ protected: class MavlinkStreamViconPositionEstimate : public MavlinkStream { public: - const char *get_name() - { - return "VICON_POSITION_ESTIMATE"; - } + const char *get_name() + { + return "VICON_POSITION_ESTIMATE"; + } - MavlinkStream *new_instance() - { - return new MavlinkStreamViconPositionEstimate(); - } + MavlinkStream *new_instance() + { + return new MavlinkStreamViconPositionEstimate(); + } private: - MavlinkOrbSubscription *pos_sub; - struct vehicle_vicon_position_s *pos; + MavlinkOrbSubscription *pos_sub; + struct vehicle_vicon_position_s *pos; protected: - void subscribe(Mavlink *mavlink) - { - pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position)); - pos = (struct vehicle_vicon_position_s *)pos_sub->get_data(); - } + void subscribe(Mavlink *mavlink) + { + pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position)); + pos = (struct vehicle_vicon_position_s *)pos_sub->get_data(); + } - void send(const hrt_abstime t) - { - if (pos_sub->update(t)) { - mavlink_msg_vicon_position_estimate_send(_channel, - pos->timestamp / 1000, - pos->x, - pos->y, - pos->z, - pos->roll, - pos->pitch, - pos->yaw); - } - } + void send(const hrt_abstime t) + { + if (pos_sub->update(t)) { + mavlink_msg_vicon_position_estimate_send(_channel, + pos->timestamp / 1000, + pos->x, + pos->y, + pos->z, + pos->roll, + pos->pitch, + pos->yaw); + } + } }; @@ -869,10 +869,10 @@ protected: } mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], - mavlink_base_mode, - 0); + hrt_absolute_time(), + out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], + mavlink_base_mode, + 0); } else { /* fixed wing: scale all channels except throttle -1 .. 1 @@ -897,10 +897,10 @@ protected: } mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], - mavlink_base_mode, - 0); + hrt_absolute_time(), + out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], + mavlink_base_mode, + 0); } } } @@ -1175,12 +1175,12 @@ protected: { if (flow_sub->update(t)) { mavlink_msg_optical_flow_send(_channel, - flow->timestamp, - flow->sensor_id, - flow->flow_raw_x, flow->flow_raw_y, - flow->flow_comp_x_m, flow->flow_comp_y_m, - flow->quality, - flow->ground_distance_m); + flow->timestamp, + flow->sensor_id, + flow->flow_raw_x, flow->flow_raw_y, + flow->flow_comp_x_m, flow->flow_comp_y_m, + flow->quality, + flow->ground_distance_m); } } }; @@ -1300,7 +1300,7 @@ protected: (void)status_sub->update(t); if (status->arming_state == ARMING_STATE_ARMED - || status->arming_state == ARMING_STATE_ARMED_ERROR) { + || status->arming_state == ARMING_STATE_ARMED_ERROR) { /* send camera capture on */ mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); @@ -1338,6 +1338,6 @@ MavlinkStream *streams_list[] = { new MavlinkStreamAttitudeControls(), new MavlinkStreamNamedValueFloat(), new MavlinkStreamCameraCapture(), - new MavlinkStreamViconPositionEstimate(), + new MavlinkStreamViconPositionEstimate(), nullptr }; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 61ef2b043e..b4fe65fd28 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -182,9 +182,9 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) } } - /* If we've received a valid message, mark the flag indicating so. - This is used in the '-w' command-line flag. */ - _mavlink->set_has_received_messages(true); + /* If we've received a valid message, mark the flag indicating so. + This is used in the '-w' command-line flag. */ + _mavlink->set_has_received_messages(true); } void From e7c8fdc586e59d50579470336157feb14c65ac5b Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Thu, 10 Apr 2014 10:41:00 +0200 Subject: [PATCH 370/381] More whitespace all the time. --- src/modules/mavlink/mavlink_messages.cpp | 134 +++++++++++------------ 1 file changed, 67 insertions(+), 67 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 2d1d92243c..648228e3be 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -94,7 +94,7 @@ cm_uint16_from_m_float(float m) } void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, - uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode) + uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode) { *mavlink_state = 0; *mavlink_base_mode = 0; @@ -107,7 +107,7 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set /* arming state */ if (status->arming_state == ARMING_STATE_ARMED - || status->arming_state == ARMING_STATE_ARMED_ERROR) { + || status->arming_state == ARMING_STATE_ARMED_ERROR) { *mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } @@ -163,8 +163,8 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set /* set system state */ if (status->arming_state == ARMING_STATE_INIT - || status->arming_state == ARMING_STATE_IN_AIR_RESTORE - || status->arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review + || status->arming_state == ARMING_STATE_IN_AIR_RESTORE + || status->arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review *mavlink_state = MAV_STATE_UNINIT; } else if (status->arming_state == ARMING_STATE_ARMED) { @@ -344,13 +344,13 @@ protected: } mavlink_msg_highres_imu_send(_channel, - sensor->timestamp, - sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2], - sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2], - sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2], - sensor->baro_pres_mbar, sensor->differential_pressure_pa, - sensor->baro_alt_meter, sensor->baro_temp_celcius, - fields_updated); + sensor->timestamp, + sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2], + sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2], + sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2], + sensor->baro_pres_mbar, sensor->differential_pressure_pa, + sensor->baro_alt_meter, sensor->baro_temp_celcius, + fields_updated); } } }; @@ -420,14 +420,14 @@ protected: { if (att_sub->update(t)) { mavlink_msg_attitude_quaternion_send(_channel, - att->timestamp / 1000, - att->q[0], - att->q[1], - att->q[2], - att->q[3], - att->rollspeed, - att->pitchspeed, - att->yawspeed); + att->timestamp / 1000, + att->q[0], + att->q[1], + att->q[2], + att->q[3], + att->rollspeed, + att->pitchspeed, + att->yawspeed); } } }; @@ -534,16 +534,16 @@ protected: { if (gps_sub->update(t)) { mavlink_msg_gps_raw_int_send(_channel, - gps->timestamp_position, - gps->fix_type, - gps->lat, - gps->lon, - gps->alt, - cm_uint16_from_m_float(gps->eph_m), - cm_uint16_from_m_float(gps->epv_m), - gps->vel_m_s * 100.0f, - _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f, - gps->satellites_visible); + gps->timestamp_position, + gps->fix_type, + gps->lat, + gps->lon, + gps->alt, + cm_uint16_from_m_float(gps->eph_m), + cm_uint16_from_m_float(gps->epv_m), + gps->vel_m_s * 100.0f, + _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f, + gps->satellites_visible); } } }; @@ -586,15 +586,15 @@ protected: if (updated) { mavlink_msg_global_position_int_send(_channel, - pos->timestamp / 1000, - pos->lat * 1e7, - pos->lon * 1e7, - pos->alt * 1000.0f, - (pos->alt - home->alt) * 1000.0f, - pos->vel_n * 100.0f, - pos->vel_e * 100.0f, - pos->vel_d * 100.0f, - _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f); + pos->timestamp / 1000, + pos->lat * 1e7, + pos->lon * 1e7, + pos->alt * 1000.0f, + (pos->alt - home->alt) * 1000.0f, + pos->vel_n * 100.0f, + pos->vel_e * 100.0f, + pos->vel_d * 100.0f, + _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f); } } }; @@ -628,13 +628,13 @@ protected: { if (pos_sub->update(t)) { mavlink_msg_local_position_ned_send(_channel, - pos->timestamp / 1000, - pos->x, - pos->y, - pos->z, - pos->vx, - pos->vy, - pos->vz); + pos->timestamp / 1000, + pos->x, + pos->y, + pos->z, + pos->vx, + pos->vy, + pos->vz); } } }; @@ -669,13 +669,13 @@ protected: { if (pos_sub->update(t)) { mavlink_msg_vicon_position_estimate_send(_channel, - pos->timestamp / 1000, - pos->x, - pos->y, - pos->z, - pos->roll, - pos->pitch, - pos->yaw); + pos->timestamp / 1000, + pos->x, + pos->y, + pos->z, + pos->roll, + pos->pitch, + pos->yaw); } } }; @@ -869,10 +869,10 @@ protected: } mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], - mavlink_base_mode, - 0); + hrt_absolute_time(), + out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], + mavlink_base_mode, + 0); } else { /* fixed wing: scale all channels except throttle -1 .. 1 @@ -897,10 +897,10 @@ protected: } mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], - mavlink_base_mode, - 0); + hrt_absolute_time(), + out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], + mavlink_base_mode, + 0); } } } @@ -1175,12 +1175,12 @@ protected: { if (flow_sub->update(t)) { mavlink_msg_optical_flow_send(_channel, - flow->timestamp, - flow->sensor_id, - flow->flow_raw_x, flow->flow_raw_y, - flow->flow_comp_x_m, flow->flow_comp_y_m, - flow->quality, - flow->ground_distance_m); + flow->timestamp, + flow->sensor_id, + flow->flow_raw_x, flow->flow_raw_y, + flow->flow_comp_x_m, flow->flow_comp_y_m, + flow->quality, + flow->ground_distance_m); } } }; @@ -1300,7 +1300,7 @@ protected: (void)status_sub->update(t); if (status->arming_state == ARMING_STATE_ARMED - || status->arming_state == ARMING_STATE_ARMED_ERROR) { + || status->arming_state == ARMING_STATE_ARMED_ERROR) { /* send camera capture on */ mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0); From 88357c58bde304567f841abc48a495794d4e250c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Apr 2014 20:45:03 +0200 Subject: [PATCH 371/381] mavlink: Change to size optimization --- src/modules/mavlink/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index 515fbfadce..c44354ff04 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -46,3 +46,5 @@ SRCS += mavlink_main.cpp \ mavlink_commands.cpp INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink + +MAXOPTIMIZATION = -Os From 14dae529f0b2a41ce7441871f30a9da462be8e72 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Apr 2014 09:41:23 +0200 Subject: [PATCH 372/381] mavlink: Updated protocol files --- .../v1.0/ardupilotmega/ardupilotmega.h | 108 +-- .../v1.0/ardupilotmega/mavlink_msg_ahrs.h | 44 ++ .../v1.0/ardupilotmega/mavlink_msg_ahrs2.h | 42 ++ .../mavlink_msg_airspeed_autocal.h | 54 ++ .../v1.0/ardupilotmega/mavlink_msg_ap_adc.h | 42 ++ .../mavlink_msg_compassmot_status.h | 329 +++++++++ .../v1.0/ardupilotmega/mavlink_msg_data16.h | 34 + .../v1.0/ardupilotmega/mavlink_msg_data32.h | 34 + .../v1.0/ardupilotmega/mavlink_msg_data64.h | 34 + .../v1.0/ardupilotmega/mavlink_msg_data96.h | 34 + .../mavlink_msg_digicam_configure.h | 52 ++ .../mavlink_msg_digicam_control.h | 50 ++ .../mavlink_msg_fence_fetch_point.h | 36 + .../ardupilotmega/mavlink_msg_fence_point.h | 42 ++ .../ardupilotmega/mavlink_msg_fence_status.h | 38 + .../v1.0/ardupilotmega/mavlink_msg_hwstatus.h | 34 + .../ardupilotmega/mavlink_msg_limits_status.h | 48 ++ .../v1.0/ardupilotmega/mavlink_msg_meminfo.h | 34 + .../mavlink_msg_mount_configure.h | 42 ++ .../ardupilotmega/mavlink_msg_mount_control.h | 42 ++ .../ardupilotmega/mavlink_msg_mount_status.h | 40 + .../v1.0/ardupilotmega/mavlink_msg_radio.h | 44 ++ .../mavlink_msg_rally_fetch_point.h | 36 + .../ardupilotmega/mavlink_msg_rally_point.h | 50 ++ .../ardupilotmega/mavlink_msg_rangefinder.h | 34 + .../mavlink_msg_sensor_offsets.h | 54 ++ .../mavlink_msg_set_mag_offsets.h | 40 + .../v1.0/ardupilotmega/mavlink_msg_simstate.h | 52 ++ .../v1.0/ardupilotmega/mavlink_msg_wind.h | 36 + .../mavlink/v1.0/ardupilotmega/testsuite.h | 54 ++ .../mavlink/v1.0/ardupilotmega/version.h | 2 +- .../include/mavlink/v1.0/autoquad/autoquad.h | 17 +- .../autoquad/mavlink_msg_aq_telemetry_f.h | 72 ++ .../include/mavlink/v1.0/autoquad/version.h | 2 +- mavlink/include/mavlink/v1.0/common/common.h | 124 +++- .../v1.0/common/mavlink_msg_attitude.h | 44 ++ .../common/mavlink_msg_attitude_quaternion.h | 46 ++ .../v1.0/common/mavlink_msg_auth_key.h | 32 + .../v1.0/common/mavlink_msg_battery_status.h | 52 ++ .../mavlink_msg_change_operator_control.h | 36 + .../mavlink_msg_change_operator_control_ack.h | 36 + .../v1.0/common/mavlink_msg_command_ack.h | 34 + .../v1.0/common/mavlink_msg_command_long.h | 52 ++ .../v1.0/common/mavlink_msg_data_stream.h | 36 + .../mavlink_msg_data_transmission_handshake.h | 44 ++ .../mavlink/v1.0/common/mavlink_msg_debug.h | 36 + .../v1.0/common/mavlink_msg_debug_vect.h | 38 + .../v1.0/common/mavlink_msg_distance_sensor.h | 377 ++++++++++ .../common/mavlink_msg_encapsulated_data.h | 32 + .../mavlink_msg_file_transfer_dir_list.h | 34 + .../common/mavlink_msg_file_transfer_res.h | 34 + .../common/mavlink_msg_file_transfer_start.h | 38 + .../common/mavlink_msg_global_position_int.h | 48 ++ ...mavlink_msg_global_position_setpoint_int.h | 40 + ...link_msg_global_vision_position_estimate.h | 44 ++ .../v1.0/common/mavlink_msg_gps2_raw.h | 54 ++ .../common/mavlink_msg_gps_global_origin.h | 36 + .../v1.0/common/mavlink_msg_gps_inject_data.h | 36 + .../v1.0/common/mavlink_msg_gps_raw_int.h | 50 ++ .../v1.0/common/mavlink_msg_gps_status.h | 40 + .../v1.0/common/mavlink_msg_heartbeat.h | 42 ++ .../v1.0/common/mavlink_msg_highres_imu.h | 60 ++ .../v1.0/common/mavlink_msg_hil_controls.h | 52 ++ .../mavlink/v1.0/common/mavlink_msg_hil_gps.h | 56 ++ .../common/mavlink_msg_hil_optical_flow.h | 46 ++ .../common/mavlink_msg_hil_rc_inputs_raw.h | 58 ++ .../v1.0/common/mavlink_msg_hil_sensor.h | 60 ++ .../v1.0/common/mavlink_msg_hil_state.h | 62 ++ .../common/mavlink_msg_hil_state_quaternion.h | 60 ++ .../common/mavlink_msg_local_position_ned.h | 44 ++ ..._local_position_ned_system_global_offset.h | 44 ++ .../mavlink_msg_local_position_setpoint.h | 40 + .../v1.0/common/mavlink_msg_log_data.h | 36 + .../v1.0/common/mavlink_msg_log_entry.h | 40 + .../v1.0/common/mavlink_msg_log_erase.h | 34 + .../common/mavlink_msg_log_request_data.h | 40 + .../v1.0/common/mavlink_msg_log_request_end.h | 34 + .../common/mavlink_msg_log_request_list.h | 38 + .../v1.0/common/mavlink_msg_manual_control.h | 42 ++ .../v1.0/common/mavlink_msg_manual_setpoint.h | 44 ++ .../v1.0/common/mavlink_msg_memory_vect.h | 36 + .../v1.0/common/mavlink_msg_mission_ack.h | 36 + .../common/mavlink_msg_mission_clear_all.h | 34 + .../v1.0/common/mavlink_msg_mission_count.h | 36 + .../v1.0/common/mavlink_msg_mission_current.h | 32 + .../v1.0/common/mavlink_msg_mission_item.h | 58 ++ .../common/mavlink_msg_mission_item_reached.h | 32 + .../v1.0/common/mavlink_msg_mission_request.h | 36 + .../common/mavlink_msg_mission_request_list.h | 34 + ...mavlink_msg_mission_request_partial_list.h | 38 + .../common/mavlink_msg_mission_set_current.h | 36 + .../mavlink_msg_mission_write_partial_list.h | 38 + .../common/mavlink_msg_named_value_float.h | 34 + .../v1.0/common/mavlink_msg_named_value_int.h | 34 + .../mavlink_msg_nav_controller_output.h | 46 ++ .../common/mavlink_msg_omnidirectional_flow.h | 40 + .../v1.0/common/mavlink_msg_optical_flow.h | 46 ++ .../common/mavlink_msg_param_request_list.h | 34 + .../common/mavlink_msg_param_request_read.h | 36 + .../v1.0/common/mavlink_msg_param_set.h | 38 + .../v1.0/common/mavlink_msg_param_value.h | 38 + .../mavlink/v1.0/common/mavlink_msg_ping.h | 38 + .../v1.0/common/mavlink_msg_power_status.h | 257 +++++++ .../v1.0/common/mavlink_msg_radio_status.h | 44 ++ .../mavlink/v1.0/common/mavlink_msg_raw_imu.h | 50 ++ .../v1.0/common/mavlink_msg_raw_pressure.h | 40 + .../v1.0/common/mavlink_msg_rc_channels.h | 689 ++++++++++++++++++ .../common/mavlink_msg_rc_channels_override.h | 50 ++ .../v1.0/common/mavlink_msg_rc_channels_raw.h | 52 ++ .../common/mavlink_msg_rc_channels_scaled.h | 52 ++ .../common/mavlink_msg_request_data_stream.h | 40 + ...msg_roll_pitch_yaw_rates_thrust_setpoint.h | 40 + ...msg_roll_pitch_yaw_speed_thrust_setpoint.h | 40 + ...vlink_msg_roll_pitch_yaw_thrust_setpoint.h | 40 + .../common/mavlink_msg_safety_allowed_area.h | 44 ++ .../mavlink_msg_safety_set_allowed_area.h | 48 ++ .../v1.0/common/mavlink_msg_scaled_imu.h | 50 ++ .../v1.0/common/mavlink_msg_scaled_imu2.h | 50 ++ .../v1.0/common/mavlink_msg_scaled_pressure.h | 38 + .../v1.0/common/mavlink_msg_serial_control.h | 321 ++++++++ .../common/mavlink_msg_servo_output_raw.h | 50 ++ ...ink_msg_set_global_position_setpoint_int.h | 40 + .../mavlink_msg_set_gps_global_origin.h | 38 + .../mavlink_msg_set_local_position_setpoint.h | 44 ++ .../v1.0/common/mavlink_msg_set_mode.h | 36 + .../mavlink_msg_set_quad_motors_setpoint.h | 40 + ...set_quad_swarm_led_roll_pitch_yaw_thrust.h | 46 ++ ...msg_set_quad_swarm_roll_pitch_yaw_thrust.h | 40 + ...link_msg_set_roll_pitch_yaw_speed_thrust.h | 42 ++ .../mavlink_msg_set_roll_pitch_yaw_thrust.h | 42 ++ .../v1.0/common/mavlink_msg_setpoint_6dof.h | 44 ++ .../v1.0/common/mavlink_msg_setpoint_8dof.h | 48 ++ .../v1.0/common/mavlink_msg_sim_state.h | 72 ++ .../common/mavlink_msg_state_correction.h | 48 ++ .../v1.0/common/mavlink_msg_statustext.h | 32 + .../v1.0/common/mavlink_msg_sys_status.h | 56 ++ .../v1.0/common/mavlink_msg_system_time.h | 34 + .../mavlink/v1.0/common/mavlink_msg_vfr_hud.h | 42 ++ .../mavlink_msg_vicon_position_estimate.h | 44 ++ .../mavlink_msg_vision_position_estimate.h | 44 ++ .../mavlink_msg_vision_speed_estimate.h | 38 + .../include/mavlink/v1.0/common/testsuite.h | 244 +++++++ mavlink/include/mavlink/v1.0/common/version.h | 2 +- .../mavlink/v1.0/matrixpilot/matrixpilot.h | 17 +- .../v1.0/matrixpilot/mavlink_msg_airspeeds.h | 44 ++ .../v1.0/matrixpilot/mavlink_msg_altitudes.h | 44 ++ ...avlink_msg_flexifunction_buffer_function.h | 42 ++ ...nk_msg_flexifunction_buffer_function_ack.h | 38 + .../mavlink_msg_flexifunction_command.h | 36 + .../mavlink_msg_flexifunction_command_ack.h | 34 + .../mavlink_msg_flexifunction_directory.h | 40 + .../mavlink_msg_flexifunction_directory_ack.h | 42 ++ .../mavlink_msg_flexifunction_read_req.h | 38 + .../mavlink_msg_flexifunction_set.h | 34 + .../mavlink_msg_serial_udb_extra_f13.h | 38 + .../mavlink_msg_serial_udb_extra_f14.h | 52 ++ .../mavlink_msg_serial_udb_extra_f15.h | 34 + .../mavlink_msg_serial_udb_extra_f16.h | 34 + .../mavlink_msg_serial_udb_extra_f2_a.h | 86 +++ .../mavlink_msg_serial_udb_extra_f2_b.h | 96 +++ .../mavlink_msg_serial_udb_extra_f4.h | 50 ++ .../mavlink_msg_serial_udb_extra_f5.h | 42 ++ .../mavlink_msg_serial_udb_extra_f6.h | 40 + .../mavlink_msg_serial_udb_extra_f7.h | 42 ++ .../mavlink_msg_serial_udb_extra_f8.h | 44 ++ .../mavlink/v1.0/matrixpilot/version.h | 2 +- .../pixhawk/mavlink_msg_attitude_control.h | 48 ++ .../v1.0/pixhawk/mavlink_msg_brief_feature.h | 44 ++ .../pixhawk/mavlink_msg_image_available.h | 76 ++ .../mavlink_msg_image_trigger_control.h | 32 + .../pixhawk/mavlink_msg_image_triggered.h | 54 ++ .../mavlink/v1.0/pixhawk/mavlink_msg_marker.h | 44 ++ .../pixhawk/mavlink_msg_pattern_detected.h | 36 + .../pixhawk/mavlink_msg_point_of_interest.h | 44 ++ ...mavlink_msg_point_of_interest_connection.h | 50 ++ .../mavlink_msg_position_control_setpoint.h | 40 + .../v1.0/pixhawk/mavlink_msg_raw_aux.h | 44 ++ .../pixhawk/mavlink_msg_set_cam_shutter.h | 42 ++ .../mavlink_msg_set_position_control_offset.h | 42 ++ .../pixhawk/mavlink_msg_watchdog_command.h | 38 + .../pixhawk/mavlink_msg_watchdog_heartbeat.h | 34 + .../mavlink_msg_watchdog_process_info.h | 38 + .../mavlink_msg_watchdog_process_status.h | 42 ++ .../include/mavlink/v1.0/pixhawk/pixhawk.h | 17 +- .../include/mavlink/v1.0/pixhawk/version.h | 2 +- .../sensesoar/mavlink_msg_cmd_airspeed_ack.h | 34 + .../sensesoar/mavlink_msg_cmd_airspeed_chng.h | 34 + .../v1.0/sensesoar/mavlink_msg_filt_rot_vel.h | 32 + .../v1.0/sensesoar/mavlink_msg_llc_out.h | 34 + .../v1.0/sensesoar/mavlink_msg_obs_air_temp.h | 32 + .../sensesoar/mavlink_msg_obs_air_velocity.h | 36 + .../v1.0/sensesoar/mavlink_msg_obs_attitude.h | 32 + .../v1.0/sensesoar/mavlink_msg_obs_bias.h | 34 + .../v1.0/sensesoar/mavlink_msg_obs_position.h | 36 + .../v1.0/sensesoar/mavlink_msg_obs_qff.h | 32 + .../v1.0/sensesoar/mavlink_msg_obs_velocity.h | 32 + .../v1.0/sensesoar/mavlink_msg_obs_wind.h | 32 + .../v1.0/sensesoar/mavlink_msg_pm_elec.h | 34 + .../v1.0/sensesoar/mavlink_msg_sys_stat.h | 38 + .../mavlink/v1.0/sensesoar/sensesoar.h | 6 +- .../include/mavlink/v1.0/sensesoar/version.h | 2 +- 201 files changed, 10148 insertions(+), 120 deletions(-) create mode 100644 mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_compassmot_status.h create mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_distance_sensor.h create mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_power_status.h create mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels.h create mode 100644 mavlink/include/mavlink/v1.0/common/mavlink_msg_serial_control.h diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h b/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h index 2acb7965ca..c887dc68a1 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h @@ -12,15 +12,15 @@ extern "C" { // MESSAGE LENGTHS AND CRCS #ifndef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 0, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 22, 6, 14, 12, 97, 2, 2, 113, 35, 0, 0, 0, 0, 0, 13, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 24, 33, 25, 42, 8, 4, 12, 15, 13, 6, 15, 14, 0, 12, 3, 8, 28, 44, 3, 9, 22, 12, 18, 34, 66, 98, 8, 48, 19, 3, 0, 24, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0} +#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 42, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 22, 6, 14, 12, 97, 2, 2, 113, 35, 6, 79, 0, 0, 0, 13, 255, 14, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 24, 33, 25, 42, 8, 4, 12, 15, 13, 6, 15, 14, 0, 12, 3, 8, 28, 44, 3, 9, 22, 12, 18, 34, 66, 98, 8, 48, 19, 3, 20, 24, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0} #endif #ifndef MAVLINK_MESSAGE_CRCS -#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 0, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 0, 0, 0, 0, 0, 29, 223, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 134, 219, 208, 188, 84, 22, 19, 21, 134, 0, 78, 68, 189, 127, 154, 21, 21, 144, 1, 234, 73, 181, 22, 83, 167, 138, 234, 0, 47, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0} +#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 118, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 203, 220, 0, 0, 0, 29, 223, 85, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 134, 219, 208, 188, 84, 22, 19, 21, 134, 0, 78, 68, 189, 127, 154, 21, 21, 144, 1, 234, 73, 181, 22, 83, 167, 138, 234, 240, 47, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0} #endif #ifndef MAVLINK_MESSAGE_INFO -#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, MAVLINK_MESSAGE_INFO_AP_ADC, MAVLINK_MESSAGE_INFO_DIGICAM_CONFIGURE, MAVLINK_MESSAGE_INFO_DIGICAM_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_CONFIGURE, MAVLINK_MESSAGE_INFO_MOUNT_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_STATUS, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_FENCE_POINT, MAVLINK_MESSAGE_INFO_FENCE_FETCH_POINT, MAVLINK_MESSAGE_INFO_FENCE_STATUS, MAVLINK_MESSAGE_INFO_AHRS, MAVLINK_MESSAGE_INFO_SIMSTATE, MAVLINK_MESSAGE_INFO_HWSTATUS, MAVLINK_MESSAGE_INFO_RADIO, MAVLINK_MESSAGE_INFO_LIMITS_STATUS, MAVLINK_MESSAGE_INFO_WIND, MAVLINK_MESSAGE_INFO_DATA16, MAVLINK_MESSAGE_INFO_DATA32, MAVLINK_MESSAGE_INFO_DATA64, MAVLINK_MESSAGE_INFO_DATA96, MAVLINK_MESSAGE_INFO_RANGEFINDER, MAVLINK_MESSAGE_INFO_AIRSPEED_AUTOCAL, MAVLINK_MESSAGE_INFO_RALLY_POINT, MAVLINK_MESSAGE_INFO_RALLY_FETCH_POINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_AHRS2, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} +#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_RC_CHANNELS, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, MAVLINK_MESSAGE_INFO_POWER_STATUS, MAVLINK_MESSAGE_INFO_SERIAL_CONTROL, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, MAVLINK_MESSAGE_INFO_DISTANCE_SENSOR, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, MAVLINK_MESSAGE_INFO_AP_ADC, MAVLINK_MESSAGE_INFO_DIGICAM_CONFIGURE, MAVLINK_MESSAGE_INFO_DIGICAM_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_CONFIGURE, MAVLINK_MESSAGE_INFO_MOUNT_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_STATUS, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_FENCE_POINT, MAVLINK_MESSAGE_INFO_FENCE_FETCH_POINT, MAVLINK_MESSAGE_INFO_FENCE_STATUS, MAVLINK_MESSAGE_INFO_AHRS, MAVLINK_MESSAGE_INFO_SIMSTATE, MAVLINK_MESSAGE_INFO_HWSTATUS, MAVLINK_MESSAGE_INFO_RADIO, MAVLINK_MESSAGE_INFO_LIMITS_STATUS, MAVLINK_MESSAGE_INFO_WIND, MAVLINK_MESSAGE_INFO_DATA16, MAVLINK_MESSAGE_INFO_DATA32, MAVLINK_MESSAGE_INFO_DATA64, MAVLINK_MESSAGE_INFO_DATA96, MAVLINK_MESSAGE_INFO_RANGEFINDER, MAVLINK_MESSAGE_INFO_AIRSPEED_AUTOCAL, MAVLINK_MESSAGE_INFO_RALLY_POINT, MAVLINK_MESSAGE_INFO_RALLY_FETCH_POINT, MAVLINK_MESSAGE_INFO_COMPASSMOT_STATUS, MAVLINK_MESSAGE_INFO_AHRS2, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} #endif #include "../protocol.h" @@ -30,95 +30,6 @@ extern "C" { // ENUM DEFINITIONS -/** @brief Enumeration of possible mount operation modes */ -#ifndef HAVE_ENUM_MAV_MOUNT_MODE -#define HAVE_ENUM_MAV_MOUNT_MODE -enum MAV_MOUNT_MODE -{ - MAV_MOUNT_MODE_RETRACT=0, /* Load and keep safe position (Roll,Pitch,Yaw) from EEPROM and stop stabilization | */ - MAV_MOUNT_MODE_NEUTRAL=1, /* Load and keep neutral position (Roll,Pitch,Yaw) from EEPROM. | */ - MAV_MOUNT_MODE_MAVLINK_TARGETING=2, /* Load neutral position and start MAVLink Roll,Pitch,Yaw control with stabilization | */ - MAV_MOUNT_MODE_RC_TARGETING=3, /* Load neutral position and start RC Roll,Pitch,Yaw control with stabilization | */ - MAV_MOUNT_MODE_GPS_POINT=4, /* Load neutral position and start to point to Lat,Lon,Alt | */ - MAV_MOUNT_MODE_ENUM_END=5, /* | */ -}; -#endif - -/** @brief */ -#ifndef HAVE_ENUM_MAV_CMD -#define HAVE_ENUM_MAV_CMD -enum MAV_CMD -{ - MAV_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| */ - MAV_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| */ - MAV_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| */ - MAV_CMD_NAV_LOITER_TIME=19, /* Loiter around this MISSION for X seconds |Seconds (decimal)| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_RETURN_TO_LAUNCH=20, /* Return to launch location |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_NAV_LAND=21, /* Land at location |Empty| Empty| Empty| Desired yaw angle.| Latitude| Longitude| Altitude| */ - MAV_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| */ - MAV_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| */ - MAV_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| */ - MAV_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| */ - MAV_CMD_CONDITION_DELAY=112, /* Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_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| */ - MAV_CMD_CONDITION_DISTANCE=114, /* Delay mission state machine until within desired distance of next NAV point. |Distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_CONDITION_YAW=115, /* Reach a certain target angle. |target angle: [0-360], 0 is north| speed during yaw change:[deg per second]| direction: negative: counter clockwise, positive: clockwise [-1,1]| relative offset or absolute angle: [ 1,0]| Empty| Empty| Empty| */ - MAV_CMD_CONDITION_LAST=159, /* NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_MODE=176, /* Set system mode. |Mode, as defined by ENUM MAV_MODE| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_JUMP=177, /* Jump to the desired command in the mission list. Repeat this action only the specified number of times |Sequence number| Repeat count| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_CHANGE_SPEED=178, /* Change speed and/or throttle set points. |Speed type (0=Airspeed, 1=Ground Speed)| Speed (m/s, -1 indicates no change)| Throttle ( Percent, -1 indicates no change)| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_HOME=179, /* Changes the home location either to the current location or a specified location. |Use current (1=use current location, 0=use specified location)| Empty| Empty| Empty| Latitude| Longitude| Altitude| */ - MAV_CMD_DO_SET_PARAMETER=180, /* Set a system parameter. Caution! Use of this command requires knowledge of the numeric enumeration value of the parameter. |Parameter number| Parameter value| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_RELAY=181, /* Set a relay to a condition. |Relay number| Setting (1=on, 0=off, others possible depending on system hardware)| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_REPEAT_RELAY=182, /* Cycle a relay on and off for a desired number of cyles with a desired period. |Relay number| Cycle count| Cycle time (seconds, decimal)| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_SERVO=183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| */ - MAV_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_ROI=201, /* 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| */ - MAV_CMD_DO_DIGICAM_CONFIGURE=202, /* Mission command to configure an on-board camera controller system. |Modes: P, TV, AV, M, Etc| Shutter speed: Divisor number for one second| Aperture: F stop number| ISO number e.g. 80, 100, 200, Etc| Exposure type enumerator| Command Identity| Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)| */ - MAV_CMD_DO_DIGICAM_CONTROL=203, /* Mission command to control an on-board camera controller system. |Session control e.g. show/hide lens| Zoom's absolute position| Zooming step value to offset zoom from the current position| Focus Locking, Unlocking or Re-locking| Shooting Command| Command Identity| Empty| */ - MAV_CMD_DO_MOUNT_CONFIGURE=204, /* Mission command to configure a camera or antenna mount |Mount operation mode (see MAV_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty| */ - MAV_CMD_DO_MOUNT_CONTROL=205, /* Mission command to control a camera or antenna mount |pitch(deg*100) or lat, depending on mount mode.| roll(deg*100) or lon depending on mount mode| yaw(deg*100) or alt (in cm) depending on mount mode| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_CAM_TRIGG_DIST=206, /* Mission command to set CAM_TRIGG_DIST for this flight |Camera trigger distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty| */ - MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS=242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| */ - MAV_CMD_PREFLIGHT_STORAGE=245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty| */ - MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN=246, /* Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty| */ - MAV_CMD_OVERRIDE_GOTO=252, /* Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position| */ - MAV_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)| */ - MAV_CMD_COMPONENT_ARM_DISARM=400, /* Arms / Disarms a component |1 to arm, 0 to disarm| */ - MAV_CMD_START_RX_PAIR=500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| */ - MAV_CMD_ENUM_END=501, /* | */ -}; -#endif - -/** @brief */ -#ifndef HAVE_ENUM_FENCE_ACTION -#define HAVE_ENUM_FENCE_ACTION -enum FENCE_ACTION -{ - FENCE_ACTION_NONE=0, /* Disable fenced mode | */ - FENCE_ACTION_GUIDED=1, /* Switched to guided mode to return point (fence point 0) | */ - FENCE_ACTION_REPORT=2, /* Report fence breach, but don't take action | */ - FENCE_ACTION_GUIDED_THR_PASS=3, /* Switched to guided mode to return point (fence point 0) with manual throttle control | */ - FENCE_ACTION_ENUM_END=4, /* | */ -}; -#endif - -/** @brief */ -#ifndef HAVE_ENUM_FENCE_BREACH -#define HAVE_ENUM_FENCE_BREACH -enum FENCE_BREACH -{ - FENCE_BREACH_NONE=0, /* No last fence breach | */ - FENCE_BREACH_MINALT=1, /* Breached minimum altitude | */ - FENCE_BREACH_MAXALT=2, /* Breached maximum altitude | */ - FENCE_BREACH_BOUNDARY=3, /* Breached fence boundary | */ - FENCE_BREACH_ENUM_END=4, /* | */ -}; -#endif - /** @brief */ #ifndef HAVE_ENUM_LIMITS_STATE #define HAVE_ENUM_LIMITS_STATE @@ -157,6 +68,18 @@ enum RALLY_FLAGS }; #endif +/** @brief */ +#ifndef HAVE_ENUM_PARACHUTE_ACTION +#define HAVE_ENUM_PARACHUTE_ACTION +enum PARACHUTE_ACTION +{ + PARACHUTE_DISABLE=0, /* Disable parachute release | */ + PARACHUTE_ENABLE=1, /* Enable parachute release | */ + PARACHUTE_RELEASE=2, /* Release parachute | */ + PARACHUTE_ACTION_ENUM_END=3, /* | */ +}; +#endif + #include "../common/common.h" // MAVLINK VERSION @@ -197,6 +120,7 @@ enum RALLY_FLAGS #include "./mavlink_msg_airspeed_autocal.h" #include "./mavlink_msg_rally_point.h" #include "./mavlink_msg_rally_fetch_point.h" +#include "./mavlink_msg_compassmot_status.h" #include "./mavlink_msg_ahrs2.h" #ifdef __cplusplus diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs.h index c3ead11401..50ddc79e5a 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs.h @@ -212,6 +212,50 @@ static inline void mavlink_msg_ahrs_send(mavlink_channel_t chan, float omegaIx, #endif } +#if MAVLINK_MSG_ID_AHRS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_ahrs_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float omegaIx, float omegaIy, float omegaIz, float accel_weight, float renorm_val, float error_rp, float error_yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, omegaIx); + _mav_put_float(buf, 4, omegaIy); + _mav_put_float(buf, 8, omegaIz); + _mav_put_float(buf, 12, accel_weight); + _mav_put_float(buf, 16, renorm_val); + _mav_put_float(buf, 20, error_rp); + _mav_put_float(buf, 24, error_yaw); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, buf, MAVLINK_MSG_ID_AHRS_LEN, MAVLINK_MSG_ID_AHRS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, buf, MAVLINK_MSG_ID_AHRS_LEN); +#endif +#else + mavlink_ahrs_t *packet = (mavlink_ahrs_t *)msgbuf; + packet->omegaIx = omegaIx; + packet->omegaIy = omegaIy; + packet->omegaIz = omegaIz; + packet->accel_weight = accel_weight; + packet->renorm_val = renorm_val; + packet->error_rp = error_rp; + packet->error_yaw = error_yaw; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, (const char *)packet, MAVLINK_MSG_ID_AHRS_LEN, MAVLINK_MSG_ID_AHRS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, (const char *)packet, MAVLINK_MSG_ID_AHRS_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE AHRS UNPACKING diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs2.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs2.h index f6fde9590c..33e0066882 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs2.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs2.h @@ -201,6 +201,48 @@ static inline void mavlink_msg_ahrs2_send(mavlink_channel_t chan, float roll, fl #endif } +#if MAVLINK_MSG_ID_AHRS2_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_ahrs2_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float roll, float pitch, float yaw, float altitude, int32_t lat, int32_t lng) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, roll); + _mav_put_float(buf, 4, pitch); + _mav_put_float(buf, 8, yaw); + _mav_put_float(buf, 12, altitude); + _mav_put_int32_t(buf, 16, lat); + _mav_put_int32_t(buf, 20, lng); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS2, buf, MAVLINK_MSG_ID_AHRS2_LEN, MAVLINK_MSG_ID_AHRS2_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS2, buf, MAVLINK_MSG_ID_AHRS2_LEN); +#endif +#else + mavlink_ahrs2_t *packet = (mavlink_ahrs2_t *)msgbuf; + packet->roll = roll; + packet->pitch = pitch; + packet->yaw = yaw; + packet->altitude = altitude; + packet->lat = lat; + packet->lng = lng; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS2, (const char *)packet, MAVLINK_MSG_ID_AHRS2_LEN, MAVLINK_MSG_ID_AHRS2_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS2, (const char *)packet, MAVLINK_MSG_ID_AHRS2_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE AHRS2 UNPACKING diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_airspeed_autocal.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_airspeed_autocal.h index d046f2ad09..521831c8ff 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_airspeed_autocal.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_airspeed_autocal.h @@ -267,6 +267,60 @@ static inline void mavlink_msg_airspeed_autocal_send(mavlink_channel_t chan, flo #endif } +#if MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_airspeed_autocal_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float vx, float vy, float vz, float diff_pressure, float EAS2TAS, float ratio, float state_x, float state_y, float state_z, float Pax, float Pby, float Pcz) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, vx); + _mav_put_float(buf, 4, vy); + _mav_put_float(buf, 8, vz); + _mav_put_float(buf, 12, diff_pressure); + _mav_put_float(buf, 16, EAS2TAS); + _mav_put_float(buf, 20, ratio); + _mav_put_float(buf, 24, state_x); + _mav_put_float(buf, 28, state_y); + _mav_put_float(buf, 32, state_z); + _mav_put_float(buf, 36, Pax); + _mav_put_float(buf, 40, Pby); + _mav_put_float(buf, 44, Pcz); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL, buf, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL, buf, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); +#endif +#else + mavlink_airspeed_autocal_t *packet = (mavlink_airspeed_autocal_t *)msgbuf; + packet->vx = vx; + packet->vy = vy; + packet->vz = vz; + packet->diff_pressure = diff_pressure; + packet->EAS2TAS = EAS2TAS; + packet->ratio = ratio; + packet->state_x = state_x; + packet->state_y = state_y; + packet->state_z = state_z; + packet->Pax = Pax; + packet->Pby = Pby; + packet->Pcz = Pcz; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL, (const char *)packet, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL, (const char *)packet, MAVLINK_MSG_ID_AIRSPEED_AUTOCAL_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE AIRSPEED_AUTOCAL UNPACKING diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ap_adc.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ap_adc.h index 821ce73e47..3c18e644ef 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ap_adc.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ap_adc.h @@ -201,6 +201,48 @@ static inline void mavlink_msg_ap_adc_send(mavlink_channel_t chan, uint16_t adc1 #endif } +#if MAVLINK_MSG_ID_AP_ADC_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_ap_adc_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t adc1, uint16_t adc2, uint16_t adc3, uint16_t adc4, uint16_t adc5, uint16_t adc6) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, adc1); + _mav_put_uint16_t(buf, 2, adc2); + _mav_put_uint16_t(buf, 4, adc3); + _mav_put_uint16_t(buf, 6, adc4); + _mav_put_uint16_t(buf, 8, adc5); + _mav_put_uint16_t(buf, 10, adc6); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AP_ADC, buf, MAVLINK_MSG_ID_AP_ADC_LEN, MAVLINK_MSG_ID_AP_ADC_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AP_ADC, buf, MAVLINK_MSG_ID_AP_ADC_LEN); +#endif +#else + mavlink_ap_adc_t *packet = (mavlink_ap_adc_t *)msgbuf; + packet->adc1 = adc1; + packet->adc2 = adc2; + packet->adc3 = adc3; + packet->adc4 = adc4; + packet->adc5 = adc5; + packet->adc6 = adc6; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AP_ADC, (const char *)packet, MAVLINK_MSG_ID_AP_ADC_LEN, MAVLINK_MSG_ID_AP_ADC_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AP_ADC, (const char *)packet, MAVLINK_MSG_ID_AP_ADC_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE AP_ADC UNPACKING diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_compassmot_status.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_compassmot_status.h new file mode 100644 index 0000000000..9655459889 --- /dev/null +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_compassmot_status.h @@ -0,0 +1,329 @@ +// MESSAGE COMPASSMOT_STATUS PACKING + +#define MAVLINK_MSG_ID_COMPASSMOT_STATUS 177 + +typedef struct __mavlink_compassmot_status_t +{ + float current; ///< current (amps) + float CompensationX; ///< Motor Compensation X + float CompensationY; ///< Motor Compensation Y + float CompensationZ; ///< Motor Compensation Z + uint16_t throttle; ///< throttle (percent*10) + uint16_t interference; ///< interference (percent) +} mavlink_compassmot_status_t; + +#define MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN 20 +#define MAVLINK_MSG_ID_177_LEN 20 + +#define MAVLINK_MSG_ID_COMPASSMOT_STATUS_CRC 240 +#define MAVLINK_MSG_ID_177_CRC 240 + + + +#define MAVLINK_MESSAGE_INFO_COMPASSMOT_STATUS { \ + "COMPASSMOT_STATUS", \ + 6, \ + { { "current", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_compassmot_status_t, current) }, \ + { "CompensationX", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_compassmot_status_t, CompensationX) }, \ + { "CompensationY", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_compassmot_status_t, CompensationY) }, \ + { "CompensationZ", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_compassmot_status_t, CompensationZ) }, \ + { "throttle", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_compassmot_status_t, throttle) }, \ + { "interference", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_compassmot_status_t, interference) }, \ + } \ +} + + +/** + * @brief Pack a compassmot_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param throttle throttle (percent*10) + * @param current current (amps) + * @param interference interference (percent) + * @param CompensationX Motor Compensation X + * @param CompensationY Motor Compensation Y + * @param CompensationZ Motor Compensation Z + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_compassmot_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint16_t throttle, float current, uint16_t interference, float CompensationX, float CompensationY, float CompensationZ) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN]; + _mav_put_float(buf, 0, current); + _mav_put_float(buf, 4, CompensationX); + _mav_put_float(buf, 8, CompensationY); + _mav_put_float(buf, 12, CompensationZ); + _mav_put_uint16_t(buf, 16, throttle); + _mav_put_uint16_t(buf, 18, interference); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN); +#else + mavlink_compassmot_status_t packet; + packet.current = current; + packet.CompensationX = CompensationX; + packet.CompensationY = CompensationY; + packet.CompensationZ = CompensationZ; + packet.throttle = throttle; + packet.interference = interference; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_COMPASSMOT_STATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN, MAVLINK_MSG_ID_COMPASSMOT_STATUS_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN); +#endif +} + +/** + * @brief Pack a compassmot_status message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param throttle throttle (percent*10) + * @param current current (amps) + * @param interference interference (percent) + * @param CompensationX Motor Compensation X + * @param CompensationY Motor Compensation Y + * @param CompensationZ Motor Compensation Z + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_compassmot_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint16_t throttle,float current,uint16_t interference,float CompensationX,float CompensationY,float CompensationZ) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN]; + _mav_put_float(buf, 0, current); + _mav_put_float(buf, 4, CompensationX); + _mav_put_float(buf, 8, CompensationY); + _mav_put_float(buf, 12, CompensationZ); + _mav_put_uint16_t(buf, 16, throttle); + _mav_put_uint16_t(buf, 18, interference); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN); +#else + mavlink_compassmot_status_t packet; + packet.current = current; + packet.CompensationX = CompensationX; + packet.CompensationY = CompensationY; + packet.CompensationZ = CompensationZ; + packet.throttle = throttle; + packet.interference = interference; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_COMPASSMOT_STATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN, MAVLINK_MSG_ID_COMPASSMOT_STATUS_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN); +#endif +} + +/** + * @brief Encode a compassmot_status struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param compassmot_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_compassmot_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_compassmot_status_t* compassmot_status) +{ + return mavlink_msg_compassmot_status_pack(system_id, component_id, msg, compassmot_status->throttle, compassmot_status->current, compassmot_status->interference, compassmot_status->CompensationX, compassmot_status->CompensationY, compassmot_status->CompensationZ); +} + +/** + * @brief Encode a compassmot_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param compassmot_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_compassmot_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_compassmot_status_t* compassmot_status) +{ + return mavlink_msg_compassmot_status_pack_chan(system_id, component_id, chan, msg, compassmot_status->throttle, compassmot_status->current, compassmot_status->interference, compassmot_status->CompensationX, compassmot_status->CompensationY, compassmot_status->CompensationZ); +} + +/** + * @brief Send a compassmot_status message + * @param chan MAVLink channel to send the message + * + * @param throttle throttle (percent*10) + * @param current current (amps) + * @param interference interference (percent) + * @param CompensationX Motor Compensation X + * @param CompensationY Motor Compensation Y + * @param CompensationZ Motor Compensation Z + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_compassmot_status_send(mavlink_channel_t chan, uint16_t throttle, float current, uint16_t interference, float CompensationX, float CompensationY, float CompensationZ) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN]; + _mav_put_float(buf, 0, current); + _mav_put_float(buf, 4, CompensationX); + _mav_put_float(buf, 8, CompensationY); + _mav_put_float(buf, 12, CompensationZ); + _mav_put_uint16_t(buf, 16, throttle); + _mav_put_uint16_t(buf, 18, interference); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPASSMOT_STATUS, buf, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN, MAVLINK_MSG_ID_COMPASSMOT_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPASSMOT_STATUS, buf, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN); +#endif +#else + mavlink_compassmot_status_t packet; + packet.current = current; + packet.CompensationX = CompensationX; + packet.CompensationY = CompensationY; + packet.CompensationZ = CompensationZ; + packet.throttle = throttle; + packet.interference = interference; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPASSMOT_STATUS, (const char *)&packet, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN, MAVLINK_MSG_ID_COMPASSMOT_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPASSMOT_STATUS, (const char *)&packet, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_compassmot_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t throttle, float current, uint16_t interference, float CompensationX, float CompensationY, float CompensationZ) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, current); + _mav_put_float(buf, 4, CompensationX); + _mav_put_float(buf, 8, CompensationY); + _mav_put_float(buf, 12, CompensationZ); + _mav_put_uint16_t(buf, 16, throttle); + _mav_put_uint16_t(buf, 18, interference); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPASSMOT_STATUS, buf, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN, MAVLINK_MSG_ID_COMPASSMOT_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPASSMOT_STATUS, buf, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN); +#endif +#else + mavlink_compassmot_status_t *packet = (mavlink_compassmot_status_t *)msgbuf; + packet->current = current; + packet->CompensationX = CompensationX; + packet->CompensationY = CompensationY; + packet->CompensationZ = CompensationZ; + packet->throttle = throttle; + packet->interference = interference; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPASSMOT_STATUS, (const char *)packet, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN, MAVLINK_MSG_ID_COMPASSMOT_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPASSMOT_STATUS, (const char *)packet, MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE COMPASSMOT_STATUS UNPACKING + + +/** + * @brief Get field throttle from compassmot_status message + * + * @return throttle (percent*10) + */ +static inline uint16_t mavlink_msg_compassmot_status_get_throttle(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 16); +} + +/** + * @brief Get field current from compassmot_status message + * + * @return current (amps) + */ +static inline float mavlink_msg_compassmot_status_get_current(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field interference from compassmot_status message + * + * @return interference (percent) + */ +static inline uint16_t mavlink_msg_compassmot_status_get_interference(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 18); +} + +/** + * @brief Get field CompensationX from compassmot_status message + * + * @return Motor Compensation X + */ +static inline float mavlink_msg_compassmot_status_get_CompensationX(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field CompensationY from compassmot_status message + * + * @return Motor Compensation Y + */ +static inline float mavlink_msg_compassmot_status_get_CompensationY(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field CompensationZ from compassmot_status message + * + * @return Motor Compensation Z + */ +static inline float mavlink_msg_compassmot_status_get_CompensationZ(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Decode a compassmot_status message into a struct + * + * @param msg The message to decode + * @param compassmot_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_compassmot_status_decode(const mavlink_message_t* msg, mavlink_compassmot_status_t* compassmot_status) +{ +#if MAVLINK_NEED_BYTE_SWAP + compassmot_status->current = mavlink_msg_compassmot_status_get_current(msg); + compassmot_status->CompensationX = mavlink_msg_compassmot_status_get_CompensationX(msg); + compassmot_status->CompensationY = mavlink_msg_compassmot_status_get_CompensationY(msg); + compassmot_status->CompensationZ = mavlink_msg_compassmot_status_get_CompensationZ(msg); + compassmot_status->throttle = mavlink_msg_compassmot_status_get_throttle(msg); + compassmot_status->interference = mavlink_msg_compassmot_status_get_interference(msg); +#else + memcpy(compassmot_status, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_COMPASSMOT_STATUS_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data16.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data16.h index 9200eefa0d..7bad1b23de 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data16.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data16.h @@ -162,6 +162,40 @@ static inline void mavlink_msg_data16_send(mavlink_channel_t chan, uint8_t type, #endif } +#if MAVLINK_MSG_ID_DATA16_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_data16_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t type, uint8_t len, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, type); + _mav_put_uint8_t(buf, 1, len); + _mav_put_uint8_t_array(buf, 2, data, 16); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA16, buf, MAVLINK_MSG_ID_DATA16_LEN, MAVLINK_MSG_ID_DATA16_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA16, buf, MAVLINK_MSG_ID_DATA16_LEN); +#endif +#else + mavlink_data16_t *packet = (mavlink_data16_t *)msgbuf; + packet->type = type; + packet->len = len; + mav_array_memcpy(packet->data, data, sizeof(uint8_t)*16); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA16, (const char *)packet, MAVLINK_MSG_ID_DATA16_LEN, MAVLINK_MSG_ID_DATA16_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA16, (const char *)packet, MAVLINK_MSG_ID_DATA16_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE DATA16 UNPACKING diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data32.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data32.h index 3afedb7874..df7a2ec801 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data32.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data32.h @@ -162,6 +162,40 @@ static inline void mavlink_msg_data32_send(mavlink_channel_t chan, uint8_t type, #endif } +#if MAVLINK_MSG_ID_DATA32_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_data32_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t type, uint8_t len, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, type); + _mav_put_uint8_t(buf, 1, len); + _mav_put_uint8_t_array(buf, 2, data, 32); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA32, buf, MAVLINK_MSG_ID_DATA32_LEN, MAVLINK_MSG_ID_DATA32_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA32, buf, MAVLINK_MSG_ID_DATA32_LEN); +#endif +#else + mavlink_data32_t *packet = (mavlink_data32_t *)msgbuf; + packet->type = type; + packet->len = len; + mav_array_memcpy(packet->data, data, sizeof(uint8_t)*32); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA32, (const char *)packet, MAVLINK_MSG_ID_DATA32_LEN, MAVLINK_MSG_ID_DATA32_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA32, (const char *)packet, MAVLINK_MSG_ID_DATA32_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE DATA32 UNPACKING diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data64.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data64.h index 6931ada167..c354f8908a 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data64.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data64.h @@ -162,6 +162,40 @@ static inline void mavlink_msg_data64_send(mavlink_channel_t chan, uint8_t type, #endif } +#if MAVLINK_MSG_ID_DATA64_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_data64_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t type, uint8_t len, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, type); + _mav_put_uint8_t(buf, 1, len); + _mav_put_uint8_t_array(buf, 2, data, 64); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA64, buf, MAVLINK_MSG_ID_DATA64_LEN, MAVLINK_MSG_ID_DATA64_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA64, buf, MAVLINK_MSG_ID_DATA64_LEN); +#endif +#else + mavlink_data64_t *packet = (mavlink_data64_t *)msgbuf; + packet->type = type; + packet->len = len; + mav_array_memcpy(packet->data, data, sizeof(uint8_t)*64); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA64, (const char *)packet, MAVLINK_MSG_ID_DATA64_LEN, MAVLINK_MSG_ID_DATA64_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA64, (const char *)packet, MAVLINK_MSG_ID_DATA64_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE DATA64 UNPACKING diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data96.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data96.h index cffc7d7e7f..634e3f81d5 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data96.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_data96.h @@ -162,6 +162,40 @@ static inline void mavlink_msg_data96_send(mavlink_channel_t chan, uint8_t type, #endif } +#if MAVLINK_MSG_ID_DATA96_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_data96_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t type, uint8_t len, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, type); + _mav_put_uint8_t(buf, 1, len); + _mav_put_uint8_t_array(buf, 2, data, 96); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA96, buf, MAVLINK_MSG_ID_DATA96_LEN, MAVLINK_MSG_ID_DATA96_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA96, buf, MAVLINK_MSG_ID_DATA96_LEN); +#endif +#else + mavlink_data96_t *packet = (mavlink_data96_t *)msgbuf; + packet->type = type; + packet->len = len; + mav_array_memcpy(packet->data, data, sizeof(uint8_t)*96); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA96, (const char *)packet, MAVLINK_MSG_ID_DATA96_LEN, MAVLINK_MSG_ID_DATA96_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA96, (const char *)packet, MAVLINK_MSG_ID_DATA96_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE DATA96 UNPACKING diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_configure.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_configure.h index c6518c4199..d5f9e7132c 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_configure.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_configure.h @@ -256,6 +256,58 @@ static inline void mavlink_msg_digicam_configure_send(mavlink_channel_t chan, ui #endif } +#if MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_digicam_configure_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t mode, uint16_t shutter_speed, uint8_t aperture, uint8_t iso, uint8_t exposure_type, uint8_t command_id, uint8_t engine_cut_off, uint8_t extra_param, float extra_value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, extra_value); + _mav_put_uint16_t(buf, 4, shutter_speed); + _mav_put_uint8_t(buf, 6, target_system); + _mav_put_uint8_t(buf, 7, target_component); + _mav_put_uint8_t(buf, 8, mode); + _mav_put_uint8_t(buf, 9, aperture); + _mav_put_uint8_t(buf, 10, iso); + _mav_put_uint8_t(buf, 11, exposure_type); + _mav_put_uint8_t(buf, 12, command_id); + _mav_put_uint8_t(buf, 13, engine_cut_off); + _mav_put_uint8_t(buf, 14, extra_param); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONFIGURE, buf, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONFIGURE, buf, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN); +#endif +#else + mavlink_digicam_configure_t *packet = (mavlink_digicam_configure_t *)msgbuf; + packet->extra_value = extra_value; + packet->shutter_speed = shutter_speed; + packet->target_system = target_system; + packet->target_component = target_component; + packet->mode = mode; + packet->aperture = aperture; + packet->iso = iso; + packet->exposure_type = exposure_type; + packet->command_id = command_id; + packet->engine_cut_off = engine_cut_off; + packet->extra_param = extra_param; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONFIGURE, (const char *)packet, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONFIGURE, (const char *)packet, MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE DIGICAM_CONFIGURE UNPACKING diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_control.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_control.h index bfa5414a39..68484f7b84 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_control.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_digicam_control.h @@ -245,6 +245,56 @@ static inline void mavlink_msg_digicam_control_send(mavlink_channel_t chan, uint #endif } +#if MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_digicam_control_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t session, uint8_t zoom_pos, int8_t zoom_step, uint8_t focus_lock, uint8_t shot, uint8_t command_id, uint8_t extra_param, float extra_value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, extra_value); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 6, session); + _mav_put_uint8_t(buf, 7, zoom_pos); + _mav_put_int8_t(buf, 8, zoom_step); + _mav_put_uint8_t(buf, 9, focus_lock); + _mav_put_uint8_t(buf, 10, shot); + _mav_put_uint8_t(buf, 11, command_id); + _mav_put_uint8_t(buf, 12, extra_param); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONTROL, buf, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN, MAVLINK_MSG_ID_DIGICAM_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONTROL, buf, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN); +#endif +#else + mavlink_digicam_control_t *packet = (mavlink_digicam_control_t *)msgbuf; + packet->extra_value = extra_value; + packet->target_system = target_system; + packet->target_component = target_component; + packet->session = session; + packet->zoom_pos = zoom_pos; + packet->zoom_step = zoom_step; + packet->focus_lock = focus_lock; + packet->shot = shot; + packet->command_id = command_id; + packet->extra_param = extra_param; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONTROL, (const char *)packet, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN, MAVLINK_MSG_ID_DIGICAM_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIGICAM_CONTROL, (const char *)packet, MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE DIGICAM_CONTROL UNPACKING diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_fetch_point.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_fetch_point.h index fe3677d53d..4021c7a096 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_fetch_point.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_fetch_point.h @@ -168,6 +168,42 @@ static inline void mavlink_msg_fence_fetch_point_send(mavlink_channel_t chan, ui #endif } +#if MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_fence_fetch_point_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t idx) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, idx); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_FETCH_POINT, buf, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN, MAVLINK_MSG_ID_FENCE_FETCH_POINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_FETCH_POINT, buf, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN); +#endif +#else + mavlink_fence_fetch_point_t *packet = (mavlink_fence_fetch_point_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + packet->idx = idx; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_FETCH_POINT, (const char *)packet, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN, MAVLINK_MSG_ID_FENCE_FETCH_POINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_FETCH_POINT, (const char *)packet, MAVLINK_MSG_ID_FENCE_FETCH_POINT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE FENCE_FETCH_POINT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_point.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_point.h index febda6cdc5..6fc2c83fc4 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_point.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_point.h @@ -201,6 +201,48 @@ static inline void mavlink_msg_fence_point_send(mavlink_channel_t chan, uint8_t #endif } +#if MAVLINK_MSG_ID_FENCE_POINT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_fence_point_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t idx, uint8_t count, float lat, float lng) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, lat); + _mav_put_float(buf, 4, lng); + _mav_put_uint8_t(buf, 8, target_system); + _mav_put_uint8_t(buf, 9, target_component); + _mav_put_uint8_t(buf, 10, idx); + _mav_put_uint8_t(buf, 11, count); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_POINT, buf, MAVLINK_MSG_ID_FENCE_POINT_LEN, MAVLINK_MSG_ID_FENCE_POINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_POINT, buf, MAVLINK_MSG_ID_FENCE_POINT_LEN); +#endif +#else + mavlink_fence_point_t *packet = (mavlink_fence_point_t *)msgbuf; + packet->lat = lat; + packet->lng = lng; + packet->target_system = target_system; + packet->target_component = target_component; + packet->idx = idx; + packet->count = count; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_POINT, (const char *)packet, MAVLINK_MSG_ID_FENCE_POINT_LEN, MAVLINK_MSG_ID_FENCE_POINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_POINT, (const char *)packet, MAVLINK_MSG_ID_FENCE_POINT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE FENCE_POINT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_status.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_status.h index 6120904061..c50d37724e 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_status.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_fence_status.h @@ -179,6 +179,44 @@ static inline void mavlink_msg_fence_status_send(mavlink_channel_t chan, uint8_t #endif } +#if MAVLINK_MSG_ID_FENCE_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_fence_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t breach_status, uint16_t breach_count, uint8_t breach_type, uint32_t breach_time) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, breach_time); + _mav_put_uint16_t(buf, 4, breach_count); + _mav_put_uint8_t(buf, 6, breach_status); + _mav_put_uint8_t(buf, 7, breach_type); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_STATUS, buf, MAVLINK_MSG_ID_FENCE_STATUS_LEN, MAVLINK_MSG_ID_FENCE_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_STATUS, buf, MAVLINK_MSG_ID_FENCE_STATUS_LEN); +#endif +#else + mavlink_fence_status_t *packet = (mavlink_fence_status_t *)msgbuf; + packet->breach_time = breach_time; + packet->breach_count = breach_count; + packet->breach_status = breach_status; + packet->breach_type = breach_type; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_STATUS, (const char *)packet, MAVLINK_MSG_ID_FENCE_STATUS_LEN, MAVLINK_MSG_ID_FENCE_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_STATUS, (const char *)packet, MAVLINK_MSG_ID_FENCE_STATUS_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE FENCE_STATUS UNPACKING diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_hwstatus.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_hwstatus.h index 2f5dea513a..acf031f62d 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_hwstatus.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_hwstatus.h @@ -157,6 +157,40 @@ static inline void mavlink_msg_hwstatus_send(mavlink_channel_t chan, uint16_t Vc #endif } +#if MAVLINK_MSG_ID_HWSTATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_hwstatus_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t Vcc, uint8_t I2Cerr) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, Vcc); + _mav_put_uint8_t(buf, 2, I2Cerr); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HWSTATUS, buf, MAVLINK_MSG_ID_HWSTATUS_LEN, MAVLINK_MSG_ID_HWSTATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HWSTATUS, buf, MAVLINK_MSG_ID_HWSTATUS_LEN); +#endif +#else + mavlink_hwstatus_t *packet = (mavlink_hwstatus_t *)msgbuf; + packet->Vcc = Vcc; + packet->I2Cerr = I2Cerr; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HWSTATUS, (const char *)packet, MAVLINK_MSG_ID_HWSTATUS_LEN, MAVLINK_MSG_ID_HWSTATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HWSTATUS, (const char *)packet, MAVLINK_MSG_ID_HWSTATUS_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE HWSTATUS UNPACKING diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_limits_status.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_limits_status.h index 34743fd021..5fef937181 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_limits_status.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_limits_status.h @@ -234,6 +234,54 @@ static inline void mavlink_msg_limits_status_send(mavlink_channel_t chan, uint8_ #endif } +#if MAVLINK_MSG_ID_LIMITS_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_limits_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t limits_state, uint32_t last_trigger, uint32_t last_action, uint32_t last_recovery, uint32_t last_clear, uint16_t breach_count, uint8_t mods_enabled, uint8_t mods_required, uint8_t mods_triggered) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, last_trigger); + _mav_put_uint32_t(buf, 4, last_action); + _mav_put_uint32_t(buf, 8, last_recovery); + _mav_put_uint32_t(buf, 12, last_clear); + _mav_put_uint16_t(buf, 16, breach_count); + _mav_put_uint8_t(buf, 18, limits_state); + _mav_put_uint8_t(buf, 19, mods_enabled); + _mav_put_uint8_t(buf, 20, mods_required); + _mav_put_uint8_t(buf, 21, mods_triggered); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LIMITS_STATUS, buf, MAVLINK_MSG_ID_LIMITS_STATUS_LEN, MAVLINK_MSG_ID_LIMITS_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LIMITS_STATUS, buf, MAVLINK_MSG_ID_LIMITS_STATUS_LEN); +#endif +#else + mavlink_limits_status_t *packet = (mavlink_limits_status_t *)msgbuf; + packet->last_trigger = last_trigger; + packet->last_action = last_action; + packet->last_recovery = last_recovery; + packet->last_clear = last_clear; + packet->breach_count = breach_count; + packet->limits_state = limits_state; + packet->mods_enabled = mods_enabled; + packet->mods_required = mods_required; + packet->mods_triggered = mods_triggered; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LIMITS_STATUS, (const char *)packet, MAVLINK_MSG_ID_LIMITS_STATUS_LEN, MAVLINK_MSG_ID_LIMITS_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LIMITS_STATUS, (const char *)packet, MAVLINK_MSG_ID_LIMITS_STATUS_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE LIMITS_STATUS UNPACKING diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_meminfo.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_meminfo.h index 55f772bbc5..c64b2e9377 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_meminfo.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_meminfo.h @@ -157,6 +157,40 @@ static inline void mavlink_msg_meminfo_send(mavlink_channel_t chan, uint16_t brk #endif } +#if MAVLINK_MSG_ID_MEMINFO_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_meminfo_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t brkval, uint16_t freemem) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, brkval); + _mav_put_uint16_t(buf, 2, freemem); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMINFO, buf, MAVLINK_MSG_ID_MEMINFO_LEN, MAVLINK_MSG_ID_MEMINFO_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMINFO, buf, MAVLINK_MSG_ID_MEMINFO_LEN); +#endif +#else + mavlink_meminfo_t *packet = (mavlink_meminfo_t *)msgbuf; + packet->brkval = brkval; + packet->freemem = freemem; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMINFO, (const char *)packet, MAVLINK_MSG_ID_MEMINFO_LEN, MAVLINK_MSG_ID_MEMINFO_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMINFO, (const char *)packet, MAVLINK_MSG_ID_MEMINFO_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE MEMINFO UNPACKING diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_configure.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_configure.h index de717dfa4d..350bb0b785 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_configure.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_configure.h @@ -201,6 +201,48 @@ static inline void mavlink_msg_mount_configure_send(mavlink_channel_t chan, uint #endif } +#if MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mount_configure_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t mount_mode, uint8_t stab_roll, uint8_t stab_pitch, uint8_t stab_yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, mount_mode); + _mav_put_uint8_t(buf, 3, stab_roll); + _mav_put_uint8_t(buf, 4, stab_pitch); + _mav_put_uint8_t(buf, 5, stab_yaw); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONFIGURE, buf, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN, MAVLINK_MSG_ID_MOUNT_CONFIGURE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONFIGURE, buf, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN); +#endif +#else + mavlink_mount_configure_t *packet = (mavlink_mount_configure_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + packet->mount_mode = mount_mode; + packet->stab_roll = stab_roll; + packet->stab_pitch = stab_pitch; + packet->stab_yaw = stab_yaw; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONFIGURE, (const char *)packet, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN, MAVLINK_MSG_ID_MOUNT_CONFIGURE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONFIGURE, (const char *)packet, MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE MOUNT_CONFIGURE UNPACKING diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_control.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_control.h index 44416353ed..72d6e24197 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_control.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_control.h @@ -201,6 +201,48 @@ static inline void mavlink_msg_mount_control_send(mavlink_channel_t chan, uint8_ #endif } +#if MAVLINK_MSG_ID_MOUNT_CONTROL_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mount_control_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int32_t input_a, int32_t input_b, int32_t input_c, uint8_t save_position) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int32_t(buf, 0, input_a); + _mav_put_int32_t(buf, 4, input_b); + _mav_put_int32_t(buf, 8, input_c); + _mav_put_uint8_t(buf, 12, target_system); + _mav_put_uint8_t(buf, 13, target_component); + _mav_put_uint8_t(buf, 14, save_position); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONTROL, buf, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN, MAVLINK_MSG_ID_MOUNT_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONTROL, buf, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN); +#endif +#else + mavlink_mount_control_t *packet = (mavlink_mount_control_t *)msgbuf; + packet->input_a = input_a; + packet->input_b = input_b; + packet->input_c = input_c; + packet->target_system = target_system; + packet->target_component = target_component; + packet->save_position = save_position; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONTROL, (const char *)packet, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN, MAVLINK_MSG_ID_MOUNT_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_CONTROL, (const char *)packet, MAVLINK_MSG_ID_MOUNT_CONTROL_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE MOUNT_CONTROL UNPACKING diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_status.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_status.h index 4905905dc5..c140dd2c7d 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_status.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_mount_status.h @@ -190,6 +190,46 @@ static inline void mavlink_msg_mount_status_send(mavlink_channel_t chan, uint8_t #endif } +#if MAVLINK_MSG_ID_MOUNT_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mount_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int32_t pointing_a, int32_t pointing_b, int32_t pointing_c) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int32_t(buf, 0, pointing_a); + _mav_put_int32_t(buf, 4, pointing_b); + _mav_put_int32_t(buf, 8, pointing_c); + _mav_put_uint8_t(buf, 12, target_system); + _mav_put_uint8_t(buf, 13, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_STATUS, buf, MAVLINK_MSG_ID_MOUNT_STATUS_LEN, MAVLINK_MSG_ID_MOUNT_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_STATUS, buf, MAVLINK_MSG_ID_MOUNT_STATUS_LEN); +#endif +#else + mavlink_mount_status_t *packet = (mavlink_mount_status_t *)msgbuf; + packet->pointing_a = pointing_a; + packet->pointing_b = pointing_b; + packet->pointing_c = pointing_c; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_STATUS, (const char *)packet, MAVLINK_MSG_ID_MOUNT_STATUS_LEN, MAVLINK_MSG_ID_MOUNT_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_STATUS, (const char *)packet, MAVLINK_MSG_ID_MOUNT_STATUS_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE MOUNT_STATUS UNPACKING diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_radio.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_radio.h index 8e9740e828..830eb4639a 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_radio.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_radio.h @@ -212,6 +212,50 @@ static inline void mavlink_msg_radio_send(mavlink_channel_t chan, uint8_t rssi, #endif } +#if MAVLINK_MSG_ID_RADIO_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_radio_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t rssi, uint8_t remrssi, uint8_t txbuf, uint8_t noise, uint8_t remnoise, uint16_t rxerrors, uint16_t fixed) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, rxerrors); + _mav_put_uint16_t(buf, 2, fixed); + _mav_put_uint8_t(buf, 4, rssi); + _mav_put_uint8_t(buf, 5, remrssi); + _mav_put_uint8_t(buf, 6, txbuf); + _mav_put_uint8_t(buf, 7, noise); + _mav_put_uint8_t(buf, 8, remnoise); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO, buf, MAVLINK_MSG_ID_RADIO_LEN, MAVLINK_MSG_ID_RADIO_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO, buf, MAVLINK_MSG_ID_RADIO_LEN); +#endif +#else + mavlink_radio_t *packet = (mavlink_radio_t *)msgbuf; + packet->rxerrors = rxerrors; + packet->fixed = fixed; + packet->rssi = rssi; + packet->remrssi = remrssi; + packet->txbuf = txbuf; + packet->noise = noise; + packet->remnoise = remnoise; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO, (const char *)packet, MAVLINK_MSG_ID_RADIO_LEN, MAVLINK_MSG_ID_RADIO_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO, (const char *)packet, MAVLINK_MSG_ID_RADIO_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE RADIO UNPACKING diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rally_fetch_point.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rally_fetch_point.h index f057e3c335..4598251fe1 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rally_fetch_point.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rally_fetch_point.h @@ -168,6 +168,42 @@ static inline void mavlink_msg_rally_fetch_point_send(mavlink_channel_t chan, ui #endif } +#if MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_rally_fetch_point_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t idx) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, idx); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_FETCH_POINT, buf, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN, MAVLINK_MSG_ID_RALLY_FETCH_POINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_FETCH_POINT, buf, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN); +#endif +#else + mavlink_rally_fetch_point_t *packet = (mavlink_rally_fetch_point_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + packet->idx = idx; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_FETCH_POINT, (const char *)packet, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN, MAVLINK_MSG_ID_RALLY_FETCH_POINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_FETCH_POINT, (const char *)packet, MAVLINK_MSG_ID_RALLY_FETCH_POINT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE RALLY_FETCH_POINT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rally_point.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rally_point.h index 2c21db4c0e..b44e764a94 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rally_point.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rally_point.h @@ -245,6 +245,56 @@ static inline void mavlink_msg_rally_point_send(mavlink_channel_t chan, uint8_t #endif } +#if MAVLINK_MSG_ID_RALLY_POINT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_rally_point_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t idx, uint8_t count, int32_t lat, int32_t lng, int16_t alt, int16_t break_alt, uint16_t land_dir, uint8_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int32_t(buf, 0, lat); + _mav_put_int32_t(buf, 4, lng); + _mav_put_int16_t(buf, 8, alt); + _mav_put_int16_t(buf, 10, break_alt); + _mav_put_uint16_t(buf, 12, land_dir); + _mav_put_uint8_t(buf, 14, target_system); + _mav_put_uint8_t(buf, 15, target_component); + _mav_put_uint8_t(buf, 16, idx); + _mav_put_uint8_t(buf, 17, count); + _mav_put_uint8_t(buf, 18, flags); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_POINT, buf, MAVLINK_MSG_ID_RALLY_POINT_LEN, MAVLINK_MSG_ID_RALLY_POINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_POINT, buf, MAVLINK_MSG_ID_RALLY_POINT_LEN); +#endif +#else + mavlink_rally_point_t *packet = (mavlink_rally_point_t *)msgbuf; + packet->lat = lat; + packet->lng = lng; + packet->alt = alt; + packet->break_alt = break_alt; + packet->land_dir = land_dir; + packet->target_system = target_system; + packet->target_component = target_component; + packet->idx = idx; + packet->count = count; + packet->flags = flags; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_POINT, (const char *)packet, MAVLINK_MSG_ID_RALLY_POINT_LEN, MAVLINK_MSG_ID_RALLY_POINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RALLY_POINT, (const char *)packet, MAVLINK_MSG_ID_RALLY_POINT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE RALLY_POINT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rangefinder.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rangefinder.h index c476447a87..464ce8a47f 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rangefinder.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_rangefinder.h @@ -157,6 +157,40 @@ static inline void mavlink_msg_rangefinder_send(mavlink_channel_t chan, float di #endif } +#if MAVLINK_MSG_ID_RANGEFINDER_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_rangefinder_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float distance, float voltage) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, distance); + _mav_put_float(buf, 4, voltage); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RANGEFINDER, buf, MAVLINK_MSG_ID_RANGEFINDER_LEN, MAVLINK_MSG_ID_RANGEFINDER_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RANGEFINDER, buf, MAVLINK_MSG_ID_RANGEFINDER_LEN); +#endif +#else + mavlink_rangefinder_t *packet = (mavlink_rangefinder_t *)msgbuf; + packet->distance = distance; + packet->voltage = voltage; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RANGEFINDER, (const char *)packet, MAVLINK_MSG_ID_RANGEFINDER_LEN, MAVLINK_MSG_ID_RANGEFINDER_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RANGEFINDER, (const char *)packet, MAVLINK_MSG_ID_RANGEFINDER_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE RANGEFINDER UNPACKING diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_sensor_offsets.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_sensor_offsets.h index 31b7d989de..d18f31c954 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_sensor_offsets.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_sensor_offsets.h @@ -267,6 +267,60 @@ static inline void mavlink_msg_sensor_offsets_send(mavlink_channel_t chan, int16 #endif } +#if MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_sensor_offsets_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, int16_t mag_ofs_x, int16_t mag_ofs_y, int16_t mag_ofs_z, float mag_declination, int32_t raw_press, int32_t raw_temp, float gyro_cal_x, float gyro_cal_y, float gyro_cal_z, float accel_cal_x, float accel_cal_y, float accel_cal_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, mag_declination); + _mav_put_int32_t(buf, 4, raw_press); + _mav_put_int32_t(buf, 8, raw_temp); + _mav_put_float(buf, 12, gyro_cal_x); + _mav_put_float(buf, 16, gyro_cal_y); + _mav_put_float(buf, 20, gyro_cal_z); + _mav_put_float(buf, 24, accel_cal_x); + _mav_put_float(buf, 28, accel_cal_y); + _mav_put_float(buf, 32, accel_cal_z); + _mav_put_int16_t(buf, 36, mag_ofs_x); + _mav_put_int16_t(buf, 38, mag_ofs_y); + _mav_put_int16_t(buf, 40, mag_ofs_z); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SENSOR_OFFSETS, buf, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN, MAVLINK_MSG_ID_SENSOR_OFFSETS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SENSOR_OFFSETS, buf, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN); +#endif +#else + mavlink_sensor_offsets_t *packet = (mavlink_sensor_offsets_t *)msgbuf; + packet->mag_declination = mag_declination; + packet->raw_press = raw_press; + packet->raw_temp = raw_temp; + packet->gyro_cal_x = gyro_cal_x; + packet->gyro_cal_y = gyro_cal_y; + packet->gyro_cal_z = gyro_cal_z; + packet->accel_cal_x = accel_cal_x; + packet->accel_cal_y = accel_cal_y; + packet->accel_cal_z = accel_cal_z; + packet->mag_ofs_x = mag_ofs_x; + packet->mag_ofs_y = mag_ofs_y; + packet->mag_ofs_z = mag_ofs_z; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SENSOR_OFFSETS, (const char *)packet, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN, MAVLINK_MSG_ID_SENSOR_OFFSETS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SENSOR_OFFSETS, (const char *)packet, MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SENSOR_OFFSETS UNPACKING diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_set_mag_offsets.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_set_mag_offsets.h index b2c629c39a..fc6aa71e06 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_set_mag_offsets.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_set_mag_offsets.h @@ -190,6 +190,46 @@ static inline void mavlink_msg_set_mag_offsets_send(mavlink_channel_t chan, uint #endif } +#if MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_set_mag_offsets_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int16_t mag_ofs_x, int16_t mag_ofs_y, int16_t mag_ofs_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int16_t(buf, 0, mag_ofs_x); + _mav_put_int16_t(buf, 2, mag_ofs_y); + _mav_put_int16_t(buf, 4, mag_ofs_z); + _mav_put_uint8_t(buf, 6, target_system); + _mav_put_uint8_t(buf, 7, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MAG_OFFSETS, buf, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN, MAVLINK_MSG_ID_SET_MAG_OFFSETS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MAG_OFFSETS, buf, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN); +#endif +#else + mavlink_set_mag_offsets_t *packet = (mavlink_set_mag_offsets_t *)msgbuf; + packet->mag_ofs_x = mag_ofs_x; + packet->mag_ofs_y = mag_ofs_y; + packet->mag_ofs_z = mag_ofs_z; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MAG_OFFSETS, (const char *)packet, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN, MAVLINK_MSG_ID_SET_MAG_OFFSETS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MAG_OFFSETS, (const char *)packet, MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SET_MAG_OFFSETS UNPACKING diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_simstate.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_simstate.h index 8ef44f76d5..48cfb6ffd5 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_simstate.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_simstate.h @@ -256,6 +256,58 @@ static inline void mavlink_msg_simstate_send(mavlink_channel_t chan, float roll, #endif } +#if MAVLINK_MSG_ID_SIMSTATE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_simstate_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float roll, float pitch, float yaw, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, int32_t lat, int32_t lng) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, roll); + _mav_put_float(buf, 4, pitch); + _mav_put_float(buf, 8, yaw); + _mav_put_float(buf, 12, xacc); + _mav_put_float(buf, 16, yacc); + _mav_put_float(buf, 20, zacc); + _mav_put_float(buf, 24, xgyro); + _mav_put_float(buf, 28, ygyro); + _mav_put_float(buf, 32, zgyro); + _mav_put_int32_t(buf, 36, lat); + _mav_put_int32_t(buf, 40, lng); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIMSTATE, buf, MAVLINK_MSG_ID_SIMSTATE_LEN, MAVLINK_MSG_ID_SIMSTATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIMSTATE, buf, MAVLINK_MSG_ID_SIMSTATE_LEN); +#endif +#else + mavlink_simstate_t *packet = (mavlink_simstate_t *)msgbuf; + packet->roll = roll; + packet->pitch = pitch; + packet->yaw = yaw; + packet->xacc = xacc; + packet->yacc = yacc; + packet->zacc = zacc; + packet->xgyro = xgyro; + packet->ygyro = ygyro; + packet->zgyro = zgyro; + packet->lat = lat; + packet->lng = lng; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIMSTATE, (const char *)packet, MAVLINK_MSG_ID_SIMSTATE_LEN, MAVLINK_MSG_ID_SIMSTATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIMSTATE, (const char *)packet, MAVLINK_MSG_ID_SIMSTATE_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SIMSTATE UNPACKING diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_wind.h b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_wind.h index 7608a7bd10..5d5edc4cc3 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_wind.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_wind.h @@ -168,6 +168,42 @@ static inline void mavlink_msg_wind_send(mavlink_channel_t chan, float direction #endif } +#if MAVLINK_MSG_ID_WIND_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_wind_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float direction, float speed, float speed_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, direction); + _mav_put_float(buf, 4, speed); + _mav_put_float(buf, 8, speed_z); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WIND, buf, MAVLINK_MSG_ID_WIND_LEN, MAVLINK_MSG_ID_WIND_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WIND, buf, MAVLINK_MSG_ID_WIND_LEN); +#endif +#else + mavlink_wind_t *packet = (mavlink_wind_t *)msgbuf; + packet->direction = direction; + packet->speed = speed; + packet->speed_z = speed_z; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WIND, (const char *)packet, MAVLINK_MSG_ID_WIND_LEN, MAVLINK_MSG_ID_WIND_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WIND, (const char *)packet, MAVLINK_MSG_ID_WIND_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE WIND UNPACKING diff --git a/mavlink/include/mavlink/v1.0/ardupilotmega/testsuite.h b/mavlink/include/mavlink/v1.0/ardupilotmega/testsuite.h index b2496334f7..489553ea4d 100644 --- a/mavlink/include/mavlink/v1.0/ardupilotmega/testsuite.h +++ b/mavlink/include/mavlink/v1.0/ardupilotmega/testsuite.h @@ -1398,6 +1398,59 @@ static void mavlink_test_rally_fetch_point(uint8_t system_id, uint8_t component_ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } +static void mavlink_test_compassmot_status(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_compassmot_status_t packet_in = { + 17.0, + }45.0, + }73.0, + }101.0, + }18067, + }18171, + }; + mavlink_compassmot_status_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.current = packet_in.current; + packet1.CompensationX = packet_in.CompensationX; + packet1.CompensationY = packet_in.CompensationY; + packet1.CompensationZ = packet_in.CompensationZ; + packet1.throttle = packet_in.throttle; + packet1.interference = packet_in.interference; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_compassmot_status_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_compassmot_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_compassmot_status_pack(system_id, component_id, &msg , packet1.throttle , packet1.current , packet1.interference , packet1.CompensationX , packet1.CompensationY , packet1.CompensationZ ); + mavlink_msg_compassmot_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_compassmot_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.throttle , packet1.current , packet1.interference , packet1.CompensationX , packet1.CompensationY , packet1.CompensationZ ); + mavlink_msg_compassmot_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ MAV_CMD_DO_SET_ROI=201, /* 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| */ + MAV_CMD_DO_DIGICAM_CONFIGURE=202, /* Mission command to configure an on-board camera controller system. |Modes: P, TV, AV, M, Etc| Shutter speed: Divisor number for one second| Aperture: F stop number| ISO number e.g. 80, 100, 200, Etc| Exposure type enumerator| Command Identity| Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)| */ + MAV_CMD_DO_DIGICAM_CONTROL=203, /* Mission command to control an on-board camera controller system. |Session control e.g. show/hide lens| Zoom's absolute position| Zooming step value to offset zoom from the current position| Focus Locking, Unlocking or Re-locking| Shooting Command| Command Identity| Empty| */ + MAV_CMD_DO_MOUNT_CONFIGURE=204, /* Mission command to configure a camera or antenna mount |Mount operation mode (see MAV_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty| */ + MAV_CMD_DO_MOUNT_CONTROL=205, /* Mission command to control a camera or antenna mount |pitch or lat in degrees, depending on mount mode.| roll or lon in degrees depending on mount mode| yaw or alt (in meters) depending on mount mode| reserved| reserved| reserved| MAV_MOUNT_MODE enum value| */ + MAV_CMD_DO_SET_CAM_TRIGG_DIST=206, /* Mission command to set CAM_TRIGG_DIST for this flight |Camera trigger distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_FENCE_ENABLE=207, /* Mission command to enable the geofence |enable? (0=disable, 1=enable)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_PARACHUTE=208, /* Mission command to trigger a parachute |action (0=disable, 1=enable, 2=release, for some systems see PARACHUTE_ACTION enum, not in general message set.)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_MOUNT_CONTROL_QUAT=220, /* Mission command to control a camera or antenna mount, using a quaternion as reference. |q1 - quaternion param #1| q2 - quaternion param #2| q3 - quaternion param #3| q4 - quaternion param #4| Empty| Empty| Empty| */ MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty| */ + MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Compass/Motor interference calibration: 0: no, 1: yes| Empty| */ MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS=242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| */ MAV_CMD_PREFLIGHT_STORAGE=245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty| */ MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN=246, /* Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty| */ diff --git a/mavlink/include/mavlink/v1.0/autoquad/mavlink_msg_aq_telemetry_f.h b/mavlink/include/mavlink/v1.0/autoquad/mavlink_msg_aq_telemetry_f.h index ed7c86bcb8..e78d5687aa 100644 --- a/mavlink/include/mavlink/v1.0/autoquad/mavlink_msg_aq_telemetry_f.h +++ b/mavlink/include/mavlink/v1.0/autoquad/mavlink_msg_aq_telemetry_f.h @@ -366,6 +366,78 @@ static inline void mavlink_msg_aq_telemetry_f_send(mavlink_channel_t chan, uint1 #endif } +#if MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_aq_telemetry_f_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t Index, float value1, float value2, float value3, float value4, float value5, float value6, float value7, float value8, float value9, float value10, float value11, float value12, float value13, float value14, float value15, float value16, float value17, float value18, float value19, float value20) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, value1); + _mav_put_float(buf, 4, value2); + _mav_put_float(buf, 8, value3); + _mav_put_float(buf, 12, value4); + _mav_put_float(buf, 16, value5); + _mav_put_float(buf, 20, value6); + _mav_put_float(buf, 24, value7); + _mav_put_float(buf, 28, value8); + _mav_put_float(buf, 32, value9); + _mav_put_float(buf, 36, value10); + _mav_put_float(buf, 40, value11); + _mav_put_float(buf, 44, value12); + _mav_put_float(buf, 48, value13); + _mav_put_float(buf, 52, value14); + _mav_put_float(buf, 56, value15); + _mav_put_float(buf, 60, value16); + _mav_put_float(buf, 64, value17); + _mav_put_float(buf, 68, value18); + _mav_put_float(buf, 72, value19); + _mav_put_float(buf, 76, value20); + _mav_put_uint16_t(buf, 80, Index); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AQ_TELEMETRY_F, buf, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN, MAVLINK_MSG_ID_AQ_TELEMETRY_F_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AQ_TELEMETRY_F, buf, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN); +#endif +#else + mavlink_aq_telemetry_f_t *packet = (mavlink_aq_telemetry_f_t *)msgbuf; + packet->value1 = value1; + packet->value2 = value2; + packet->value3 = value3; + packet->value4 = value4; + packet->value5 = value5; + packet->value6 = value6; + packet->value7 = value7; + packet->value8 = value8; + packet->value9 = value9; + packet->value10 = value10; + packet->value11 = value11; + packet->value12 = value12; + packet->value13 = value13; + packet->value14 = value14; + packet->value15 = value15; + packet->value16 = value16; + packet->value17 = value17; + packet->value18 = value18; + packet->value19 = value19; + packet->value20 = value20; + packet->Index = Index; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AQ_TELEMETRY_F, (const char *)packet, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN, MAVLINK_MSG_ID_AQ_TELEMETRY_F_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AQ_TELEMETRY_F, (const char *)packet, MAVLINK_MSG_ID_AQ_TELEMETRY_F_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE AQ_TELEMETRY_F UNPACKING diff --git a/mavlink/include/mavlink/v1.0/autoquad/version.h b/mavlink/include/mavlink/v1.0/autoquad/version.h index d125e92b1a..5d2f33b601 100644 --- a/mavlink/include/mavlink/v1.0/autoquad/version.h +++ b/mavlink/include/mavlink/v1.0/autoquad/version.h @@ -5,7 +5,7 @@ #ifndef MAVLINK_VERSION_H #define MAVLINK_VERSION_H -#define MAVLINK_BUILD_DATE "Tue Feb 4 15:27:43 2014" +#define MAVLINK_BUILD_DATE "Sun Apr 13 09:38:19 2014" #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 diff --git a/mavlink/include/mavlink/v1.0/common/common.h b/mavlink/include/mavlink/v1.0/common/common.h index 7e1cf765bf..020211d406 100644 --- a/mavlink/include/mavlink/v1.0/common/common.h +++ b/mavlink/include/mavlink/v1.0/common/common.h @@ -12,15 +12,15 @@ extern "C" { // MESSAGE LENGTHS AND CRCS #ifndef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 0, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 22, 6, 14, 12, 97, 2, 2, 113, 35, 0, 0, 0, 0, 0, 13, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 24, 33, 25, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0} +#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 42, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 22, 6, 14, 12, 97, 2, 2, 113, 35, 6, 79, 0, 0, 0, 13, 255, 14, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 24, 33, 25, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0} #endif #ifndef MAVLINK_MESSAGE_CRCS -#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 0, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 0, 0, 0, 0, 0, 29, 223, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0} +#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 118, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 203, 220, 0, 0, 0, 29, 223, 85, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0} #endif #ifndef MAVLINK_MESSAGE_INFO -#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} +#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_RC_CHANNELS, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, MAVLINK_MESSAGE_INFO_POWER_STATUS, MAVLINK_MESSAGE_INFO_SERIAL_CONTROL, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, MAVLINK_MESSAGE_INFO_DISTANCE_SENSOR, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} #endif #include "../protocol.h" @@ -79,7 +79,8 @@ enum MAV_TYPE MAV_TYPE_TRICOPTER=15, /* Octorotor | */ MAV_TYPE_FLAPPING_WING=16, /* Flapping wing | */ MAV_TYPE_KITE=17, /* Flapping wing | */ - MAV_TYPE_ENUM_END=18, /* | */ + MAV_TYPE_ONBOARD_CONTROLLER=18, /* Onboard companion controller | */ + MAV_TYPE_ENUM_END=19, /* | */ }; #endif @@ -228,7 +229,8 @@ enum MAV_SYS_STATUS_SENSOR MAV_SYS_STATUS_SENSOR_3D_GYRO2=131072, /* 0x20000 2nd 3D gyro | */ MAV_SYS_STATUS_SENSOR_3D_ACCEL2=262144, /* 0x40000 2nd 3D accelerometer | */ MAV_SYS_STATUS_SENSOR_3D_MAG2=524288, /* 0x80000 2nd 3D magnetometer | */ - MAV_SYS_STATUS_SENSOR_ENUM_END=524289, /* | */ + MAV_SYS_STATUS_GEOFENCE=1048576, /* 0x100000 geofence | */ + MAV_SYS_STATUS_SENSOR_ENUM_END=1048577, /* | */ }; #endif @@ -242,7 +244,9 @@ enum MAV_FRAME MAV_FRAME_MISSION=2, /* NOT a coordinate frame, indicates a mission command. | */ MAV_FRAME_GLOBAL_RELATIVE_ALT=3, /* Global coordinate frame, WGS84 coordinate system, relative altitude over ground with respect to the home position. First value / x: latitude, second value / y: longitude, third value / z: positive altitude with 0 being at the altitude of the home location. | */ MAV_FRAME_LOCAL_ENU=4, /* Local coordinate frame, Z-down (x: east, y: north, z: up) | */ - MAV_FRAME_ENUM_END=5, /* | */ + MAV_FRAME_GLOBAL_INT=5, /* Global coordinate frame with some fields as scaled integers, WGS84 coordinate system. First value / x: latitude, second value / y: longitude, third value / z: positive altitude over mean sea level (MSL). Lat / Lon are scaled * 1E7 to avoid floating point accuracy limitations. | */ + MAV_FRAME_GLOBAL_RELATIVE_ALT_INT=6, /* Global coordinate frame with some fields as scaled integers, WGS84 coordinate system, relative altitude over ground with respect to the home position. First value / x: latitude, second value / y: longitude, third value / z: positive altitude with 0 being at the altitude of the home location. Lat / Lon are scaled * 1E7 to avoid floating point accuracy limitations. | */ + MAV_FRAME_ENUM_END=7, /* | */ }; #endif @@ -261,6 +265,57 @@ enum MAVLINK_DATA_STREAM_TYPE }; #endif +/** @brief */ +#ifndef HAVE_ENUM_FENCE_ACTION +#define HAVE_ENUM_FENCE_ACTION +enum FENCE_ACTION +{ + FENCE_ACTION_NONE=0, /* Disable fenced mode | */ + FENCE_ACTION_GUIDED=1, /* Switched to guided mode to return point (fence point 0) | */ + FENCE_ACTION_REPORT=2, /* Report fence breach, but don't take action | */ + FENCE_ACTION_GUIDED_THR_PASS=3, /* Switched to guided mode to return point (fence point 0) with manual throttle control | */ + FENCE_ACTION_ENUM_END=4, /* | */ +}; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_FENCE_BREACH +#define HAVE_ENUM_FENCE_BREACH +enum FENCE_BREACH +{ + FENCE_BREACH_NONE=0, /* No last fence breach | */ + FENCE_BREACH_MINALT=1, /* Breached minimum altitude | */ + FENCE_BREACH_MAXALT=2, /* Breached maximum altitude | */ + FENCE_BREACH_BOUNDARY=3, /* Breached fence boundary | */ + FENCE_BREACH_ENUM_END=4, /* | */ +}; +#endif + +/** @brief Enumeration of possible mount operation modes */ +#ifndef HAVE_ENUM_MAV_MOUNT_MODE +#define HAVE_ENUM_MAV_MOUNT_MODE +enum MAV_MOUNT_MODE +{ + MAV_MOUNT_MODE_RETRACT=0, /* Load and keep safe position (Roll,Pitch,Yaw) from permant memory and stop stabilization | */ + MAV_MOUNT_MODE_NEUTRAL=1, /* Load and keep neutral position (Roll,Pitch,Yaw) from permanent memory. | */ + MAV_MOUNT_MODE_MAVLINK_TARGETING=2, /* Load neutral position and start MAVLink Roll,Pitch,Yaw control with stabilization | */ + MAV_MOUNT_MODE_RC_TARGETING=3, /* Load neutral position and start RC Roll,Pitch,Yaw control with stabilization | */ + MAV_MOUNT_MODE_GPS_POINT=4, /* Load neutral position and start to point to Lat,Lon,Alt | */ + MAV_MOUNT_MODE_ENUM_END=5, /* | */ +}; +#endif + +/** @brief Enumeration of distance sensor types */ +#ifndef HAVE_ENUM_MAV_DISTANCE_SENSOR +#define HAVE_ENUM_MAV_DISTANCE_SENSOR +enum MAV_DISTANCE_SENSOR +{ + MAV_DISTANCE_SENSOR_LASER=0, /* Laser altimeter, e.g. LightWare SF02/F or PulsedLight units | */ + MAV_DISTANCE_SENSOR_ULTRASOUND=1, /* Laser altimeter, e.g. MaxBotix units | */ + MAV_DISTANCE_SENSOR_ENUM_END=2, /* | */ +}; +#endif + /** @brief Commands to be executed by the MAV. They can be executed on user request, or as part of a mission script. If the action is used in a mission, the parameter mapping to the waypoint/mission message is as follows: Param 1, Param 2, Param 3, Param 4, X: Param 5, Y:Param 6, Z:Param 7. This command list is similar what ARINC 424 is for commercial aircraft: A data format how to interpret waypoint/mission data. */ #ifndef HAVE_ENUM_MAV_CMD #define HAVE_ENUM_MAV_CMD @@ -275,6 +330,7 @@ enum MAV_CMD MAV_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| */ MAV_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| */ MAV_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| */ + MAV_CMD_NAV_SPLINE_WAYPOINT=82, /* Navigate to MISSION using a spline path. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Empty| Empty| Empty| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */ MAV_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| */ MAV_CMD_CONDITION_DELAY=112, /* Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty| */ MAV_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| */ @@ -292,8 +348,16 @@ enum MAV_CMD MAV_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| */ MAV_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ MAV_CMD_DO_SET_ROI=201, /* 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| */ + MAV_CMD_DO_DIGICAM_CONFIGURE=202, /* Mission command to configure an on-board camera controller system. |Modes: P, TV, AV, M, Etc| Shutter speed: Divisor number for one second| Aperture: F stop number| ISO number e.g. 80, 100, 200, Etc| Exposure type enumerator| Command Identity| Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)| */ + MAV_CMD_DO_DIGICAM_CONTROL=203, /* Mission command to control an on-board camera controller system. |Session control e.g. show/hide lens| Zoom's absolute position| Zooming step value to offset zoom from the current position| Focus Locking, Unlocking or Re-locking| Shooting Command| Command Identity| Empty| */ + MAV_CMD_DO_MOUNT_CONFIGURE=204, /* Mission command to configure a camera or antenna mount |Mount operation mode (see MAV_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty| */ + MAV_CMD_DO_MOUNT_CONTROL=205, /* Mission command to control a camera or antenna mount |pitch or lat in degrees, depending on mount mode.| roll or lon in degrees depending on mount mode| yaw or alt (in meters) depending on mount mode| reserved| reserved| reserved| MAV_MOUNT_MODE enum value| */ + MAV_CMD_DO_SET_CAM_TRIGG_DIST=206, /* Mission command to set CAM_TRIGG_DIST for this flight |Camera trigger distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_FENCE_ENABLE=207, /* Mission command to enable the geofence |enable? (0=disable, 1=enable)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_PARACHUTE=208, /* Mission command to trigger a parachute |action (0=disable, 1=enable, 2=release, for some systems see PARACHUTE_ACTION enum, not in general message set.)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_MOUNT_CONTROL_QUAT=220, /* Mission command to control a camera or antenna mount, using a quaternion as reference. |q1 - quaternion param #1| q2 - quaternion param #2| q3 - quaternion param #3| q4 - quaternion param #4| Empty| Empty| Empty| */ MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty| */ + MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Compass/Motor interference calibration: 0: no, 1: yes| Empty| */ MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS=242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| */ MAV_CMD_PREFLIGHT_STORAGE=245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty| */ MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN=246, /* Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty| */ @@ -433,6 +497,48 @@ enum MAV_SEVERITY }; #endif +/** @brief Power supply status flags (bitmask) */ +#ifndef HAVE_ENUM_MAV_POWER_STATUS +#define HAVE_ENUM_MAV_POWER_STATUS +enum MAV_POWER_STATUS +{ + MAV_POWER_STATUS_BRICK_VALID=1, /* main brick power supply valid | */ + MAV_POWER_STATUS_SERVO_VALID=2, /* main servo power supply valid for FMU | */ + MAV_POWER_STATUS_USB_CONNECTED=4, /* USB power is connected | */ + MAV_POWER_STATUS_PERIPH_OVERCURRENT=8, /* peripheral supply is in over-current state | */ + MAV_POWER_STATUS_PERIPH_HIPOWER_OVERCURRENT=16, /* hi-power peripheral supply is in over-current state | */ + MAV_POWER_STATUS_CHANGED=32, /* Power status has changed since boot | */ + MAV_POWER_STATUS_ENUM_END=33, /* | */ +}; +#endif + +/** @brief SERIAL_CONTROL device types */ +#ifndef HAVE_ENUM_SERIAL_CONTROL_DEV +#define HAVE_ENUM_SERIAL_CONTROL_DEV +enum SERIAL_CONTROL_DEV +{ + SERIAL_CONTROL_DEV_TELEM1=0, /* First telemetry port | */ + SERIAL_CONTROL_DEV_TELEM2=1, /* Second telemetry port | */ + SERIAL_CONTROL_DEV_GPS1=2, /* First GPS port | */ + SERIAL_CONTROL_DEV_GPS2=3, /* Second GPS port | */ + SERIAL_CONTROL_DEV_ENUM_END=4, /* | */ +}; +#endif + +/** @brief SERIAL_CONTROL flags (bitmask) */ +#ifndef HAVE_ENUM_SERIAL_CONTROL_FLAG +#define HAVE_ENUM_SERIAL_CONTROL_FLAG +enum SERIAL_CONTROL_FLAG +{ + SERIAL_CONTROL_FLAG_REPLY=1, /* Set if this is a reply | */ + SERIAL_CONTROL_FLAG_RESPOND=2, /* Set if the sender wants the receiver to send a response as another SERIAL_CONTROL message | */ + SERIAL_CONTROL_FLAG_EXCLUSIVE=4, /* Set if access to the serial port should be removed from whatever driver is currently using it, giving exclusive access to the SERIAL_CONTROL protocol. The port can be handed back by sending a request without this flag set | */ + SERIAL_CONTROL_FLAG_BLOCKING=8, /* Block on writes to the serial port | */ + SERIAL_CONTROL_FLAG_MULTI=16, /* Send multiple replies until port is drained | */ + SERIAL_CONTROL_FLAG_ENUM_END=17, /* | */ +}; +#endif + // MAVLINK VERSION @@ -500,6 +606,7 @@ enum MAV_SEVERITY #include "./mavlink_msg_nav_controller_output.h" #include "./mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust.h" #include "./mavlink_msg_state_correction.h" +#include "./mavlink_msg_rc_channels.h" #include "./mavlink_msg_request_data_stream.h" #include "./mavlink_msg_data_stream.h" #include "./mavlink_msg_manual_control.h" @@ -538,8 +645,11 @@ enum MAV_SEVERITY #include "./mavlink_msg_log_request_end.h" #include "./mavlink_msg_gps_inject_data.h" #include "./mavlink_msg_gps2_raw.h" +#include "./mavlink_msg_power_status.h" +#include "./mavlink_msg_serial_control.h" #include "./mavlink_msg_data_transmission_handshake.h" #include "./mavlink_msg_encapsulated_data.h" +#include "./mavlink_msg_distance_sensor.h" #include "./mavlink_msg_battery_status.h" #include "./mavlink_msg_setpoint_8dof.h" #include "./mavlink_msg_setpoint_6dof.h" diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude.h index 8ddf5bf099..ff2f104d40 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude.h @@ -212,6 +212,50 @@ static inline void mavlink_msg_attitude_send(mavlink_channel_t chan, uint32_t ti #endif } +#if MAVLINK_MSG_ID_ATTITUDE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_attitude_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, roll); + _mav_put_float(buf, 8, pitch); + _mav_put_float(buf, 12, yaw); + _mav_put_float(buf, 16, rollspeed); + _mav_put_float(buf, 20, pitchspeed); + _mav_put_float(buf, 24, yawspeed); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE, buf, MAVLINK_MSG_ID_ATTITUDE_LEN, MAVLINK_MSG_ID_ATTITUDE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE, buf, MAVLINK_MSG_ID_ATTITUDE_LEN); +#endif +#else + mavlink_attitude_t *packet = (mavlink_attitude_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->roll = roll; + packet->pitch = pitch; + packet->yaw = yaw; + packet->rollspeed = rollspeed; + packet->pitchspeed = pitchspeed; + packet->yawspeed = yawspeed; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE, (const char *)packet, MAVLINK_MSG_ID_ATTITUDE_LEN, MAVLINK_MSG_ID_ATTITUDE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE, (const char *)packet, MAVLINK_MSG_ID_ATTITUDE_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE ATTITUDE UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude_quaternion.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude_quaternion.h index 9f8d587598..3b97b40d0d 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude_quaternion.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_attitude_quaternion.h @@ -223,6 +223,52 @@ static inline void mavlink_msg_attitude_quaternion_send(mavlink_channel_t chan, #endif } +#if MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_attitude_quaternion_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, float q1, float q2, float q3, float q4, float rollspeed, float pitchspeed, float yawspeed) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, q1); + _mav_put_float(buf, 8, q2); + _mav_put_float(buf, 12, q3); + _mav_put_float(buf, 16, q4); + _mav_put_float(buf, 20, rollspeed); + _mav_put_float(buf, 24, pitchspeed); + _mav_put_float(buf, 28, yawspeed); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION, buf, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION, buf, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN); +#endif +#else + mavlink_attitude_quaternion_t *packet = (mavlink_attitude_quaternion_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->q1 = q1; + packet->q2 = q2; + packet->q3 = q3; + packet->q4 = q4; + packet->rollspeed = rollspeed; + packet->pitchspeed = pitchspeed; + packet->yawspeed = yawspeed; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION, (const char *)packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION, (const char *)packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE ATTITUDE_QUATERNION UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_auth_key.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_auth_key.h index 5703a59871..f31b6bbf48 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_auth_key.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_auth_key.h @@ -146,6 +146,38 @@ static inline void mavlink_msg_auth_key_send(mavlink_channel_t chan, const char #endif } +#if MAVLINK_MSG_ID_AUTH_KEY_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_auth_key_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, const char *key) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + + _mav_put_char_array(buf, 0, key, 32); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTH_KEY, buf, MAVLINK_MSG_ID_AUTH_KEY_LEN, MAVLINK_MSG_ID_AUTH_KEY_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTH_KEY, buf, MAVLINK_MSG_ID_AUTH_KEY_LEN); +#endif +#else + mavlink_auth_key_t *packet = (mavlink_auth_key_t *)msgbuf; + + mav_array_memcpy(packet->key, key, sizeof(char)*32); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTH_KEY, (const char *)packet, MAVLINK_MSG_ID_AUTH_KEY_LEN, MAVLINK_MSG_ID_AUTH_KEY_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTH_KEY, (const char *)packet, MAVLINK_MSG_ID_AUTH_KEY_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE AUTH_KEY UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_battery_status.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_battery_status.h index 03e4d569e6..faaabf9f70 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_battery_status.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_battery_status.h @@ -256,6 +256,58 @@ static inline void mavlink_msg_battery_status_send(mavlink_channel_t chan, uint8 #endif } +#if MAVLINK_MSG_ID_BATTERY_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_battery_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t accu_id, uint16_t voltage_cell_1, uint16_t voltage_cell_2, uint16_t voltage_cell_3, uint16_t voltage_cell_4, uint16_t voltage_cell_5, uint16_t voltage_cell_6, int16_t current_battery, int32_t current_consumed, int32_t energy_consumed, int8_t battery_remaining) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int32_t(buf, 0, current_consumed); + _mav_put_int32_t(buf, 4, energy_consumed); + _mav_put_uint16_t(buf, 8, voltage_cell_1); + _mav_put_uint16_t(buf, 10, voltage_cell_2); + _mav_put_uint16_t(buf, 12, voltage_cell_3); + _mav_put_uint16_t(buf, 14, voltage_cell_4); + _mav_put_uint16_t(buf, 16, voltage_cell_5); + _mav_put_uint16_t(buf, 18, voltage_cell_6); + _mav_put_int16_t(buf, 20, current_battery); + _mav_put_uint8_t(buf, 22, accu_id); + _mav_put_int8_t(buf, 23, battery_remaining); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY_STATUS, buf, MAVLINK_MSG_ID_BATTERY_STATUS_LEN, MAVLINK_MSG_ID_BATTERY_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY_STATUS, buf, MAVLINK_MSG_ID_BATTERY_STATUS_LEN); +#endif +#else + mavlink_battery_status_t *packet = (mavlink_battery_status_t *)msgbuf; + packet->current_consumed = current_consumed; + packet->energy_consumed = energy_consumed; + packet->voltage_cell_1 = voltage_cell_1; + packet->voltage_cell_2 = voltage_cell_2; + packet->voltage_cell_3 = voltage_cell_3; + packet->voltage_cell_4 = voltage_cell_4; + packet->voltage_cell_5 = voltage_cell_5; + packet->voltage_cell_6 = voltage_cell_6; + packet->current_battery = current_battery; + packet->accu_id = accu_id; + packet->battery_remaining = battery_remaining; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY_STATUS, (const char *)packet, MAVLINK_MSG_ID_BATTERY_STATUS_LEN, MAVLINK_MSG_ID_BATTERY_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY_STATUS, (const char *)packet, MAVLINK_MSG_ID_BATTERY_STATUS_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE BATTERY_STATUS UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control.h index 0b6de930d2..3f0987f10a 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control.h @@ -173,6 +173,42 @@ static inline void mavlink_msg_change_operator_control_send(mavlink_channel_t ch #endif } +#if MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_change_operator_control_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t control_request, uint8_t version, const char *passkey) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, control_request); + _mav_put_uint8_t(buf, 2, version); + _mav_put_char_array(buf, 3, passkey, 25); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL, buf, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL, buf, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN); +#endif +#else + mavlink_change_operator_control_t *packet = (mavlink_change_operator_control_t *)msgbuf; + packet->target_system = target_system; + packet->control_request = control_request; + packet->version = version; + mav_array_memcpy(packet->passkey, passkey, sizeof(char)*25); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL, (const char *)packet, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL, (const char *)packet, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE CHANGE_OPERATOR_CONTROL UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control_ack.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control_ack.h index c6f6a28e4b..768e7ed5ca 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control_ack.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_change_operator_control_ack.h @@ -168,6 +168,42 @@ static inline void mavlink_msg_change_operator_control_ack_send(mavlink_channel_ #endif } +#if MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_change_operator_control_ack_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t gcs_system_id, uint8_t control_request, uint8_t ack) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, gcs_system_id); + _mav_put_uint8_t(buf, 1, control_request); + _mav_put_uint8_t(buf, 2, ack); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK, buf, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK, buf, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN); +#endif +#else + mavlink_change_operator_control_ack_t *packet = (mavlink_change_operator_control_ack_t *)msgbuf; + packet->gcs_system_id = gcs_system_id; + packet->control_request = control_request; + packet->ack = ack; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK, (const char *)packet, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK, (const char *)packet, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE CHANGE_OPERATOR_CONTROL_ACK UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_ack.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_ack.h index dca2fe6819..d3d1630419 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_ack.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_ack.h @@ -157,6 +157,40 @@ static inline void mavlink_msg_command_ack_send(mavlink_channel_t chan, uint16_t #endif } +#if MAVLINK_MSG_ID_COMMAND_ACK_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_command_ack_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t command, uint8_t result) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, command); + _mav_put_uint8_t(buf, 2, result); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_ACK, buf, MAVLINK_MSG_ID_COMMAND_ACK_LEN, MAVLINK_MSG_ID_COMMAND_ACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_ACK, buf, MAVLINK_MSG_ID_COMMAND_ACK_LEN); +#endif +#else + mavlink_command_ack_t *packet = (mavlink_command_ack_t *)msgbuf; + packet->command = command; + packet->result = result; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_ACK, (const char *)packet, MAVLINK_MSG_ID_COMMAND_ACK_LEN, MAVLINK_MSG_ID_COMMAND_ACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_ACK, (const char *)packet, MAVLINK_MSG_ID_COMMAND_ACK_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE COMMAND_ACK UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_long.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_long.h index 8f705c0dd2..161896b973 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_long.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_command_long.h @@ -256,6 +256,58 @@ static inline void mavlink_msg_command_long_send(mavlink_channel_t chan, uint8_t #endif } +#if MAVLINK_MSG_ID_COMMAND_LONG_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_command_long_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t command, uint8_t confirmation, float param1, float param2, float param3, float param4, float param5, float param6, float param7) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, param1); + _mav_put_float(buf, 4, param2); + _mav_put_float(buf, 8, param3); + _mav_put_float(buf, 12, param4); + _mav_put_float(buf, 16, param5); + _mav_put_float(buf, 20, param6); + _mav_put_float(buf, 24, param7); + _mav_put_uint16_t(buf, 28, command); + _mav_put_uint8_t(buf, 30, target_system); + _mav_put_uint8_t(buf, 31, target_component); + _mav_put_uint8_t(buf, 32, confirmation); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_LONG, buf, MAVLINK_MSG_ID_COMMAND_LONG_LEN, MAVLINK_MSG_ID_COMMAND_LONG_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_LONG, buf, MAVLINK_MSG_ID_COMMAND_LONG_LEN); +#endif +#else + mavlink_command_long_t *packet = (mavlink_command_long_t *)msgbuf; + packet->param1 = param1; + packet->param2 = param2; + packet->param3 = param3; + packet->param4 = param4; + packet->param5 = param5; + packet->param6 = param6; + packet->param7 = param7; + packet->command = command; + packet->target_system = target_system; + packet->target_component = target_component; + packet->confirmation = confirmation; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_LONG, (const char *)packet, MAVLINK_MSG_ID_COMMAND_LONG_LEN, MAVLINK_MSG_ID_COMMAND_LONG_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_LONG, (const char *)packet, MAVLINK_MSG_ID_COMMAND_LONG_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE COMMAND_LONG UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_data_stream.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_data_stream.h index dc0768e128..640ffebf44 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_data_stream.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_data_stream.h @@ -168,6 +168,42 @@ static inline void mavlink_msg_data_stream_send(mavlink_channel_t chan, uint8_t #endif } +#if MAVLINK_MSG_ID_DATA_STREAM_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_data_stream_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t stream_id, uint16_t message_rate, uint8_t on_off) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, message_rate); + _mav_put_uint8_t(buf, 2, stream_id); + _mav_put_uint8_t(buf, 3, on_off); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_STREAM, buf, MAVLINK_MSG_ID_DATA_STREAM_LEN, MAVLINK_MSG_ID_DATA_STREAM_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_STREAM, buf, MAVLINK_MSG_ID_DATA_STREAM_LEN); +#endif +#else + mavlink_data_stream_t *packet = (mavlink_data_stream_t *)msgbuf; + packet->message_rate = message_rate; + packet->stream_id = stream_id; + packet->on_off = on_off; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_STREAM, (const char *)packet, MAVLINK_MSG_ID_DATA_STREAM_LEN, MAVLINK_MSG_ID_DATA_STREAM_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_STREAM, (const char *)packet, MAVLINK_MSG_ID_DATA_STREAM_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE DATA_STREAM UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_data_transmission_handshake.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_data_transmission_handshake.h index fdd8fddd87..d84a73709d 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_data_transmission_handshake.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_data_transmission_handshake.h @@ -212,6 +212,50 @@ static inline void mavlink_msg_data_transmission_handshake_send(mavlink_channel_ #endif } +#if MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_data_transmission_handshake_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t type, uint32_t size, uint16_t width, uint16_t height, uint16_t packets, uint8_t payload, uint8_t jpg_quality) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, size); + _mav_put_uint16_t(buf, 4, width); + _mav_put_uint16_t(buf, 6, height); + _mav_put_uint16_t(buf, 8, packets); + _mav_put_uint8_t(buf, 10, type); + _mav_put_uint8_t(buf, 11, payload); + _mav_put_uint8_t(buf, 12, jpg_quality); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE, buf, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE, buf, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN); +#endif +#else + mavlink_data_transmission_handshake_t *packet = (mavlink_data_transmission_handshake_t *)msgbuf; + packet->size = size; + packet->width = width; + packet->height = height; + packet->packets = packets; + packet->type = type; + packet->payload = payload; + packet->jpg_quality = jpg_quality; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE, (const char *)packet, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE, (const char *)packet, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE DATA_TRANSMISSION_HANDSHAKE UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug.h index 9a6ed87eeb..2102af8623 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug.h @@ -168,6 +168,42 @@ static inline void mavlink_msg_debug_send(mavlink_channel_t chan, uint32_t time_ #endif } +#if MAVLINK_MSG_ID_DEBUG_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_debug_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t ind, float value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, value); + _mav_put_uint8_t(buf, 8, ind); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG, buf, MAVLINK_MSG_ID_DEBUG_LEN, MAVLINK_MSG_ID_DEBUG_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG, buf, MAVLINK_MSG_ID_DEBUG_LEN); +#endif +#else + mavlink_debug_t *packet = (mavlink_debug_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->value = value; + packet->ind = ind; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG, (const char *)packet, MAVLINK_MSG_ID_DEBUG_LEN, MAVLINK_MSG_ID_DEBUG_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG, (const char *)packet, MAVLINK_MSG_ID_DEBUG_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE DEBUG UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug_vect.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug_vect.h index 6cfc75212e..67c339e898 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug_vect.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_debug_vect.h @@ -184,6 +184,44 @@ static inline void mavlink_msg_debug_vect_send(mavlink_channel_t chan, const cha #endif } +#if MAVLINK_MSG_ID_DEBUG_VECT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_debug_vect_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, const char *name, uint64_t time_usec, float x, float y, float z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + _mav_put_char_array(buf, 20, name, 10); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG_VECT, buf, MAVLINK_MSG_ID_DEBUG_VECT_LEN, MAVLINK_MSG_ID_DEBUG_VECT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG_VECT, buf, MAVLINK_MSG_ID_DEBUG_VECT_LEN); +#endif +#else + mavlink_debug_vect_t *packet = (mavlink_debug_vect_t *)msgbuf; + packet->time_usec = time_usec; + packet->x = x; + packet->y = y; + packet->z = z; + mav_array_memcpy(packet->name, name, sizeof(char)*10); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG_VECT, (const char *)packet, MAVLINK_MSG_ID_DEBUG_VECT_LEN, MAVLINK_MSG_ID_DEBUG_VECT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG_VECT, (const char *)packet, MAVLINK_MSG_ID_DEBUG_VECT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE DEBUG_VECT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_distance_sensor.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_distance_sensor.h new file mode 100644 index 0000000000..39b3843966 --- /dev/null +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_distance_sensor.h @@ -0,0 +1,377 @@ +// MESSAGE DISTANCE_SENSOR PACKING + +#define MAVLINK_MSG_ID_DISTANCE_SENSOR 132 + +typedef struct __mavlink_distance_sensor_t +{ + uint32_t time_boot_ms; ///< Time since system boot + uint16_t min_distance; ///< Minimum distance the sensor can measure in centimeters + uint16_t max_distance; ///< Maximum distance the sensor can measure in centimeters + uint16_t current_distance; ///< Current distance reading + uint8_t type; ///< Type from MAV_DISTANCE_SENSOR enum. + uint8_t id; ///< Onboard ID of the sensor + uint8_t orientation; ///< Direction the sensor faces from FIXME enum. + uint8_t covariance; ///< Measurement covariance in centimeters, 0 for unknown / invalid readings +} mavlink_distance_sensor_t; + +#define MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN 14 +#define MAVLINK_MSG_ID_132_LEN 14 + +#define MAVLINK_MSG_ID_DISTANCE_SENSOR_CRC 85 +#define MAVLINK_MSG_ID_132_CRC 85 + + + +#define MAVLINK_MESSAGE_INFO_DISTANCE_SENSOR { \ + "DISTANCE_SENSOR", \ + 8, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_distance_sensor_t, time_boot_ms) }, \ + { "min_distance", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_distance_sensor_t, min_distance) }, \ + { "max_distance", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_distance_sensor_t, max_distance) }, \ + { "current_distance", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_distance_sensor_t, current_distance) }, \ + { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_distance_sensor_t, type) }, \ + { "id", NULL, MAVLINK_TYPE_UINT8_T, 0, 11, offsetof(mavlink_distance_sensor_t, id) }, \ + { "orientation", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_distance_sensor_t, orientation) }, \ + { "covariance", NULL, MAVLINK_TYPE_UINT8_T, 0, 13, offsetof(mavlink_distance_sensor_t, covariance) }, \ + } \ +} + + +/** + * @brief Pack a distance_sensor message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms Time since system boot + * @param type Type from MAV_DISTANCE_SENSOR enum. + * @param id Onboard ID of the sensor + * @param orientation Direction the sensor faces from FIXME enum. + * @param min_distance Minimum distance the sensor can measure in centimeters + * @param max_distance Maximum distance the sensor can measure in centimeters + * @param current_distance Current distance reading + * @param covariance Measurement covariance in centimeters, 0 for unknown / invalid readings + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_distance_sensor_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, uint8_t type, uint8_t id, uint8_t orientation, uint16_t min_distance, uint16_t max_distance, uint16_t current_distance, uint8_t covariance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint16_t(buf, 4, min_distance); + _mav_put_uint16_t(buf, 6, max_distance); + _mav_put_uint16_t(buf, 8, current_distance); + _mav_put_uint8_t(buf, 10, type); + _mav_put_uint8_t(buf, 11, id); + _mav_put_uint8_t(buf, 12, orientation); + _mav_put_uint8_t(buf, 13, covariance); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); +#else + mavlink_distance_sensor_t packet; + packet.time_boot_ms = time_boot_ms; + packet.min_distance = min_distance; + packet.max_distance = max_distance; + packet.current_distance = current_distance; + packet.type = type; + packet.id = id; + packet.orientation = orientation; + packet.covariance = covariance; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_DISTANCE_SENSOR; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); +#endif +} + +/** + * @brief Pack a distance_sensor message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms Time since system boot + * @param type Type from MAV_DISTANCE_SENSOR enum. + * @param id Onboard ID of the sensor + * @param orientation Direction the sensor faces from FIXME enum. + * @param min_distance Minimum distance the sensor can measure in centimeters + * @param max_distance Maximum distance the sensor can measure in centimeters + * @param current_distance Current distance reading + * @param covariance Measurement covariance in centimeters, 0 for unknown / invalid readings + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_distance_sensor_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,uint8_t type,uint8_t id,uint8_t orientation,uint16_t min_distance,uint16_t max_distance,uint16_t current_distance,uint8_t covariance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint16_t(buf, 4, min_distance); + _mav_put_uint16_t(buf, 6, max_distance); + _mav_put_uint16_t(buf, 8, current_distance); + _mav_put_uint8_t(buf, 10, type); + _mav_put_uint8_t(buf, 11, id); + _mav_put_uint8_t(buf, 12, orientation); + _mav_put_uint8_t(buf, 13, covariance); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); +#else + mavlink_distance_sensor_t packet; + packet.time_boot_ms = time_boot_ms; + packet.min_distance = min_distance; + packet.max_distance = max_distance; + packet.current_distance = current_distance; + packet.type = type; + packet.id = id; + packet.orientation = orientation; + packet.covariance = covariance; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_DISTANCE_SENSOR; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); +#endif +} + +/** + * @brief Encode a distance_sensor struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param distance_sensor C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_distance_sensor_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_distance_sensor_t* distance_sensor) +{ + return mavlink_msg_distance_sensor_pack(system_id, component_id, msg, distance_sensor->time_boot_ms, distance_sensor->type, distance_sensor->id, distance_sensor->orientation, distance_sensor->min_distance, distance_sensor->max_distance, distance_sensor->current_distance, distance_sensor->covariance); +} + +/** + * @brief Encode a distance_sensor struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param distance_sensor C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_distance_sensor_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_distance_sensor_t* distance_sensor) +{ + return mavlink_msg_distance_sensor_pack_chan(system_id, component_id, chan, msg, distance_sensor->time_boot_ms, distance_sensor->type, distance_sensor->id, distance_sensor->orientation, distance_sensor->min_distance, distance_sensor->max_distance, distance_sensor->current_distance, distance_sensor->covariance); +} + +/** + * @brief Send a distance_sensor message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms Time since system boot + * @param type Type from MAV_DISTANCE_SENSOR enum. + * @param id Onboard ID of the sensor + * @param orientation Direction the sensor faces from FIXME enum. + * @param min_distance Minimum distance the sensor can measure in centimeters + * @param max_distance Maximum distance the sensor can measure in centimeters + * @param current_distance Current distance reading + * @param covariance Measurement covariance in centimeters, 0 for unknown / invalid readings + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_distance_sensor_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t type, uint8_t id, uint8_t orientation, uint16_t min_distance, uint16_t max_distance, uint16_t current_distance, uint8_t covariance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint16_t(buf, 4, min_distance); + _mav_put_uint16_t(buf, 6, max_distance); + _mav_put_uint16_t(buf, 8, current_distance); + _mav_put_uint8_t(buf, 10, type); + _mav_put_uint8_t(buf, 11, id); + _mav_put_uint8_t(buf, 12, orientation); + _mav_put_uint8_t(buf, 13, covariance); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, buf, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, buf, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); +#endif +#else + mavlink_distance_sensor_t packet; + packet.time_boot_ms = time_boot_ms; + packet.min_distance = min_distance; + packet.max_distance = max_distance; + packet.current_distance = current_distance; + packet.type = type; + packet.id = id; + packet.orientation = orientation; + packet.covariance = covariance; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, (const char *)&packet, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, (const char *)&packet, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_distance_sensor_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t type, uint8_t id, uint8_t orientation, uint16_t min_distance, uint16_t max_distance, uint16_t current_distance, uint8_t covariance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint16_t(buf, 4, min_distance); + _mav_put_uint16_t(buf, 6, max_distance); + _mav_put_uint16_t(buf, 8, current_distance); + _mav_put_uint8_t(buf, 10, type); + _mav_put_uint8_t(buf, 11, id); + _mav_put_uint8_t(buf, 12, orientation); + _mav_put_uint8_t(buf, 13, covariance); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, buf, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, buf, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); +#endif +#else + mavlink_distance_sensor_t *packet = (mavlink_distance_sensor_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->min_distance = min_distance; + packet->max_distance = max_distance; + packet->current_distance = current_distance; + packet->type = type; + packet->id = id; + packet->orientation = orientation; + packet->covariance = covariance; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, (const char *)packet, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, (const char *)packet, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE DISTANCE_SENSOR UNPACKING + + +/** + * @brief Get field time_boot_ms from distance_sensor message + * + * @return Time since system boot + */ +static inline uint32_t mavlink_msg_distance_sensor_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field type from distance_sensor message + * + * @return Type from MAV_DISTANCE_SENSOR enum. + */ +static inline uint8_t mavlink_msg_distance_sensor_get_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 10); +} + +/** + * @brief Get field id from distance_sensor message + * + * @return Onboard ID of the sensor + */ +static inline uint8_t mavlink_msg_distance_sensor_get_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 11); +} + +/** + * @brief Get field orientation from distance_sensor message + * + * @return Direction the sensor faces from FIXME enum. + */ +static inline uint8_t mavlink_msg_distance_sensor_get_orientation(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 12); +} + +/** + * @brief Get field min_distance from distance_sensor message + * + * @return Minimum distance the sensor can measure in centimeters + */ +static inline uint16_t mavlink_msg_distance_sensor_get_min_distance(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 4); +} + +/** + * @brief Get field max_distance from distance_sensor message + * + * @return Maximum distance the sensor can measure in centimeters + */ +static inline uint16_t mavlink_msg_distance_sensor_get_max_distance(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 6); +} + +/** + * @brief Get field current_distance from distance_sensor message + * + * @return Current distance reading + */ +static inline uint16_t mavlink_msg_distance_sensor_get_current_distance(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 8); +} + +/** + * @brief Get field covariance from distance_sensor message + * + * @return Measurement covariance in centimeters, 0 for unknown / invalid readings + */ +static inline uint8_t mavlink_msg_distance_sensor_get_covariance(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 13); +} + +/** + * @brief Decode a distance_sensor message into a struct + * + * @param msg The message to decode + * @param distance_sensor C-struct to decode the message contents into + */ +static inline void mavlink_msg_distance_sensor_decode(const mavlink_message_t* msg, mavlink_distance_sensor_t* distance_sensor) +{ +#if MAVLINK_NEED_BYTE_SWAP + distance_sensor->time_boot_ms = mavlink_msg_distance_sensor_get_time_boot_ms(msg); + distance_sensor->min_distance = mavlink_msg_distance_sensor_get_min_distance(msg); + distance_sensor->max_distance = mavlink_msg_distance_sensor_get_max_distance(msg); + distance_sensor->current_distance = mavlink_msg_distance_sensor_get_current_distance(msg); + distance_sensor->type = mavlink_msg_distance_sensor_get_type(msg); + distance_sensor->id = mavlink_msg_distance_sensor_get_id(msg); + distance_sensor->orientation = mavlink_msg_distance_sensor_get_orientation(msg); + distance_sensor->covariance = mavlink_msg_distance_sensor_get_covariance(msg); +#else + memcpy(distance_sensor, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_encapsulated_data.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_encapsulated_data.h index 5900ea8388..dacd7c9f40 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_encapsulated_data.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_encapsulated_data.h @@ -151,6 +151,38 @@ static inline void mavlink_msg_encapsulated_data_send(mavlink_channel_t chan, ui #endif } +#if MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_encapsulated_data_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t seqnr, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, seqnr); + _mav_put_uint8_t_array(buf, 2, data, 253); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ENCAPSULATED_DATA, buf, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN, MAVLINK_MSG_ID_ENCAPSULATED_DATA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ENCAPSULATED_DATA, buf, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN); +#endif +#else + mavlink_encapsulated_data_t *packet = (mavlink_encapsulated_data_t *)msgbuf; + packet->seqnr = seqnr; + mav_array_memcpy(packet->data, data, sizeof(uint8_t)*253); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ENCAPSULATED_DATA, (const char *)packet, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN, MAVLINK_MSG_ID_ENCAPSULATED_DATA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ENCAPSULATED_DATA, (const char *)packet, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE ENCAPSULATED_DATA UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_dir_list.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_dir_list.h index 4f31698d50..81c7031ced 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_dir_list.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_dir_list.h @@ -162,6 +162,40 @@ static inline void mavlink_msg_file_transfer_dir_list_send(mavlink_channel_t cha #endif } +#if MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_file_transfer_dir_list_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t transfer_uid, const char *dir_path, uint8_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, transfer_uid); + _mav_put_uint8_t(buf, 248, flags); + _mav_put_char_array(buf, 8, dir_path, 240); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST, buf, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST, buf, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN); +#endif +#else + mavlink_file_transfer_dir_list_t *packet = (mavlink_file_transfer_dir_list_t *)msgbuf; + packet->transfer_uid = transfer_uid; + packet->flags = flags; + mav_array_memcpy(packet->dir_path, dir_path, sizeof(char)*240); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST, (const char *)packet, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST, (const char *)packet, MAVLINK_MSG_ID_FILE_TRANSFER_DIR_LIST_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE FILE_TRANSFER_DIR_LIST UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_res.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_res.h index fc6247faca..04981fa85a 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_res.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_res.h @@ -157,6 +157,40 @@ static inline void mavlink_msg_file_transfer_res_send(mavlink_channel_t chan, ui #endif } +#if MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_file_transfer_res_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t transfer_uid, uint8_t result) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, transfer_uid); + _mav_put_uint8_t(buf, 8, result); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_RES, buf, MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_RES_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_RES, buf, MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN); +#endif +#else + mavlink_file_transfer_res_t *packet = (mavlink_file_transfer_res_t *)msgbuf; + packet->transfer_uid = transfer_uid; + packet->result = result; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_RES, (const char *)packet, MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_RES_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_RES, (const char *)packet, MAVLINK_MSG_ID_FILE_TRANSFER_RES_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE FILE_TRANSFER_RES UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_start.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_start.h index 05be77339b..7b1fba519a 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_start.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_file_transfer_start.h @@ -184,6 +184,44 @@ static inline void mavlink_msg_file_transfer_start_send(mavlink_channel_t chan, #endif } +#if MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_file_transfer_start_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t transfer_uid, const char *dest_path, uint8_t direction, uint32_t file_size, uint8_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, transfer_uid); + _mav_put_uint32_t(buf, 8, file_size); + _mav_put_uint8_t(buf, 252, direction); + _mav_put_uint8_t(buf, 253, flags); + _mav_put_char_array(buf, 12, dest_path, 240); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_START, buf, MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_START_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_START, buf, MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN); +#endif +#else + mavlink_file_transfer_start_t *packet = (mavlink_file_transfer_start_t *)msgbuf; + packet->transfer_uid = transfer_uid; + packet->file_size = file_size; + packet->direction = direction; + packet->flags = flags; + mav_array_memcpy(packet->dest_path, dest_path, sizeof(char)*240); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_START, (const char *)packet, MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN, MAVLINK_MSG_ID_FILE_TRANSFER_START_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILE_TRANSFER_START, (const char *)packet, MAVLINK_MSG_ID_FILE_TRANSFER_START_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE FILE_TRANSFER_START UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_int.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_int.h index 7ed3d2a636..ba15bf3654 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_int.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_int.h @@ -234,6 +234,54 @@ static inline void mavlink_msg_global_position_int_send(mavlink_channel_t chan, #endif } +#if MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_global_position_int_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, int32_t lat, int32_t lon, int32_t alt, int32_t relative_alt, int16_t vx, int16_t vy, int16_t vz, uint16_t hdg) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int32_t(buf, 4, lat); + _mav_put_int32_t(buf, 8, lon); + _mav_put_int32_t(buf, 12, alt); + _mav_put_int32_t(buf, 16, relative_alt); + _mav_put_int16_t(buf, 20, vx); + _mav_put_int16_t(buf, 22, vy); + _mav_put_int16_t(buf, 24, vz); + _mav_put_uint16_t(buf, 26, hdg); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT, buf, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT, buf, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN); +#endif +#else + mavlink_global_position_int_t *packet = (mavlink_global_position_int_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->lat = lat; + packet->lon = lon; + packet->alt = alt; + packet->relative_alt = relative_alt; + packet->vx = vx; + packet->vy = vy; + packet->vz = vz; + packet->hdg = hdg; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT, (const char *)packet, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT, (const char *)packet, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE GLOBAL_POSITION_INT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_setpoint_int.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_setpoint_int.h index 1a1c97199e..77d735cf4f 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_setpoint_int.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_position_setpoint_int.h @@ -190,6 +190,46 @@ static inline void mavlink_msg_global_position_setpoint_int_send(mavlink_channel #endif } +#if MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_global_position_setpoint_int_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t coordinate_frame, int32_t latitude, int32_t longitude, int32_t altitude, int16_t yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int32_t(buf, 0, latitude); + _mav_put_int32_t(buf, 4, longitude); + _mav_put_int32_t(buf, 8, altitude); + _mav_put_int16_t(buf, 12, yaw); + _mav_put_uint8_t(buf, 14, coordinate_frame); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT, buf, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT, buf, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN); +#endif +#else + mavlink_global_position_setpoint_int_t *packet = (mavlink_global_position_setpoint_int_t *)msgbuf; + packet->latitude = latitude; + packet->longitude = longitude; + packet->altitude = altitude; + packet->yaw = yaw; + packet->coordinate_frame = coordinate_frame; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT, (const char *)packet, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT, (const char *)packet, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE GLOBAL_POSITION_SETPOINT_INT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_vision_position_estimate.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_vision_position_estimate.h index f7be74c917..b2b75d7ef5 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_vision_position_estimate.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_global_vision_position_estimate.h @@ -212,6 +212,50 @@ static inline void mavlink_msg_global_vision_position_estimate_send(mavlink_chan #endif } +#if MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_global_vision_position_estimate_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + _mav_put_float(buf, 20, roll); + _mav_put_float(buf, 24, pitch); + _mav_put_float(buf, 28, yaw); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN); +#endif +#else + mavlink_global_vision_position_estimate_t *packet = (mavlink_global_vision_position_estimate_t *)msgbuf; + packet->usec = usec; + packet->x = x; + packet->y = y; + packet->z = z; + packet->roll = roll; + packet->pitch = pitch; + packet->yaw = yaw; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE GLOBAL_VISION_POSITION_ESTIMATE UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps2_raw.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps2_raw.h index 17e5bd0026..b184e7c9cc 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps2_raw.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps2_raw.h @@ -267,6 +267,60 @@ static inline void mavlink_msg_gps2_raw_send(mavlink_channel_t chan, uint64_t ti #endif } +#if MAVLINK_MSG_ID_GPS2_RAW_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_gps2_raw_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible, uint8_t dgps_numch, uint32_t dgps_age) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lon); + _mav_put_int32_t(buf, 16, alt); + _mav_put_uint32_t(buf, 20, dgps_age); + _mav_put_uint16_t(buf, 24, eph); + _mav_put_uint16_t(buf, 26, epv); + _mav_put_uint16_t(buf, 28, vel); + _mav_put_uint16_t(buf, 30, cog); + _mav_put_uint8_t(buf, 32, fix_type); + _mav_put_uint8_t(buf, 33, satellites_visible); + _mav_put_uint8_t(buf, 34, dgps_numch); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, buf, MAVLINK_MSG_ID_GPS2_RAW_LEN, MAVLINK_MSG_ID_GPS2_RAW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, buf, MAVLINK_MSG_ID_GPS2_RAW_LEN); +#endif +#else + mavlink_gps2_raw_t *packet = (mavlink_gps2_raw_t *)msgbuf; + packet->time_usec = time_usec; + packet->lat = lat; + packet->lon = lon; + packet->alt = alt; + packet->dgps_age = dgps_age; + packet->eph = eph; + packet->epv = epv; + packet->vel = vel; + packet->cog = cog; + packet->fix_type = fix_type; + packet->satellites_visible = satellites_visible; + packet->dgps_numch = dgps_numch; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, (const char *)packet, MAVLINK_MSG_ID_GPS2_RAW_LEN, MAVLINK_MSG_ID_GPS2_RAW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, (const char *)packet, MAVLINK_MSG_ID_GPS2_RAW_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE GPS2_RAW UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_global_origin.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_global_origin.h index 016e9cb0e6..1589c8ca59 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_global_origin.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_global_origin.h @@ -168,6 +168,42 @@ static inline void mavlink_msg_gps_global_origin_send(mavlink_channel_t chan, in #endif } +#if MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_gps_global_origin_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, int32_t latitude, int32_t longitude, int32_t altitude) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int32_t(buf, 0, latitude); + _mav_put_int32_t(buf, 4, longitude); + _mav_put_int32_t(buf, 8, altitude); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, buf, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, buf, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN); +#endif +#else + mavlink_gps_global_origin_t *packet = (mavlink_gps_global_origin_t *)msgbuf; + packet->latitude = latitude; + packet->longitude = longitude; + packet->altitude = altitude; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, (const char *)packet, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, (const char *)packet, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE GPS_GLOBAL_ORIGIN UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_inject_data.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_inject_data.h index 485d8a4afc..362e2d7b9a 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_inject_data.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_inject_data.h @@ -173,6 +173,42 @@ static inline void mavlink_msg_gps_inject_data_send(mavlink_channel_t chan, uint #endif } +#if MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_gps_inject_data_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t len, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, len); + _mav_put_uint8_t_array(buf, 3, data, 110); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INJECT_DATA, buf, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN, MAVLINK_MSG_ID_GPS_INJECT_DATA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INJECT_DATA, buf, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN); +#endif +#else + mavlink_gps_inject_data_t *packet = (mavlink_gps_inject_data_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + packet->len = len; + mav_array_memcpy(packet->data, data, sizeof(uint8_t)*110); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INJECT_DATA, (const char *)packet, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN, MAVLINK_MSG_ID_GPS_INJECT_DATA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INJECT_DATA, (const char *)packet, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE GPS_INJECT_DATA UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_raw_int.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_raw_int.h index a105f8cdad..b539962069 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_raw_int.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_raw_int.h @@ -245,6 +245,56 @@ static inline void mavlink_msg_gps_raw_int_send(mavlink_channel_t chan, uint64_t #endif } +#if MAVLINK_MSG_ID_GPS_RAW_INT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_gps_raw_int_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lon); + _mav_put_int32_t(buf, 16, alt); + _mav_put_uint16_t(buf, 20, eph); + _mav_put_uint16_t(buf, 22, epv); + _mav_put_uint16_t(buf, 24, vel); + _mav_put_uint16_t(buf, 26, cog); + _mav_put_uint8_t(buf, 28, fix_type); + _mav_put_uint8_t(buf, 29, satellites_visible); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RAW_INT, buf, MAVLINK_MSG_ID_GPS_RAW_INT_LEN, MAVLINK_MSG_ID_GPS_RAW_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RAW_INT, buf, MAVLINK_MSG_ID_GPS_RAW_INT_LEN); +#endif +#else + mavlink_gps_raw_int_t *packet = (mavlink_gps_raw_int_t *)msgbuf; + packet->time_usec = time_usec; + packet->lat = lat; + packet->lon = lon; + packet->alt = alt; + packet->eph = eph; + packet->epv = epv; + packet->vel = vel; + packet->cog = cog; + packet->fix_type = fix_type; + packet->satellites_visible = satellites_visible; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RAW_INT, (const char *)packet, MAVLINK_MSG_ID_GPS_RAW_INT_LEN, MAVLINK_MSG_ID_GPS_RAW_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RAW_INT, (const char *)packet, MAVLINK_MSG_ID_GPS_RAW_INT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE GPS_RAW_INT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_status.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_status.h index 28d6b57d19..10659d0dd8 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_status.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_status.h @@ -199,6 +199,46 @@ static inline void mavlink_msg_gps_status_send(mavlink_channel_t chan, uint8_t s #endif } +#if MAVLINK_MSG_ID_GPS_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_gps_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t satellites_visible, const uint8_t *satellite_prn, const uint8_t *satellite_used, const uint8_t *satellite_elevation, const uint8_t *satellite_azimuth, const uint8_t *satellite_snr) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, satellites_visible); + _mav_put_uint8_t_array(buf, 1, satellite_prn, 20); + _mav_put_uint8_t_array(buf, 21, satellite_used, 20); + _mav_put_uint8_t_array(buf, 41, satellite_elevation, 20); + _mav_put_uint8_t_array(buf, 61, satellite_azimuth, 20); + _mav_put_uint8_t_array(buf, 81, satellite_snr, 20); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_STATUS, buf, MAVLINK_MSG_ID_GPS_STATUS_LEN, MAVLINK_MSG_ID_GPS_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_STATUS, buf, MAVLINK_MSG_ID_GPS_STATUS_LEN); +#endif +#else + mavlink_gps_status_t *packet = (mavlink_gps_status_t *)msgbuf; + packet->satellites_visible = satellites_visible; + mav_array_memcpy(packet->satellite_prn, satellite_prn, sizeof(uint8_t)*20); + mav_array_memcpy(packet->satellite_used, satellite_used, sizeof(uint8_t)*20); + mav_array_memcpy(packet->satellite_elevation, satellite_elevation, sizeof(uint8_t)*20); + mav_array_memcpy(packet->satellite_azimuth, satellite_azimuth, sizeof(uint8_t)*20); + mav_array_memcpy(packet->satellite_snr, satellite_snr, sizeof(uint8_t)*20); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_STATUS, (const char *)packet, MAVLINK_MSG_ID_GPS_STATUS_LEN, MAVLINK_MSG_ID_GPS_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_STATUS, (const char *)packet, MAVLINK_MSG_ID_GPS_STATUS_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE GPS_STATUS UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_heartbeat.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_heartbeat.h index 826138fad1..902e381ca8 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_heartbeat.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_heartbeat.h @@ -198,6 +198,48 @@ static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t ty #endif } +#if MAVLINK_MSG_ID_HEARTBEAT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_heartbeat_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t type, uint8_t autopilot, uint8_t base_mode, uint32_t custom_mode, uint8_t system_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, custom_mode); + _mav_put_uint8_t(buf, 4, type); + _mav_put_uint8_t(buf, 5, autopilot); + _mav_put_uint8_t(buf, 6, base_mode); + _mav_put_uint8_t(buf, 7, system_status); + _mav_put_uint8_t(buf, 8, 3); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, buf, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, buf, MAVLINK_MSG_ID_HEARTBEAT_LEN); +#endif +#else + mavlink_heartbeat_t *packet = (mavlink_heartbeat_t *)msgbuf; + packet->custom_mode = custom_mode; + packet->type = type; + packet->autopilot = autopilot; + packet->base_mode = base_mode; + packet->system_status = system_status; + packet->mavlink_version = 3; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, (const char *)packet, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, (const char *)packet, MAVLINK_MSG_ID_HEARTBEAT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE HEARTBEAT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_highres_imu.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_highres_imu.h index 0dcd95ed32..2749cb097f 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_highres_imu.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_highres_imu.h @@ -300,6 +300,66 @@ static inline void mavlink_msg_highres_imu_send(mavlink_channel_t chan, uint64_t #endif } +#if MAVLINK_MSG_ID_HIGHRES_IMU_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_highres_imu_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, uint16_t fields_updated) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, xacc); + _mav_put_float(buf, 12, yacc); + _mav_put_float(buf, 16, zacc); + _mav_put_float(buf, 20, xgyro); + _mav_put_float(buf, 24, ygyro); + _mav_put_float(buf, 28, zgyro); + _mav_put_float(buf, 32, xmag); + _mav_put_float(buf, 36, ymag); + _mav_put_float(buf, 40, zmag); + _mav_put_float(buf, 44, abs_pressure); + _mav_put_float(buf, 48, diff_pressure); + _mav_put_float(buf, 52, pressure_alt); + _mav_put_float(buf, 56, temperature); + _mav_put_uint16_t(buf, 60, fields_updated); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIGHRES_IMU, buf, MAVLINK_MSG_ID_HIGHRES_IMU_LEN, MAVLINK_MSG_ID_HIGHRES_IMU_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIGHRES_IMU, buf, MAVLINK_MSG_ID_HIGHRES_IMU_LEN); +#endif +#else + mavlink_highres_imu_t *packet = (mavlink_highres_imu_t *)msgbuf; + packet->time_usec = time_usec; + packet->xacc = xacc; + packet->yacc = yacc; + packet->zacc = zacc; + packet->xgyro = xgyro; + packet->ygyro = ygyro; + packet->zgyro = zgyro; + packet->xmag = xmag; + packet->ymag = ymag; + packet->zmag = zmag; + packet->abs_pressure = abs_pressure; + packet->diff_pressure = diff_pressure; + packet->pressure_alt = pressure_alt; + packet->temperature = temperature; + packet->fields_updated = fields_updated; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIGHRES_IMU, (const char *)packet, MAVLINK_MSG_ID_HIGHRES_IMU_LEN, MAVLINK_MSG_ID_HIGHRES_IMU_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIGHRES_IMU, (const char *)packet, MAVLINK_MSG_ID_HIGHRES_IMU_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE HIGHRES_IMU UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_controls.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_controls.h index aed5108d05..f7507b1cf7 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_controls.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_controls.h @@ -256,6 +256,58 @@ static inline void mavlink_msg_hil_controls_send(mavlink_channel_t chan, uint64_ #endif } +#if MAVLINK_MSG_ID_HIL_CONTROLS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_hil_controls_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, float roll_ailerons, float pitch_elevator, float yaw_rudder, float throttle, float aux1, float aux2, float aux3, float aux4, uint8_t mode, uint8_t nav_mode) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, roll_ailerons); + _mav_put_float(buf, 12, pitch_elevator); + _mav_put_float(buf, 16, yaw_rudder); + _mav_put_float(buf, 20, throttle); + _mav_put_float(buf, 24, aux1); + _mav_put_float(buf, 28, aux2); + _mav_put_float(buf, 32, aux3); + _mav_put_float(buf, 36, aux4); + _mav_put_uint8_t(buf, 40, mode); + _mav_put_uint8_t(buf, 41, nav_mode); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_CONTROLS, buf, MAVLINK_MSG_ID_HIL_CONTROLS_LEN, MAVLINK_MSG_ID_HIL_CONTROLS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_CONTROLS, buf, MAVLINK_MSG_ID_HIL_CONTROLS_LEN); +#endif +#else + mavlink_hil_controls_t *packet = (mavlink_hil_controls_t *)msgbuf; + packet->time_usec = time_usec; + packet->roll_ailerons = roll_ailerons; + packet->pitch_elevator = pitch_elevator; + packet->yaw_rudder = yaw_rudder; + packet->throttle = throttle; + packet->aux1 = aux1; + packet->aux2 = aux2; + packet->aux3 = aux3; + packet->aux4 = aux4; + packet->mode = mode; + packet->nav_mode = nav_mode; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_CONTROLS, (const char *)packet, MAVLINK_MSG_ID_HIL_CONTROLS_LEN, MAVLINK_MSG_ID_HIL_CONTROLS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_CONTROLS, (const char *)packet, MAVLINK_MSG_ID_HIL_CONTROLS_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE HIL_CONTROLS UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_gps.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_gps.h index 91aec5b08c..a22c0472f0 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_gps.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_gps.h @@ -278,6 +278,62 @@ static inline void mavlink_msg_hil_gps_send(mavlink_channel_t chan, uint64_t tim #endif } +#if MAVLINK_MSG_ID_HIL_GPS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_hil_gps_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, int16_t vn, int16_t ve, int16_t vd, uint16_t cog, uint8_t satellites_visible) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lon); + _mav_put_int32_t(buf, 16, alt); + _mav_put_uint16_t(buf, 20, eph); + _mav_put_uint16_t(buf, 22, epv); + _mav_put_uint16_t(buf, 24, vel); + _mav_put_int16_t(buf, 26, vn); + _mav_put_int16_t(buf, 28, ve); + _mav_put_int16_t(buf, 30, vd); + _mav_put_uint16_t(buf, 32, cog); + _mav_put_uint8_t(buf, 34, fix_type); + _mav_put_uint8_t(buf, 35, satellites_visible); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_GPS, buf, MAVLINK_MSG_ID_HIL_GPS_LEN, MAVLINK_MSG_ID_HIL_GPS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_GPS, buf, MAVLINK_MSG_ID_HIL_GPS_LEN); +#endif +#else + mavlink_hil_gps_t *packet = (mavlink_hil_gps_t *)msgbuf; + packet->time_usec = time_usec; + packet->lat = lat; + packet->lon = lon; + packet->alt = alt; + packet->eph = eph; + packet->epv = epv; + packet->vel = vel; + packet->vn = vn; + packet->ve = ve; + packet->vd = vd; + packet->cog = cog; + packet->fix_type = fix_type; + packet->satellites_visible = satellites_visible; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_GPS, (const char *)packet, MAVLINK_MSG_ID_HIL_GPS_LEN, MAVLINK_MSG_ID_HIL_GPS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_GPS, (const char *)packet, MAVLINK_MSG_ID_HIL_GPS_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE HIL_GPS UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_optical_flow.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_optical_flow.h index acb1392e14..eaadf24359 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_optical_flow.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_optical_flow.h @@ -223,6 +223,52 @@ static inline void mavlink_msg_hil_optical_flow_send(mavlink_channel_t chan, uin #endif } +#if MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_hil_optical_flow_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t sensor_id, int16_t flow_x, int16_t flow_y, float flow_comp_m_x, float flow_comp_m_y, uint8_t quality, float ground_distance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, flow_comp_m_x); + _mav_put_float(buf, 12, flow_comp_m_y); + _mav_put_float(buf, 16, ground_distance); + _mav_put_int16_t(buf, 20, flow_x); + _mav_put_int16_t(buf, 22, flow_y); + _mav_put_uint8_t(buf, 24, sensor_id); + _mav_put_uint8_t(buf, 25, quality); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW, buf, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW, buf, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN); +#endif +#else + mavlink_hil_optical_flow_t *packet = (mavlink_hil_optical_flow_t *)msgbuf; + packet->time_usec = time_usec; + packet->flow_comp_m_x = flow_comp_m_x; + packet->flow_comp_m_y = flow_comp_m_y; + packet->ground_distance = ground_distance; + packet->flow_x = flow_x; + packet->flow_y = flow_y; + packet->sensor_id = sensor_id; + packet->quality = quality; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW, (const char *)packet, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW, (const char *)packet, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE HIL_OPTICAL_FLOW UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_rc_inputs_raw.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_rc_inputs_raw.h index a42bde50b2..227cd9d94e 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_rc_inputs_raw.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_rc_inputs_raw.h @@ -289,6 +289,64 @@ static inline void mavlink_msg_hil_rc_inputs_raw_send(mavlink_channel_t chan, ui #endif } +#if MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_hil_rc_inputs_raw_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint16_t chan9_raw, uint16_t chan10_raw, uint16_t chan11_raw, uint16_t chan12_raw, uint8_t rssi) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint16_t(buf, 8, chan1_raw); + _mav_put_uint16_t(buf, 10, chan2_raw); + _mav_put_uint16_t(buf, 12, chan3_raw); + _mav_put_uint16_t(buf, 14, chan4_raw); + _mav_put_uint16_t(buf, 16, chan5_raw); + _mav_put_uint16_t(buf, 18, chan6_raw); + _mav_put_uint16_t(buf, 20, chan7_raw); + _mav_put_uint16_t(buf, 22, chan8_raw); + _mav_put_uint16_t(buf, 24, chan9_raw); + _mav_put_uint16_t(buf, 26, chan10_raw); + _mav_put_uint16_t(buf, 28, chan11_raw); + _mav_put_uint16_t(buf, 30, chan12_raw); + _mav_put_uint8_t(buf, 32, rssi); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW, buf, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW, buf, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN); +#endif +#else + mavlink_hil_rc_inputs_raw_t *packet = (mavlink_hil_rc_inputs_raw_t *)msgbuf; + packet->time_usec = time_usec; + packet->chan1_raw = chan1_raw; + packet->chan2_raw = chan2_raw; + packet->chan3_raw = chan3_raw; + packet->chan4_raw = chan4_raw; + packet->chan5_raw = chan5_raw; + packet->chan6_raw = chan6_raw; + packet->chan7_raw = chan7_raw; + packet->chan8_raw = chan8_raw; + packet->chan9_raw = chan9_raw; + packet->chan10_raw = chan10_raw; + packet->chan11_raw = chan11_raw; + packet->chan12_raw = chan12_raw; + packet->rssi = rssi; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW, (const char *)packet, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW, (const char *)packet, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE HIL_RC_INPUTS_RAW UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_sensor.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_sensor.h index 6c2667473d..8672f8b8a3 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_sensor.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_sensor.h @@ -300,6 +300,66 @@ static inline void mavlink_msg_hil_sensor_send(mavlink_channel_t chan, uint64_t #endif } +#if MAVLINK_MSG_ID_HIL_SENSOR_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_hil_sensor_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, uint32_t fields_updated) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, xacc); + _mav_put_float(buf, 12, yacc); + _mav_put_float(buf, 16, zacc); + _mav_put_float(buf, 20, xgyro); + _mav_put_float(buf, 24, ygyro); + _mav_put_float(buf, 28, zgyro); + _mav_put_float(buf, 32, xmag); + _mav_put_float(buf, 36, ymag); + _mav_put_float(buf, 40, zmag); + _mav_put_float(buf, 44, abs_pressure); + _mav_put_float(buf, 48, diff_pressure); + _mav_put_float(buf, 52, pressure_alt); + _mav_put_float(buf, 56, temperature); + _mav_put_uint32_t(buf, 60, fields_updated); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_SENSOR, buf, MAVLINK_MSG_ID_HIL_SENSOR_LEN, MAVLINK_MSG_ID_HIL_SENSOR_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_SENSOR, buf, MAVLINK_MSG_ID_HIL_SENSOR_LEN); +#endif +#else + mavlink_hil_sensor_t *packet = (mavlink_hil_sensor_t *)msgbuf; + packet->time_usec = time_usec; + packet->xacc = xacc; + packet->yacc = yacc; + packet->zacc = zacc; + packet->xgyro = xgyro; + packet->ygyro = ygyro; + packet->zgyro = zgyro; + packet->xmag = xmag; + packet->ymag = ymag; + packet->zmag = zmag; + packet->abs_pressure = abs_pressure; + packet->diff_pressure = diff_pressure; + packet->pressure_alt = pressure_alt; + packet->temperature = temperature; + packet->fields_updated = fields_updated; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_SENSOR, (const char *)packet, MAVLINK_MSG_ID_HIL_SENSOR_LEN, MAVLINK_MSG_ID_HIL_SENSOR_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_SENSOR, (const char *)packet, MAVLINK_MSG_ID_HIL_SENSOR_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE HIL_SENSOR UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state.h index bcc8577670..923ed60b95 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state.h @@ -311,6 +311,68 @@ static inline void mavlink_msg_hil_state_send(mavlink_channel_t chan, uint64_t t #endif } +#if MAVLINK_MSG_ID_HIL_STATE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_hil_state_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, int16_t xacc, int16_t yacc, int16_t zacc) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, roll); + _mav_put_float(buf, 12, pitch); + _mav_put_float(buf, 16, yaw); + _mav_put_float(buf, 20, rollspeed); + _mav_put_float(buf, 24, pitchspeed); + _mav_put_float(buf, 28, yawspeed); + _mav_put_int32_t(buf, 32, lat); + _mav_put_int32_t(buf, 36, lon); + _mav_put_int32_t(buf, 40, alt); + _mav_put_int16_t(buf, 44, vx); + _mav_put_int16_t(buf, 46, vy); + _mav_put_int16_t(buf, 48, vz); + _mav_put_int16_t(buf, 50, xacc); + _mav_put_int16_t(buf, 52, yacc); + _mav_put_int16_t(buf, 54, zacc); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE, buf, MAVLINK_MSG_ID_HIL_STATE_LEN, MAVLINK_MSG_ID_HIL_STATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE, buf, MAVLINK_MSG_ID_HIL_STATE_LEN); +#endif +#else + mavlink_hil_state_t *packet = (mavlink_hil_state_t *)msgbuf; + packet->time_usec = time_usec; + packet->roll = roll; + packet->pitch = pitch; + packet->yaw = yaw; + packet->rollspeed = rollspeed; + packet->pitchspeed = pitchspeed; + packet->yawspeed = yawspeed; + packet->lat = lat; + packet->lon = lon; + packet->alt = alt; + packet->vx = vx; + packet->vy = vy; + packet->vz = vz; + packet->xacc = xacc; + packet->yacc = yacc; + packet->zacc = zacc; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE, (const char *)packet, MAVLINK_MSG_ID_HIL_STATE_LEN, MAVLINK_MSG_ID_HIL_STATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE, (const char *)packet, MAVLINK_MSG_ID_HIL_STATE_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE HIL_STATE UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state_quaternion.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state_quaternion.h index 732176193e..1a028bdf99 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state_quaternion.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_hil_state_quaternion.h @@ -305,6 +305,66 @@ static inline void mavlink_msg_hil_state_quaternion_send(mavlink_channel_t chan, #endif } +#if MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_hil_state_quaternion_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, const float *attitude_quaternion, float rollspeed, float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, uint16_t ind_airspeed, uint16_t true_airspeed, int16_t xacc, int16_t yacc, int16_t zacc) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 24, rollspeed); + _mav_put_float(buf, 28, pitchspeed); + _mav_put_float(buf, 32, yawspeed); + _mav_put_int32_t(buf, 36, lat); + _mav_put_int32_t(buf, 40, lon); + _mav_put_int32_t(buf, 44, alt); + _mav_put_int16_t(buf, 48, vx); + _mav_put_int16_t(buf, 50, vy); + _mav_put_int16_t(buf, 52, vz); + _mav_put_uint16_t(buf, 54, ind_airspeed); + _mav_put_uint16_t(buf, 56, true_airspeed); + _mav_put_int16_t(buf, 58, xacc); + _mav_put_int16_t(buf, 60, yacc); + _mav_put_int16_t(buf, 62, zacc); + _mav_put_float_array(buf, 8, attitude_quaternion, 4); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE_QUATERNION, buf, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE_QUATERNION, buf, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN); +#endif +#else + mavlink_hil_state_quaternion_t *packet = (mavlink_hil_state_quaternion_t *)msgbuf; + packet->time_usec = time_usec; + packet->rollspeed = rollspeed; + packet->pitchspeed = pitchspeed; + packet->yawspeed = yawspeed; + packet->lat = lat; + packet->lon = lon; + packet->alt = alt; + packet->vx = vx; + packet->vy = vy; + packet->vz = vz; + packet->ind_airspeed = ind_airspeed; + packet->true_airspeed = true_airspeed; + packet->xacc = xacc; + packet->yacc = yacc; + packet->zacc = zacc; + mav_array_memcpy(packet->attitude_quaternion, attitude_quaternion, sizeof(float)*4); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE_QUATERNION, (const char *)packet, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE_QUATERNION, (const char *)packet, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE HIL_STATE_QUATERNION UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned.h index a0b72c0e1e..e18a948695 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned.h @@ -212,6 +212,50 @@ static inline void mavlink_msg_local_position_ned_send(mavlink_channel_t chan, u #endif } +#if MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_local_position_ned_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, float x, float y, float z, float vx, float vy, float vz) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, x); + _mav_put_float(buf, 8, y); + _mav_put_float(buf, 12, z); + _mav_put_float(buf, 16, vx); + _mav_put_float(buf, 20, vy); + _mav_put_float(buf, 24, vz); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED, buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED, buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN); +#endif +#else + mavlink_local_position_ned_t *packet = (mavlink_local_position_ned_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->x = x; + packet->y = y; + packet->z = z; + packet->vx = vx; + packet->vy = vy; + packet->vz = vz; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED, (const char *)packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED, (const char *)packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE LOCAL_POSITION_NED UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned_system_global_offset.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned_system_global_offset.h index 8c46862027..af7d195b01 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned_system_global_offset.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_ned_system_global_offset.h @@ -212,6 +212,50 @@ static inline void mavlink_msg_local_position_ned_system_global_offset_send(mavl #endif } +#if MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_local_position_ned_system_global_offset_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, float x, float y, float z, float roll, float pitch, float yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, x); + _mav_put_float(buf, 8, y); + _mav_put_float(buf, 12, z); + _mav_put_float(buf, 16, roll); + _mav_put_float(buf, 20, pitch); + _mav_put_float(buf, 24, yaw); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, buf, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN); +#endif +#else + mavlink_local_position_ned_system_global_offset_t *packet = (mavlink_local_position_ned_system_global_offset_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->x = x; + packet->y = y; + packet->z = z; + packet->roll = roll; + packet->pitch = pitch; + packet->yaw = yaw; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, (const char *)packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, (const char *)packet, MAVLINK_MSG_ID_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_setpoint.h index 1794815f8e..8dae721beb 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_setpoint.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_local_position_setpoint.h @@ -190,6 +190,46 @@ static inline void mavlink_msg_local_position_setpoint_send(mavlink_channel_t ch #endif } +#if MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_local_position_setpoint_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t coordinate_frame, float x, float y, float z, float yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, x); + _mav_put_float(buf, 4, y); + _mav_put_float(buf, 8, z); + _mav_put_float(buf, 12, yaw); + _mav_put_uint8_t(buf, 16, coordinate_frame); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT, buf, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT, buf, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN); +#endif +#else + mavlink_local_position_setpoint_t *packet = (mavlink_local_position_setpoint_t *)msgbuf; + packet->x = x; + packet->y = y; + packet->z = z; + packet->yaw = yaw; + packet->coordinate_frame = coordinate_frame; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE LOCAL_POSITION_SETPOINT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_data.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_data.h index 1cf5d15e48..48641f3eb0 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_data.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_data.h @@ -173,6 +173,42 @@ static inline void mavlink_msg_log_data_send(mavlink_channel_t chan, uint16_t id #endif } +#if MAVLINK_MSG_ID_LOG_DATA_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_log_data_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t id, uint32_t ofs, uint8_t count, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, ofs); + _mav_put_uint16_t(buf, 4, id); + _mav_put_uint8_t(buf, 6, count); + _mav_put_uint8_t_array(buf, 7, data, 90); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_DATA, buf, MAVLINK_MSG_ID_LOG_DATA_LEN, MAVLINK_MSG_ID_LOG_DATA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_DATA, buf, MAVLINK_MSG_ID_LOG_DATA_LEN); +#endif +#else + mavlink_log_data_t *packet = (mavlink_log_data_t *)msgbuf; + packet->ofs = ofs; + packet->id = id; + packet->count = count; + mav_array_memcpy(packet->data, data, sizeof(uint8_t)*90); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_DATA, (const char *)packet, MAVLINK_MSG_ID_LOG_DATA_LEN, MAVLINK_MSG_ID_LOG_DATA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_DATA, (const char *)packet, MAVLINK_MSG_ID_LOG_DATA_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE LOG_DATA UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_entry.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_entry.h index 681d8f07cd..8ecaec3ae2 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_entry.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_entry.h @@ -190,6 +190,46 @@ static inline void mavlink_msg_log_entry_send(mavlink_channel_t chan, uint16_t i #endif } +#if MAVLINK_MSG_ID_LOG_ENTRY_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_log_entry_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t id, uint16_t num_logs, uint16_t last_log_num, uint32_t time_utc, uint32_t size) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_utc); + _mav_put_uint32_t(buf, 4, size); + _mav_put_uint16_t(buf, 8, id); + _mav_put_uint16_t(buf, 10, num_logs); + _mav_put_uint16_t(buf, 12, last_log_num); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ENTRY, buf, MAVLINK_MSG_ID_LOG_ENTRY_LEN, MAVLINK_MSG_ID_LOG_ENTRY_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ENTRY, buf, MAVLINK_MSG_ID_LOG_ENTRY_LEN); +#endif +#else + mavlink_log_entry_t *packet = (mavlink_log_entry_t *)msgbuf; + packet->time_utc = time_utc; + packet->size = size; + packet->id = id; + packet->num_logs = num_logs; + packet->last_log_num = last_log_num; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ENTRY, (const char *)packet, MAVLINK_MSG_ID_LOG_ENTRY_LEN, MAVLINK_MSG_ID_LOG_ENTRY_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ENTRY, (const char *)packet, MAVLINK_MSG_ID_LOG_ENTRY_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE LOG_ENTRY UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_erase.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_erase.h index feafeaf164..957c4d4cc0 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_erase.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_erase.h @@ -157,6 +157,40 @@ static inline void mavlink_msg_log_erase_send(mavlink_channel_t chan, uint8_t ta #endif } +#if MAVLINK_MSG_ID_LOG_ERASE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_log_erase_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ERASE, buf, MAVLINK_MSG_ID_LOG_ERASE_LEN, MAVLINK_MSG_ID_LOG_ERASE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ERASE, buf, MAVLINK_MSG_ID_LOG_ERASE_LEN); +#endif +#else + mavlink_log_erase_t *packet = (mavlink_log_erase_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ERASE, (const char *)packet, MAVLINK_MSG_ID_LOG_ERASE_LEN, MAVLINK_MSG_ID_LOG_ERASE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_ERASE, (const char *)packet, MAVLINK_MSG_ID_LOG_ERASE_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE LOG_ERASE UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_data.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_data.h index 5be9eea471..ef5cbb67cb 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_data.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_data.h @@ -190,6 +190,46 @@ static inline void mavlink_msg_log_request_data_send(mavlink_channel_t chan, uin #endif } +#if MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_log_request_data_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t id, uint32_t ofs, uint32_t count) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, ofs); + _mav_put_uint32_t(buf, 4, count); + _mav_put_uint16_t(buf, 8, id); + _mav_put_uint8_t(buf, 10, target_system); + _mav_put_uint8_t(buf, 11, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_DATA, buf, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN, MAVLINK_MSG_ID_LOG_REQUEST_DATA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_DATA, buf, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN); +#endif +#else + mavlink_log_request_data_t *packet = (mavlink_log_request_data_t *)msgbuf; + packet->ofs = ofs; + packet->count = count; + packet->id = id; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_DATA, (const char *)packet, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN, MAVLINK_MSG_ID_LOG_REQUEST_DATA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_DATA, (const char *)packet, MAVLINK_MSG_ID_LOG_REQUEST_DATA_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE LOG_REQUEST_DATA UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_end.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_end.h index 48a5a03b4c..23fcca29f7 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_end.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_end.h @@ -157,6 +157,40 @@ static inline void mavlink_msg_log_request_end_send(mavlink_channel_t chan, uint #endif } +#if MAVLINK_MSG_ID_LOG_REQUEST_END_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_log_request_end_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_END, buf, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN, MAVLINK_MSG_ID_LOG_REQUEST_END_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_END, buf, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN); +#endif +#else + mavlink_log_request_end_t *packet = (mavlink_log_request_end_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_END, (const char *)packet, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN, MAVLINK_MSG_ID_LOG_REQUEST_END_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_END, (const char *)packet, MAVLINK_MSG_ID_LOG_REQUEST_END_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE LOG_REQUEST_END UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_list.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_list.h index 7a5b25c171..e511b53125 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_list.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_log_request_list.h @@ -179,6 +179,44 @@ static inline void mavlink_msg_log_request_list_send(mavlink_channel_t chan, uin #endif } +#if MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_log_request_list_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t start, uint16_t end) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, start); + _mav_put_uint16_t(buf, 2, end); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_LIST, buf, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN, MAVLINK_MSG_ID_LOG_REQUEST_LIST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_LIST, buf, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN); +#endif +#else + mavlink_log_request_list_t *packet = (mavlink_log_request_list_t *)msgbuf; + packet->start = start; + packet->end = end; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_LIST, (const char *)packet, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN, MAVLINK_MSG_ID_LOG_REQUEST_LIST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOG_REQUEST_LIST, (const char *)packet, MAVLINK_MSG_ID_LOG_REQUEST_LIST_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE LOG_REQUEST_LIST UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_control.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_control.h index 6b6e9e148a..e93b759d0a 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_control.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_control.h @@ -201,6 +201,48 @@ static inline void mavlink_msg_manual_control_send(mavlink_channel_t chan, uint8 #endif } +#if MAVLINK_MSG_ID_MANUAL_CONTROL_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_manual_control_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target, int16_t x, int16_t y, int16_t z, int16_t r, uint16_t buttons) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int16_t(buf, 0, x); + _mav_put_int16_t(buf, 2, y); + _mav_put_int16_t(buf, 4, z); + _mav_put_int16_t(buf, 6, r); + _mav_put_uint16_t(buf, 8, buttons); + _mav_put_uint8_t(buf, 10, target); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_CONTROL, buf, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN, MAVLINK_MSG_ID_MANUAL_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_CONTROL, buf, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN); +#endif +#else + mavlink_manual_control_t *packet = (mavlink_manual_control_t *)msgbuf; + packet->x = x; + packet->y = y; + packet->z = z; + packet->r = r; + packet->buttons = buttons; + packet->target = target; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_CONTROL, (const char *)packet, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN, MAVLINK_MSG_ID_MANUAL_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_CONTROL, (const char *)packet, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE MANUAL_CONTROL UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_setpoint.h index a694947c12..b276267269 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_setpoint.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_manual_setpoint.h @@ -212,6 +212,50 @@ static inline void mavlink_msg_manual_setpoint_send(mavlink_channel_t chan, uint #endif } +#if MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_manual_setpoint_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, float roll, float pitch, float yaw, float thrust, uint8_t mode_switch, uint8_t manual_override_switch) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, roll); + _mav_put_float(buf, 8, pitch); + _mav_put_float(buf, 12, yaw); + _mav_put_float(buf, 16, thrust); + _mav_put_uint8_t(buf, 20, mode_switch); + _mav_put_uint8_t(buf, 21, manual_override_switch); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_SETPOINT, buf, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN, MAVLINK_MSG_ID_MANUAL_SETPOINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_SETPOINT, buf, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN); +#endif +#else + mavlink_manual_setpoint_t *packet = (mavlink_manual_setpoint_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->roll = roll; + packet->pitch = pitch; + packet->yaw = yaw; + packet->thrust = thrust; + packet->mode_switch = mode_switch; + packet->manual_override_switch = manual_override_switch; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN, MAVLINK_MSG_ID_MANUAL_SETPOINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_MANUAL_SETPOINT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE MANUAL_SETPOINT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_memory_vect.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_memory_vect.h index 5f79329c25..2eb60cc0e6 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_memory_vect.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_memory_vect.h @@ -173,6 +173,42 @@ static inline void mavlink_msg_memory_vect_send(mavlink_channel_t chan, uint16_t #endif } +#if MAVLINK_MSG_ID_MEMORY_VECT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_memory_vect_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t address, uint8_t ver, uint8_t type, const int8_t *value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, address); + _mav_put_uint8_t(buf, 2, ver); + _mav_put_uint8_t(buf, 3, type); + _mav_put_int8_t_array(buf, 4, value, 32); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMORY_VECT, buf, MAVLINK_MSG_ID_MEMORY_VECT_LEN, MAVLINK_MSG_ID_MEMORY_VECT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMORY_VECT, buf, MAVLINK_MSG_ID_MEMORY_VECT_LEN); +#endif +#else + mavlink_memory_vect_t *packet = (mavlink_memory_vect_t *)msgbuf; + packet->address = address; + packet->ver = ver; + packet->type = type; + mav_array_memcpy(packet->value, value, sizeof(int8_t)*32); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMORY_VECT, (const char *)packet, MAVLINK_MSG_ID_MEMORY_VECT_LEN, MAVLINK_MSG_ID_MEMORY_VECT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMORY_VECT, (const char *)packet, MAVLINK_MSG_ID_MEMORY_VECT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE MEMORY_VECT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_ack.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_ack.h index 7421d8394a..43afd00f7e 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_ack.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_ack.h @@ -168,6 +168,42 @@ static inline void mavlink_msg_mission_ack_send(mavlink_channel_t chan, uint8_t #endif } +#if MAVLINK_MSG_ID_MISSION_ACK_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mission_ack_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t type) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, type); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ACK, buf, MAVLINK_MSG_ID_MISSION_ACK_LEN, MAVLINK_MSG_ID_MISSION_ACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ACK, buf, MAVLINK_MSG_ID_MISSION_ACK_LEN); +#endif +#else + mavlink_mission_ack_t *packet = (mavlink_mission_ack_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + packet->type = type; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ACK, (const char *)packet, MAVLINK_MSG_ID_MISSION_ACK_LEN, MAVLINK_MSG_ID_MISSION_ACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ACK, (const char *)packet, MAVLINK_MSG_ID_MISSION_ACK_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE MISSION_ACK UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_clear_all.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_clear_all.h index 8f441c8e5c..120986873f 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_clear_all.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_clear_all.h @@ -157,6 +157,40 @@ static inline void mavlink_msg_mission_clear_all_send(mavlink_channel_t chan, ui #endif } +#if MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mission_clear_all_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CLEAR_ALL, buf, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CLEAR_ALL, buf, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN); +#endif +#else + mavlink_mission_clear_all_t *packet = (mavlink_mission_clear_all_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CLEAR_ALL, (const char *)packet, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CLEAR_ALL, (const char *)packet, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE MISSION_CLEAR_ALL UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_count.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_count.h index eac7793067..7e4748eaef 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_count.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_count.h @@ -168,6 +168,42 @@ static inline void mavlink_msg_mission_count_send(mavlink_channel_t chan, uint8_ #endif } +#if MAVLINK_MSG_ID_MISSION_COUNT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mission_count_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t count) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, count); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_COUNT, buf, MAVLINK_MSG_ID_MISSION_COUNT_LEN, MAVLINK_MSG_ID_MISSION_COUNT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_COUNT, buf, MAVLINK_MSG_ID_MISSION_COUNT_LEN); +#endif +#else + mavlink_mission_count_t *packet = (mavlink_mission_count_t *)msgbuf; + packet->count = count; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_COUNT, (const char *)packet, MAVLINK_MSG_ID_MISSION_COUNT_LEN, MAVLINK_MSG_ID_MISSION_COUNT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_COUNT, (const char *)packet, MAVLINK_MSG_ID_MISSION_COUNT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE MISSION_COUNT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_current.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_current.h index dbcdbd3f9c..201b7a6fb4 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_current.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_current.h @@ -146,6 +146,38 @@ static inline void mavlink_msg_mission_current_send(mavlink_channel_t chan, uint #endif } +#if MAVLINK_MSG_ID_MISSION_CURRENT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mission_current_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t seq) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, seq); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CURRENT, buf, MAVLINK_MSG_ID_MISSION_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_CURRENT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CURRENT, buf, MAVLINK_MSG_ID_MISSION_CURRENT_LEN); +#endif +#else + mavlink_mission_current_t *packet = (mavlink_mission_current_t *)msgbuf; + packet->seq = seq; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CURRENT, (const char *)packet, MAVLINK_MSG_ID_MISSION_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_CURRENT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CURRENT, (const char *)packet, MAVLINK_MSG_ID_MISSION_CURRENT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE MISSION_CURRENT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item.h index a8d60eca7e..ef9394f00c 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item.h @@ -289,6 +289,64 @@ static inline void mavlink_msg_mission_item_send(mavlink_channel_t chan, uint8_t #endif } +#if MAVLINK_MSG_ID_MISSION_ITEM_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mission_item_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, float x, float y, float z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, param1); + _mav_put_float(buf, 4, param2); + _mav_put_float(buf, 8, param3); + _mav_put_float(buf, 12, param4); + _mav_put_float(buf, 16, x); + _mav_put_float(buf, 20, y); + _mav_put_float(buf, 24, z); + _mav_put_uint16_t(buf, 28, seq); + _mav_put_uint16_t(buf, 30, command); + _mav_put_uint8_t(buf, 32, target_system); + _mav_put_uint8_t(buf, 33, target_component); + _mav_put_uint8_t(buf, 34, frame); + _mav_put_uint8_t(buf, 35, current); + _mav_put_uint8_t(buf, 36, autocontinue); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM, buf, MAVLINK_MSG_ID_MISSION_ITEM_LEN, MAVLINK_MSG_ID_MISSION_ITEM_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM, buf, MAVLINK_MSG_ID_MISSION_ITEM_LEN); +#endif +#else + mavlink_mission_item_t *packet = (mavlink_mission_item_t *)msgbuf; + packet->param1 = param1; + packet->param2 = param2; + packet->param3 = param3; + packet->param4 = param4; + packet->x = x; + packet->y = y; + packet->z = z; + packet->seq = seq; + packet->command = command; + packet->target_system = target_system; + packet->target_component = target_component; + packet->frame = frame; + packet->current = current; + packet->autocontinue = autocontinue; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM, (const char *)packet, MAVLINK_MSG_ID_MISSION_ITEM_LEN, MAVLINK_MSG_ID_MISSION_ITEM_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM, (const char *)packet, MAVLINK_MSG_ID_MISSION_ITEM_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE MISSION_ITEM UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item_reached.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item_reached.h index f3744fde6c..9dfa280433 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item_reached.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_item_reached.h @@ -146,6 +146,38 @@ static inline void mavlink_msg_mission_item_reached_send(mavlink_channel_t chan, #endif } +#if MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mission_item_reached_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t seq) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, seq); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_REACHED, buf, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_REACHED, buf, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN); +#endif +#else + mavlink_mission_item_reached_t *packet = (mavlink_mission_item_reached_t *)msgbuf; + packet->seq = seq; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_REACHED, (const char *)packet, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_REACHED, (const char *)packet, MAVLINK_MSG_ID_MISSION_ITEM_REACHED_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE MISSION_ITEM_REACHED UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request.h index ac84e35540..29b0ef6ef5 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request.h @@ -168,6 +168,42 @@ static inline void mavlink_msg_mission_request_send(mavlink_channel_t chan, uint #endif } +#if MAVLINK_MSG_ID_MISSION_REQUEST_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mission_request_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, seq); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_LEN); +#endif +#else + mavlink_mission_request_t *packet = (mavlink_mission_request_t *)msgbuf; + packet->seq = seq; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST, (const char *)packet, MAVLINK_MSG_ID_MISSION_REQUEST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST, (const char *)packet, MAVLINK_MSG_ID_MISSION_REQUEST_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE MISSION_REQUEST UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_list.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_list.h index d999babdbd..a275348acc 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_list.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_list.h @@ -157,6 +157,40 @@ static inline void mavlink_msg_mission_request_list_send(mavlink_channel_t chan, #endif } +#if MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mission_request_list_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_LIST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_LIST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN); +#endif +#else + mavlink_mission_request_list_t *packet = (mavlink_mission_request_list_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_LIST, (const char *)packet, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_LIST, (const char *)packet, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE MISSION_REQUEST_LIST UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_partial_list.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_partial_list.h index 35c7e12856..79a88dc084 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_partial_list.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_request_partial_list.h @@ -179,6 +179,44 @@ static inline void mavlink_msg_mission_request_partial_list_send(mavlink_channel #endif } +#if MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mission_request_partial_list_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int16_t start_index, int16_t end_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int16_t(buf, 0, start_index); + _mav_put_int16_t(buf, 2, end_index); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN); +#endif +#else + mavlink_mission_request_partial_list_t *packet = (mavlink_mission_request_partial_list_t *)msgbuf; + packet->start_index = start_index; + packet->end_index = end_index; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST, (const char *)packet, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST, (const char *)packet, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE MISSION_REQUEST_PARTIAL_LIST UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_set_current.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_set_current.h index 63b37cf8c8..0c2a3ada4e 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_set_current.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_set_current.h @@ -168,6 +168,42 @@ static inline void mavlink_msg_mission_set_current_send(mavlink_channel_t chan, #endif } +#if MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mission_set_current_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, seq); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_SET_CURRENT, buf, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_SET_CURRENT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_SET_CURRENT, buf, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN); +#endif +#else + mavlink_mission_set_current_t *packet = (mavlink_mission_set_current_t *)msgbuf; + packet->seq = seq; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_SET_CURRENT, (const char *)packet, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN, MAVLINK_MSG_ID_MISSION_SET_CURRENT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_SET_CURRENT, (const char *)packet, MAVLINK_MSG_ID_MISSION_SET_CURRENT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE MISSION_SET_CURRENT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_write_partial_list.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_write_partial_list.h index 7de38bd7b5..17a5a6bdd8 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_write_partial_list.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_mission_write_partial_list.h @@ -179,6 +179,44 @@ static inline void mavlink_msg_mission_write_partial_list_send(mavlink_channel_t #endif } +#if MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mission_write_partial_list_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int16_t start_index, int16_t end_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int16_t(buf, 0, start_index); + _mav_put_int16_t(buf, 2, end_index); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST, buf, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST, buf, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN); +#endif +#else + mavlink_mission_write_partial_list_t *packet = (mavlink_mission_write_partial_list_t *)msgbuf; + packet->start_index = start_index; + packet->end_index = end_index; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST, (const char *)packet, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST, (const char *)packet, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE MISSION_WRITE_PARTIAL_LIST UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_float.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_float.h index fbf4f75c95..df82704a4b 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_float.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_float.h @@ -162,6 +162,40 @@ static inline void mavlink_msg_named_value_float_send(mavlink_channel_t chan, ui #endif } +#if MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_named_value_float_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, const char *name, float value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, value); + _mav_put_char_array(buf, 8, name, 10); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, buf, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, buf, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN); +#endif +#else + mavlink_named_value_float_t *packet = (mavlink_named_value_float_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->value = value; + mav_array_memcpy(packet->name, name, sizeof(char)*10); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, (const char *)packet, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, (const char *)packet, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE NAMED_VALUE_FLOAT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_int.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_int.h index 052f247935..cfdd73d61e 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_int.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_named_value_int.h @@ -162,6 +162,40 @@ static inline void mavlink_msg_named_value_int_send(mavlink_channel_t chan, uint #endif } +#if MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_named_value_int_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, const char *name, int32_t value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int32_t(buf, 4, value); + _mav_put_char_array(buf, 8, name, 10); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_INT, buf, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN, MAVLINK_MSG_ID_NAMED_VALUE_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_INT, buf, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN); +#endif +#else + mavlink_named_value_int_t *packet = (mavlink_named_value_int_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->value = value; + mav_array_memcpy(packet->name, name, sizeof(char)*10); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_INT, (const char *)packet, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN, MAVLINK_MSG_ID_NAMED_VALUE_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_INT, (const char *)packet, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE NAMED_VALUE_INT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_nav_controller_output.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_nav_controller_output.h index e95c144de7..b9a748b222 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_nav_controller_output.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_nav_controller_output.h @@ -223,6 +223,52 @@ static inline void mavlink_msg_nav_controller_output_send(mavlink_channel_t chan #endif } +#if MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_nav_controller_output_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float nav_roll, float nav_pitch, int16_t nav_bearing, int16_t target_bearing, uint16_t wp_dist, float alt_error, float aspd_error, float xtrack_error) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, nav_roll); + _mav_put_float(buf, 4, nav_pitch); + _mav_put_float(buf, 8, alt_error); + _mav_put_float(buf, 12, aspd_error); + _mav_put_float(buf, 16, xtrack_error); + _mav_put_int16_t(buf, 20, nav_bearing); + _mav_put_int16_t(buf, 22, target_bearing); + _mav_put_uint16_t(buf, 24, wp_dist); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT, buf, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT, buf, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN); +#endif +#else + mavlink_nav_controller_output_t *packet = (mavlink_nav_controller_output_t *)msgbuf; + packet->nav_roll = nav_roll; + packet->nav_pitch = nav_pitch; + packet->alt_error = alt_error; + packet->aspd_error = aspd_error; + packet->xtrack_error = xtrack_error; + packet->nav_bearing = nav_bearing; + packet->target_bearing = target_bearing; + packet->wp_dist = wp_dist; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT, (const char *)packet, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT, (const char *)packet, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE NAV_CONTROLLER_OUTPUT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_omnidirectional_flow.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_omnidirectional_flow.h index 4debb6e663..4ee9c452fb 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_omnidirectional_flow.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_omnidirectional_flow.h @@ -196,6 +196,46 @@ static inline void mavlink_msg_omnidirectional_flow_send(mavlink_channel_t chan, #endif } +#if MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_omnidirectional_flow_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t sensor_id, const int16_t *left, const int16_t *right, uint8_t quality, float front_distance_m) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, front_distance_m); + _mav_put_uint8_t(buf, 52, sensor_id); + _mav_put_uint8_t(buf, 53, quality); + _mav_put_int16_t_array(buf, 12, left, 10); + _mav_put_int16_t_array(buf, 32, right, 10); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW, buf, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW, buf, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN); +#endif +#else + mavlink_omnidirectional_flow_t *packet = (mavlink_omnidirectional_flow_t *)msgbuf; + packet->time_usec = time_usec; + packet->front_distance_m = front_distance_m; + packet->sensor_id = sensor_id; + packet->quality = quality; + mav_array_memcpy(packet->left, left, sizeof(int16_t)*10); + mav_array_memcpy(packet->right, right, sizeof(int16_t)*10); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW, (const char *)packet, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW, (const char *)packet, MAVLINK_MSG_ID_OMNIDIRECTIONAL_FLOW_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE OMNIDIRECTIONAL_FLOW UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_optical_flow.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_optical_flow.h index cf6db9147e..6a2efc664f 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_optical_flow.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_optical_flow.h @@ -223,6 +223,52 @@ static inline void mavlink_msg_optical_flow_send(mavlink_channel_t chan, uint64_ #endif } +#if MAVLINK_MSG_ID_OPTICAL_FLOW_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_optical_flow_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t sensor_id, int16_t flow_x, int16_t flow_y, float flow_comp_m_x, float flow_comp_m_y, uint8_t quality, float ground_distance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, flow_comp_m_x); + _mav_put_float(buf, 12, flow_comp_m_y); + _mav_put_float(buf, 16, ground_distance); + _mav_put_int16_t(buf, 20, flow_x); + _mav_put_int16_t(buf, 22, flow_y); + _mav_put_uint8_t(buf, 24, sensor_id); + _mav_put_uint8_t(buf, 25, quality); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW, buf, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW, buf, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN); +#endif +#else + mavlink_optical_flow_t *packet = (mavlink_optical_flow_t *)msgbuf; + packet->time_usec = time_usec; + packet->flow_comp_m_x = flow_comp_m_x; + packet->flow_comp_m_y = flow_comp_m_y; + packet->ground_distance = ground_distance; + packet->flow_x = flow_x; + packet->flow_y = flow_y; + packet->sensor_id = sensor_id; + packet->quality = quality; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW, (const char *)packet, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW, (const char *)packet, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE OPTICAL_FLOW UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_list.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_list.h index 39e0072741..f9466b002e 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_list.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_list.h @@ -157,6 +157,40 @@ static inline void mavlink_msg_param_request_list_send(mavlink_channel_t chan, u #endif } +#if MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_param_request_list_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST, buf, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST, buf, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN); +#endif +#else + mavlink_param_request_list_t *packet = (mavlink_param_request_list_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST, (const char *)packet, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST, (const char *)packet, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE PARAM_REQUEST_LIST UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_read.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_read.h index 5d9113114b..730cff0669 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_read.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_request_read.h @@ -173,6 +173,42 @@ static inline void mavlink_msg_param_request_read_send(mavlink_channel_t chan, u #endif } +#if MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_param_request_read_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const char *param_id, int16_t param_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int16_t(buf, 0, param_index); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_char_array(buf, 4, param_id, 16); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ, buf, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_READ_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ, buf, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN); +#endif +#else + mavlink_param_request_read_t *packet = (mavlink_param_request_read_t *)msgbuf; + packet->param_index = param_index; + packet->target_system = target_system; + packet->target_component = target_component; + mav_array_memcpy(packet->param_id, param_id, sizeof(char)*16); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ, (const char *)packet, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN, MAVLINK_MSG_ID_PARAM_REQUEST_READ_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ, (const char *)packet, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE PARAM_REQUEST_READ UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_set.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_set.h index 1bd1f00596..f669af1a25 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_set.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_set.h @@ -184,6 +184,44 @@ static inline void mavlink_msg_param_set_send(mavlink_channel_t chan, uint8_t ta #endif } +#if MAVLINK_MSG_ID_PARAM_SET_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_param_set_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const char *param_id, float param_value, uint8_t param_type) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, param_value); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 22, param_type); + _mav_put_char_array(buf, 6, param_id, 16); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_SET, buf, MAVLINK_MSG_ID_PARAM_SET_LEN, MAVLINK_MSG_ID_PARAM_SET_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_SET, buf, MAVLINK_MSG_ID_PARAM_SET_LEN); +#endif +#else + mavlink_param_set_t *packet = (mavlink_param_set_t *)msgbuf; + packet->param_value = param_value; + packet->target_system = target_system; + packet->target_component = target_component; + packet->param_type = param_type; + mav_array_memcpy(packet->param_id, param_id, sizeof(char)*16); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_SET, (const char *)packet, MAVLINK_MSG_ID_PARAM_SET_LEN, MAVLINK_MSG_ID_PARAM_SET_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_SET, (const char *)packet, MAVLINK_MSG_ID_PARAM_SET_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE PARAM_SET UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_value.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_value.h index 17c759811f..c27957913f 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_value.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_param_value.h @@ -184,6 +184,44 @@ static inline void mavlink_msg_param_value_send(mavlink_channel_t chan, const ch #endif } +#if MAVLINK_MSG_ID_PARAM_VALUE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_param_value_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, const char *param_id, float param_value, uint8_t param_type, uint16_t param_count, uint16_t param_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, param_value); + _mav_put_uint16_t(buf, 4, param_count); + _mav_put_uint16_t(buf, 6, param_index); + _mav_put_uint8_t(buf, 24, param_type); + _mav_put_char_array(buf, 8, param_id, 16); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_VALUE, buf, MAVLINK_MSG_ID_PARAM_VALUE_LEN, MAVLINK_MSG_ID_PARAM_VALUE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_VALUE, buf, MAVLINK_MSG_ID_PARAM_VALUE_LEN); +#endif +#else + mavlink_param_value_t *packet = (mavlink_param_value_t *)msgbuf; + packet->param_value = param_value; + packet->param_count = param_count; + packet->param_index = param_index; + packet->param_type = param_type; + mav_array_memcpy(packet->param_id, param_id, sizeof(char)*16); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_VALUE, (const char *)packet, MAVLINK_MSG_ID_PARAM_VALUE_LEN, MAVLINK_MSG_ID_PARAM_VALUE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_VALUE, (const char *)packet, MAVLINK_MSG_ID_PARAM_VALUE_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE PARAM_VALUE UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_ping.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_ping.h index 0c68ca1765..b1501ba1f8 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_ping.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_ping.h @@ -179,6 +179,44 @@ static inline void mavlink_msg_ping_send(mavlink_channel_t chan, uint64_t time_u #endif } +#if MAVLINK_MSG_ID_PING_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_ping_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint32_t seq, uint8_t target_system, uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, seq); + _mav_put_uint8_t(buf, 12, target_system); + _mav_put_uint8_t(buf, 13, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PING, buf, MAVLINK_MSG_ID_PING_LEN, MAVLINK_MSG_ID_PING_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PING, buf, MAVLINK_MSG_ID_PING_LEN); +#endif +#else + mavlink_ping_t *packet = (mavlink_ping_t *)msgbuf; + packet->time_usec = time_usec; + packet->seq = seq; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PING, (const char *)packet, MAVLINK_MSG_ID_PING_LEN, MAVLINK_MSG_ID_PING_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PING, (const char *)packet, MAVLINK_MSG_ID_PING_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE PING UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_power_status.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_power_status.h new file mode 100644 index 0000000000..1a6259aa3d --- /dev/null +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_power_status.h @@ -0,0 +1,257 @@ +// MESSAGE POWER_STATUS PACKING + +#define MAVLINK_MSG_ID_POWER_STATUS 125 + +typedef struct __mavlink_power_status_t +{ + uint16_t Vcc; ///< 5V rail voltage in millivolts + uint16_t Vservo; ///< servo rail voltage in millivolts + uint16_t flags; ///< power supply status flags (see MAV_POWER_STATUS enum) +} mavlink_power_status_t; + +#define MAVLINK_MSG_ID_POWER_STATUS_LEN 6 +#define MAVLINK_MSG_ID_125_LEN 6 + +#define MAVLINK_MSG_ID_POWER_STATUS_CRC 203 +#define MAVLINK_MSG_ID_125_CRC 203 + + + +#define MAVLINK_MESSAGE_INFO_POWER_STATUS { \ + "POWER_STATUS", \ + 3, \ + { { "Vcc", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_power_status_t, Vcc) }, \ + { "Vservo", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_power_status_t, Vservo) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_power_status_t, flags) }, \ + } \ +} + + +/** + * @brief Pack a power_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param Vcc 5V rail voltage in millivolts + * @param Vservo servo rail voltage in millivolts + * @param flags power supply status flags (see MAV_POWER_STATUS enum) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_power_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint16_t Vcc, uint16_t Vservo, uint16_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_POWER_STATUS_LEN]; + _mav_put_uint16_t(buf, 0, Vcc); + _mav_put_uint16_t(buf, 2, Vservo); + _mav_put_uint16_t(buf, 4, flags); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_POWER_STATUS_LEN); +#else + mavlink_power_status_t packet; + packet.Vcc = Vcc; + packet.Vservo = Vservo; + packet.flags = flags; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_POWER_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_POWER_STATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POWER_STATUS_LEN, MAVLINK_MSG_ID_POWER_STATUS_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POWER_STATUS_LEN); +#endif +} + +/** + * @brief Pack a power_status message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param Vcc 5V rail voltage in millivolts + * @param Vservo servo rail voltage in millivolts + * @param flags power supply status flags (see MAV_POWER_STATUS enum) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_power_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint16_t Vcc,uint16_t Vservo,uint16_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_POWER_STATUS_LEN]; + _mav_put_uint16_t(buf, 0, Vcc); + _mav_put_uint16_t(buf, 2, Vservo); + _mav_put_uint16_t(buf, 4, flags); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_POWER_STATUS_LEN); +#else + mavlink_power_status_t packet; + packet.Vcc = Vcc; + packet.Vservo = Vservo; + packet.flags = flags; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_POWER_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_POWER_STATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POWER_STATUS_LEN, MAVLINK_MSG_ID_POWER_STATUS_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POWER_STATUS_LEN); +#endif +} + +/** + * @brief Encode a power_status struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param power_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_power_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_power_status_t* power_status) +{ + return mavlink_msg_power_status_pack(system_id, component_id, msg, power_status->Vcc, power_status->Vservo, power_status->flags); +} + +/** + * @brief Encode a power_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param power_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_power_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_power_status_t* power_status) +{ + return mavlink_msg_power_status_pack_chan(system_id, component_id, chan, msg, power_status->Vcc, power_status->Vservo, power_status->flags); +} + +/** + * @brief Send a power_status message + * @param chan MAVLink channel to send the message + * + * @param Vcc 5V rail voltage in millivolts + * @param Vservo servo rail voltage in millivolts + * @param flags power supply status flags (see MAV_POWER_STATUS enum) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_power_status_send(mavlink_channel_t chan, uint16_t Vcc, uint16_t Vservo, uint16_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_POWER_STATUS_LEN]; + _mav_put_uint16_t(buf, 0, Vcc); + _mav_put_uint16_t(buf, 2, Vservo); + _mav_put_uint16_t(buf, 4, flags); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POWER_STATUS, buf, MAVLINK_MSG_ID_POWER_STATUS_LEN, MAVLINK_MSG_ID_POWER_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POWER_STATUS, buf, MAVLINK_MSG_ID_POWER_STATUS_LEN); +#endif +#else + mavlink_power_status_t packet; + packet.Vcc = Vcc; + packet.Vservo = Vservo; + packet.flags = flags; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POWER_STATUS, (const char *)&packet, MAVLINK_MSG_ID_POWER_STATUS_LEN, MAVLINK_MSG_ID_POWER_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POWER_STATUS, (const char *)&packet, MAVLINK_MSG_ID_POWER_STATUS_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_POWER_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_power_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t Vcc, uint16_t Vservo, uint16_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, Vcc); + _mav_put_uint16_t(buf, 2, Vservo); + _mav_put_uint16_t(buf, 4, flags); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POWER_STATUS, buf, MAVLINK_MSG_ID_POWER_STATUS_LEN, MAVLINK_MSG_ID_POWER_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POWER_STATUS, buf, MAVLINK_MSG_ID_POWER_STATUS_LEN); +#endif +#else + mavlink_power_status_t *packet = (mavlink_power_status_t *)msgbuf; + packet->Vcc = Vcc; + packet->Vservo = Vservo; + packet->flags = flags; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POWER_STATUS, (const char *)packet, MAVLINK_MSG_ID_POWER_STATUS_LEN, MAVLINK_MSG_ID_POWER_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POWER_STATUS, (const char *)packet, MAVLINK_MSG_ID_POWER_STATUS_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE POWER_STATUS UNPACKING + + +/** + * @brief Get field Vcc from power_status message + * + * @return 5V rail voltage in millivolts + */ +static inline uint16_t mavlink_msg_power_status_get_Vcc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 0); +} + +/** + * @brief Get field Vservo from power_status message + * + * @return servo rail voltage in millivolts + */ +static inline uint16_t mavlink_msg_power_status_get_Vservo(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 2); +} + +/** + * @brief Get field flags from power_status message + * + * @return power supply status flags (see MAV_POWER_STATUS enum) + */ +static inline uint16_t mavlink_msg_power_status_get_flags(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 4); +} + +/** + * @brief Decode a power_status message into a struct + * + * @param msg The message to decode + * @param power_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_power_status_decode(const mavlink_message_t* msg, mavlink_power_status_t* power_status) +{ +#if MAVLINK_NEED_BYTE_SWAP + power_status->Vcc = mavlink_msg_power_status_get_Vcc(msg); + power_status->Vservo = mavlink_msg_power_status_get_Vservo(msg); + power_status->flags = mavlink_msg_power_status_get_flags(msg); +#else + memcpy(power_status, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_POWER_STATUS_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_radio_status.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_radio_status.h index fceb7d1688..5763008fe2 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_radio_status.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_radio_status.h @@ -212,6 +212,50 @@ static inline void mavlink_msg_radio_status_send(mavlink_channel_t chan, uint8_t #endif } +#if MAVLINK_MSG_ID_RADIO_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_radio_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t rssi, uint8_t remrssi, uint8_t txbuf, uint8_t noise, uint8_t remnoise, uint16_t rxerrors, uint16_t fixed) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, rxerrors); + _mav_put_uint16_t(buf, 2, fixed); + _mav_put_uint8_t(buf, 4, rssi); + _mav_put_uint8_t(buf, 5, remrssi); + _mav_put_uint8_t(buf, 6, txbuf); + _mav_put_uint8_t(buf, 7, noise); + _mav_put_uint8_t(buf, 8, remnoise); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO_STATUS, buf, MAVLINK_MSG_ID_RADIO_STATUS_LEN, MAVLINK_MSG_ID_RADIO_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO_STATUS, buf, MAVLINK_MSG_ID_RADIO_STATUS_LEN); +#endif +#else + mavlink_radio_status_t *packet = (mavlink_radio_status_t *)msgbuf; + packet->rxerrors = rxerrors; + packet->fixed = fixed; + packet->rssi = rssi; + packet->remrssi = remrssi; + packet->txbuf = txbuf; + packet->noise = noise; + packet->remnoise = remnoise; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO_STATUS, (const char *)packet, MAVLINK_MSG_ID_RADIO_STATUS_LEN, MAVLINK_MSG_ID_RADIO_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO_STATUS, (const char *)packet, MAVLINK_MSG_ID_RADIO_STATUS_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE RADIO_STATUS UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_imu.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_imu.h index 62a9b6eeae..a98e8ce721 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_imu.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_imu.h @@ -245,6 +245,56 @@ static inline void mavlink_msg_raw_imu_send(mavlink_channel_t chan, uint64_t tim #endif } +#if MAVLINK_MSG_ID_RAW_IMU_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_raw_imu_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int16_t(buf, 8, xacc); + _mav_put_int16_t(buf, 10, yacc); + _mav_put_int16_t(buf, 12, zacc); + _mav_put_int16_t(buf, 14, xgyro); + _mav_put_int16_t(buf, 16, ygyro); + _mav_put_int16_t(buf, 18, zgyro); + _mav_put_int16_t(buf, 20, xmag); + _mav_put_int16_t(buf, 22, ymag); + _mav_put_int16_t(buf, 24, zmag); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_IMU, buf, MAVLINK_MSG_ID_RAW_IMU_LEN, MAVLINK_MSG_ID_RAW_IMU_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_IMU, buf, MAVLINK_MSG_ID_RAW_IMU_LEN); +#endif +#else + mavlink_raw_imu_t *packet = (mavlink_raw_imu_t *)msgbuf; + packet->time_usec = time_usec; + packet->xacc = xacc; + packet->yacc = yacc; + packet->zacc = zacc; + packet->xgyro = xgyro; + packet->ygyro = ygyro; + packet->zgyro = zgyro; + packet->xmag = xmag; + packet->ymag = ymag; + packet->zmag = zmag; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_IMU, (const char *)packet, MAVLINK_MSG_ID_RAW_IMU_LEN, MAVLINK_MSG_ID_RAW_IMU_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_IMU, (const char *)packet, MAVLINK_MSG_ID_RAW_IMU_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE RAW_IMU UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_pressure.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_pressure.h index 82c5fad4ab..6bd37fcaee 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_pressure.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_raw_pressure.h @@ -190,6 +190,46 @@ static inline void mavlink_msg_raw_pressure_send(mavlink_channel_t chan, uint64_ #endif } +#if MAVLINK_MSG_ID_RAW_PRESSURE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_raw_pressure_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, int16_t press_abs, int16_t press_diff1, int16_t press_diff2, int16_t temperature) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int16_t(buf, 8, press_abs); + _mav_put_int16_t(buf, 10, press_diff1); + _mav_put_int16_t(buf, 12, press_diff2); + _mav_put_int16_t(buf, 14, temperature); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_PRESSURE, buf, MAVLINK_MSG_ID_RAW_PRESSURE_LEN, MAVLINK_MSG_ID_RAW_PRESSURE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_PRESSURE, buf, MAVLINK_MSG_ID_RAW_PRESSURE_LEN); +#endif +#else + mavlink_raw_pressure_t *packet = (mavlink_raw_pressure_t *)msgbuf; + packet->time_usec = time_usec; + packet->press_abs = press_abs; + packet->press_diff1 = press_diff1; + packet->press_diff2 = press_diff2; + packet->temperature = temperature; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_PRESSURE, (const char *)packet, MAVLINK_MSG_ID_RAW_PRESSURE_LEN, MAVLINK_MSG_ID_RAW_PRESSURE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_PRESSURE, (const char *)packet, MAVLINK_MSG_ID_RAW_PRESSURE_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE RAW_PRESSURE UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels.h new file mode 100644 index 0000000000..479af122ff --- /dev/null +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels.h @@ -0,0 +1,689 @@ +// MESSAGE RC_CHANNELS PACKING + +#define MAVLINK_MSG_ID_RC_CHANNELS 65 + +typedef struct __mavlink_rc_channels_t +{ + uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot) + uint16_t chan1_raw; ///< RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan2_raw; ///< RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan3_raw; ///< RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan4_raw; ///< RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan5_raw; ///< RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan6_raw; ///< RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan7_raw; ///< RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan8_raw; ///< RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan9_raw; ///< RC channel 9 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan10_raw; ///< RC channel 10 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan11_raw; ///< RC channel 11 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan12_raw; ///< RC channel 12 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan13_raw; ///< RC channel 13 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan14_raw; ///< RC channel 14 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan15_raw; ///< RC channel 15 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan16_raw; ///< RC channel 16 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan17_raw; ///< RC channel 17 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint16_t chan18_raw; ///< RC channel 18 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + uint8_t chancount; ///< Total number of RC channels being received. This can be larger than 18, indicating that more channels are available but not given in this message. This value should be 0 when no RC channels are available. + uint8_t rssi; ///< Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. +} mavlink_rc_channels_t; + +#define MAVLINK_MSG_ID_RC_CHANNELS_LEN 42 +#define MAVLINK_MSG_ID_65_LEN 42 + +#define MAVLINK_MSG_ID_RC_CHANNELS_CRC 118 +#define MAVLINK_MSG_ID_65_CRC 118 + + + +#define MAVLINK_MESSAGE_INFO_RC_CHANNELS { \ + "RC_CHANNELS", \ + 21, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_rc_channels_t, time_boot_ms) }, \ + { "chan1_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_rc_channels_t, chan1_raw) }, \ + { "chan2_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_rc_channels_t, chan2_raw) }, \ + { "chan3_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_rc_channels_t, chan3_raw) }, \ + { "chan4_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_rc_channels_t, chan4_raw) }, \ + { "chan5_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_rc_channels_t, chan5_raw) }, \ + { "chan6_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_rc_channels_t, chan6_raw) }, \ + { "chan7_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_rc_channels_t, chan7_raw) }, \ + { "chan8_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_rc_channels_t, chan8_raw) }, \ + { "chan9_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 20, offsetof(mavlink_rc_channels_t, chan9_raw) }, \ + { "chan10_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 22, offsetof(mavlink_rc_channels_t, chan10_raw) }, \ + { "chan11_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_rc_channels_t, chan11_raw) }, \ + { "chan12_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 26, offsetof(mavlink_rc_channels_t, chan12_raw) }, \ + { "chan13_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_rc_channels_t, chan13_raw) }, \ + { "chan14_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 30, offsetof(mavlink_rc_channels_t, chan14_raw) }, \ + { "chan15_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 32, offsetof(mavlink_rc_channels_t, chan15_raw) }, \ + { "chan16_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 34, offsetof(mavlink_rc_channels_t, chan16_raw) }, \ + { "chan17_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 36, offsetof(mavlink_rc_channels_t, chan17_raw) }, \ + { "chan18_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 38, offsetof(mavlink_rc_channels_t, chan18_raw) }, \ + { "chancount", NULL, MAVLINK_TYPE_UINT8_T, 0, 40, offsetof(mavlink_rc_channels_t, chancount) }, \ + { "rssi", NULL, MAVLINK_TYPE_UINT8_T, 0, 41, offsetof(mavlink_rc_channels_t, rssi) }, \ + } \ +} + + +/** + * @brief Pack a rc_channels message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param chancount Total number of RC channels being received. This can be larger than 18, indicating that more channels are available but not given in this message. This value should be 0 when no RC channels are available. + * @param chan1_raw RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan2_raw RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan3_raw RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan4_raw RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan5_raw RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan6_raw RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan7_raw RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan8_raw RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan9_raw RC channel 9 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan10_raw RC channel 10 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan11_raw RC channel 11 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan12_raw RC channel 12 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan13_raw RC channel 13 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan14_raw RC channel 14 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan15_raw RC channel 15 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan16_raw RC channel 16 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan17_raw RC channel 17 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan18_raw RC channel 18 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param rssi Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_rc_channels_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, uint8_t chancount, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint16_t chan9_raw, uint16_t chan10_raw, uint16_t chan11_raw, uint16_t chan12_raw, uint16_t chan13_raw, uint16_t chan14_raw, uint16_t chan15_raw, uint16_t chan16_raw, uint16_t chan17_raw, uint16_t chan18_raw, uint8_t rssi) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RC_CHANNELS_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint16_t(buf, 4, chan1_raw); + _mav_put_uint16_t(buf, 6, chan2_raw); + _mav_put_uint16_t(buf, 8, chan3_raw); + _mav_put_uint16_t(buf, 10, chan4_raw); + _mav_put_uint16_t(buf, 12, chan5_raw); + _mav_put_uint16_t(buf, 14, chan6_raw); + _mav_put_uint16_t(buf, 16, chan7_raw); + _mav_put_uint16_t(buf, 18, chan8_raw); + _mav_put_uint16_t(buf, 20, chan9_raw); + _mav_put_uint16_t(buf, 22, chan10_raw); + _mav_put_uint16_t(buf, 24, chan11_raw); + _mav_put_uint16_t(buf, 26, chan12_raw); + _mav_put_uint16_t(buf, 28, chan13_raw); + _mav_put_uint16_t(buf, 30, chan14_raw); + _mav_put_uint16_t(buf, 32, chan15_raw); + _mav_put_uint16_t(buf, 34, chan16_raw); + _mav_put_uint16_t(buf, 36, chan17_raw); + _mav_put_uint16_t(buf, 38, chan18_raw); + _mav_put_uint8_t(buf, 40, chancount); + _mav_put_uint8_t(buf, 41, rssi); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RC_CHANNELS_LEN); +#else + mavlink_rc_channels_t packet; + packet.time_boot_ms = time_boot_ms; + packet.chan1_raw = chan1_raw; + packet.chan2_raw = chan2_raw; + packet.chan3_raw = chan3_raw; + packet.chan4_raw = chan4_raw; + packet.chan5_raw = chan5_raw; + packet.chan6_raw = chan6_raw; + packet.chan7_raw = chan7_raw; + packet.chan8_raw = chan8_raw; + packet.chan9_raw = chan9_raw; + packet.chan10_raw = chan10_raw; + packet.chan11_raw = chan11_raw; + packet.chan12_raw = chan12_raw; + packet.chan13_raw = chan13_raw; + packet.chan14_raw = chan14_raw; + packet.chan15_raw = chan15_raw; + packet.chan16_raw = chan16_raw; + packet.chan17_raw = chan17_raw; + packet.chan18_raw = chan18_raw; + packet.chancount = chancount; + packet.rssi = rssi; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RC_CHANNELS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RC_CHANNELS_LEN, MAVLINK_MSG_ID_RC_CHANNELS_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RC_CHANNELS_LEN); +#endif +} + +/** + * @brief Pack a rc_channels message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param chancount Total number of RC channels being received. This can be larger than 18, indicating that more channels are available but not given in this message. This value should be 0 when no RC channels are available. + * @param chan1_raw RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan2_raw RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan3_raw RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan4_raw RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan5_raw RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan6_raw RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan7_raw RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan8_raw RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan9_raw RC channel 9 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan10_raw RC channel 10 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan11_raw RC channel 11 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan12_raw RC channel 12 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan13_raw RC channel 13 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan14_raw RC channel 14 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan15_raw RC channel 15 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan16_raw RC channel 16 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan17_raw RC channel 17 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan18_raw RC channel 18 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param rssi Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_rc_channels_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,uint8_t chancount,uint16_t chan1_raw,uint16_t chan2_raw,uint16_t chan3_raw,uint16_t chan4_raw,uint16_t chan5_raw,uint16_t chan6_raw,uint16_t chan7_raw,uint16_t chan8_raw,uint16_t chan9_raw,uint16_t chan10_raw,uint16_t chan11_raw,uint16_t chan12_raw,uint16_t chan13_raw,uint16_t chan14_raw,uint16_t chan15_raw,uint16_t chan16_raw,uint16_t chan17_raw,uint16_t chan18_raw,uint8_t rssi) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RC_CHANNELS_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint16_t(buf, 4, chan1_raw); + _mav_put_uint16_t(buf, 6, chan2_raw); + _mav_put_uint16_t(buf, 8, chan3_raw); + _mav_put_uint16_t(buf, 10, chan4_raw); + _mav_put_uint16_t(buf, 12, chan5_raw); + _mav_put_uint16_t(buf, 14, chan6_raw); + _mav_put_uint16_t(buf, 16, chan7_raw); + _mav_put_uint16_t(buf, 18, chan8_raw); + _mav_put_uint16_t(buf, 20, chan9_raw); + _mav_put_uint16_t(buf, 22, chan10_raw); + _mav_put_uint16_t(buf, 24, chan11_raw); + _mav_put_uint16_t(buf, 26, chan12_raw); + _mav_put_uint16_t(buf, 28, chan13_raw); + _mav_put_uint16_t(buf, 30, chan14_raw); + _mav_put_uint16_t(buf, 32, chan15_raw); + _mav_put_uint16_t(buf, 34, chan16_raw); + _mav_put_uint16_t(buf, 36, chan17_raw); + _mav_put_uint16_t(buf, 38, chan18_raw); + _mav_put_uint8_t(buf, 40, chancount); + _mav_put_uint8_t(buf, 41, rssi); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RC_CHANNELS_LEN); +#else + mavlink_rc_channels_t packet; + packet.time_boot_ms = time_boot_ms; + packet.chan1_raw = chan1_raw; + packet.chan2_raw = chan2_raw; + packet.chan3_raw = chan3_raw; + packet.chan4_raw = chan4_raw; + packet.chan5_raw = chan5_raw; + packet.chan6_raw = chan6_raw; + packet.chan7_raw = chan7_raw; + packet.chan8_raw = chan8_raw; + packet.chan9_raw = chan9_raw; + packet.chan10_raw = chan10_raw; + packet.chan11_raw = chan11_raw; + packet.chan12_raw = chan12_raw; + packet.chan13_raw = chan13_raw; + packet.chan14_raw = chan14_raw; + packet.chan15_raw = chan15_raw; + packet.chan16_raw = chan16_raw; + packet.chan17_raw = chan17_raw; + packet.chan18_raw = chan18_raw; + packet.chancount = chancount; + packet.rssi = rssi; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RC_CHANNELS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RC_CHANNELS_LEN, MAVLINK_MSG_ID_RC_CHANNELS_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RC_CHANNELS_LEN); +#endif +} + +/** + * @brief Encode a rc_channels struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param rc_channels C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_rc_channels_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_rc_channels_t* rc_channels) +{ + return mavlink_msg_rc_channels_pack(system_id, component_id, msg, rc_channels->time_boot_ms, rc_channels->chancount, rc_channels->chan1_raw, rc_channels->chan2_raw, rc_channels->chan3_raw, rc_channels->chan4_raw, rc_channels->chan5_raw, rc_channels->chan6_raw, rc_channels->chan7_raw, rc_channels->chan8_raw, rc_channels->chan9_raw, rc_channels->chan10_raw, rc_channels->chan11_raw, rc_channels->chan12_raw, rc_channels->chan13_raw, rc_channels->chan14_raw, rc_channels->chan15_raw, rc_channels->chan16_raw, rc_channels->chan17_raw, rc_channels->chan18_raw, rc_channels->rssi); +} + +/** + * @brief Encode a rc_channels struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param rc_channels C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_rc_channels_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_rc_channels_t* rc_channels) +{ + return mavlink_msg_rc_channels_pack_chan(system_id, component_id, chan, msg, rc_channels->time_boot_ms, rc_channels->chancount, rc_channels->chan1_raw, rc_channels->chan2_raw, rc_channels->chan3_raw, rc_channels->chan4_raw, rc_channels->chan5_raw, rc_channels->chan6_raw, rc_channels->chan7_raw, rc_channels->chan8_raw, rc_channels->chan9_raw, rc_channels->chan10_raw, rc_channels->chan11_raw, rc_channels->chan12_raw, rc_channels->chan13_raw, rc_channels->chan14_raw, rc_channels->chan15_raw, rc_channels->chan16_raw, rc_channels->chan17_raw, rc_channels->chan18_raw, rc_channels->rssi); +} + +/** + * @brief Send a rc_channels message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms Timestamp (milliseconds since system boot) + * @param chancount Total number of RC channels being received. This can be larger than 18, indicating that more channels are available but not given in this message. This value should be 0 when no RC channels are available. + * @param chan1_raw RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan2_raw RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan3_raw RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan4_raw RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan5_raw RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan6_raw RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan7_raw RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan8_raw RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan9_raw RC channel 9 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan10_raw RC channel 10 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan11_raw RC channel 11 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan12_raw RC channel 12 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan13_raw RC channel 13 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan14_raw RC channel 14 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan15_raw RC channel 15 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan16_raw RC channel 16 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan17_raw RC channel 17 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param chan18_raw RC channel 18 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + * @param rssi Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_rc_channels_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t chancount, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint16_t chan9_raw, uint16_t chan10_raw, uint16_t chan11_raw, uint16_t chan12_raw, uint16_t chan13_raw, uint16_t chan14_raw, uint16_t chan15_raw, uint16_t chan16_raw, uint16_t chan17_raw, uint16_t chan18_raw, uint8_t rssi) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RC_CHANNELS_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint16_t(buf, 4, chan1_raw); + _mav_put_uint16_t(buf, 6, chan2_raw); + _mav_put_uint16_t(buf, 8, chan3_raw); + _mav_put_uint16_t(buf, 10, chan4_raw); + _mav_put_uint16_t(buf, 12, chan5_raw); + _mav_put_uint16_t(buf, 14, chan6_raw); + _mav_put_uint16_t(buf, 16, chan7_raw); + _mav_put_uint16_t(buf, 18, chan8_raw); + _mav_put_uint16_t(buf, 20, chan9_raw); + _mav_put_uint16_t(buf, 22, chan10_raw); + _mav_put_uint16_t(buf, 24, chan11_raw); + _mav_put_uint16_t(buf, 26, chan12_raw); + _mav_put_uint16_t(buf, 28, chan13_raw); + _mav_put_uint16_t(buf, 30, chan14_raw); + _mav_put_uint16_t(buf, 32, chan15_raw); + _mav_put_uint16_t(buf, 34, chan16_raw); + _mav_put_uint16_t(buf, 36, chan17_raw); + _mav_put_uint16_t(buf, 38, chan18_raw); + _mav_put_uint8_t(buf, 40, chancount); + _mav_put_uint8_t(buf, 41, rssi); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS, buf, MAVLINK_MSG_ID_RC_CHANNELS_LEN, MAVLINK_MSG_ID_RC_CHANNELS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS, buf, MAVLINK_MSG_ID_RC_CHANNELS_LEN); +#endif +#else + mavlink_rc_channels_t packet; + packet.time_boot_ms = time_boot_ms; + packet.chan1_raw = chan1_raw; + packet.chan2_raw = chan2_raw; + packet.chan3_raw = chan3_raw; + packet.chan4_raw = chan4_raw; + packet.chan5_raw = chan5_raw; + packet.chan6_raw = chan6_raw; + packet.chan7_raw = chan7_raw; + packet.chan8_raw = chan8_raw; + packet.chan9_raw = chan9_raw; + packet.chan10_raw = chan10_raw; + packet.chan11_raw = chan11_raw; + packet.chan12_raw = chan12_raw; + packet.chan13_raw = chan13_raw; + packet.chan14_raw = chan14_raw; + packet.chan15_raw = chan15_raw; + packet.chan16_raw = chan16_raw; + packet.chan17_raw = chan17_raw; + packet.chan18_raw = chan18_raw; + packet.chancount = chancount; + packet.rssi = rssi; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS, (const char *)&packet, MAVLINK_MSG_ID_RC_CHANNELS_LEN, MAVLINK_MSG_ID_RC_CHANNELS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS, (const char *)&packet, MAVLINK_MSG_ID_RC_CHANNELS_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_RC_CHANNELS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_rc_channels_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t chancount, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint16_t chan9_raw, uint16_t chan10_raw, uint16_t chan11_raw, uint16_t chan12_raw, uint16_t chan13_raw, uint16_t chan14_raw, uint16_t chan15_raw, uint16_t chan16_raw, uint16_t chan17_raw, uint16_t chan18_raw, uint8_t rssi) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint16_t(buf, 4, chan1_raw); + _mav_put_uint16_t(buf, 6, chan2_raw); + _mav_put_uint16_t(buf, 8, chan3_raw); + _mav_put_uint16_t(buf, 10, chan4_raw); + _mav_put_uint16_t(buf, 12, chan5_raw); + _mav_put_uint16_t(buf, 14, chan6_raw); + _mav_put_uint16_t(buf, 16, chan7_raw); + _mav_put_uint16_t(buf, 18, chan8_raw); + _mav_put_uint16_t(buf, 20, chan9_raw); + _mav_put_uint16_t(buf, 22, chan10_raw); + _mav_put_uint16_t(buf, 24, chan11_raw); + _mav_put_uint16_t(buf, 26, chan12_raw); + _mav_put_uint16_t(buf, 28, chan13_raw); + _mav_put_uint16_t(buf, 30, chan14_raw); + _mav_put_uint16_t(buf, 32, chan15_raw); + _mav_put_uint16_t(buf, 34, chan16_raw); + _mav_put_uint16_t(buf, 36, chan17_raw); + _mav_put_uint16_t(buf, 38, chan18_raw); + _mav_put_uint8_t(buf, 40, chancount); + _mav_put_uint8_t(buf, 41, rssi); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS, buf, MAVLINK_MSG_ID_RC_CHANNELS_LEN, MAVLINK_MSG_ID_RC_CHANNELS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS, buf, MAVLINK_MSG_ID_RC_CHANNELS_LEN); +#endif +#else + mavlink_rc_channels_t *packet = (mavlink_rc_channels_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->chan1_raw = chan1_raw; + packet->chan2_raw = chan2_raw; + packet->chan3_raw = chan3_raw; + packet->chan4_raw = chan4_raw; + packet->chan5_raw = chan5_raw; + packet->chan6_raw = chan6_raw; + packet->chan7_raw = chan7_raw; + packet->chan8_raw = chan8_raw; + packet->chan9_raw = chan9_raw; + packet->chan10_raw = chan10_raw; + packet->chan11_raw = chan11_raw; + packet->chan12_raw = chan12_raw; + packet->chan13_raw = chan13_raw; + packet->chan14_raw = chan14_raw; + packet->chan15_raw = chan15_raw; + packet->chan16_raw = chan16_raw; + packet->chan17_raw = chan17_raw; + packet->chan18_raw = chan18_raw; + packet->chancount = chancount; + packet->rssi = rssi; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS, (const char *)packet, MAVLINK_MSG_ID_RC_CHANNELS_LEN, MAVLINK_MSG_ID_RC_CHANNELS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS, (const char *)packet, MAVLINK_MSG_ID_RC_CHANNELS_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE RC_CHANNELS UNPACKING + + +/** + * @brief Get field time_boot_ms from rc_channels message + * + * @return Timestamp (milliseconds since system boot) + */ +static inline uint32_t mavlink_msg_rc_channels_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field chancount from rc_channels message + * + * @return Total number of RC channels being received. This can be larger than 18, indicating that more channels are available but not given in this message. This value should be 0 when no RC channels are available. + */ +static inline uint8_t mavlink_msg_rc_channels_get_chancount(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 40); +} + +/** + * @brief Get field chan1_raw from rc_channels message + * + * @return RC channel 1 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan1_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 4); +} + +/** + * @brief Get field chan2_raw from rc_channels message + * + * @return RC channel 2 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan2_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 6); +} + +/** + * @brief Get field chan3_raw from rc_channels message + * + * @return RC channel 3 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan3_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 8); +} + +/** + * @brief Get field chan4_raw from rc_channels message + * + * @return RC channel 4 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan4_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 10); +} + +/** + * @brief Get field chan5_raw from rc_channels message + * + * @return RC channel 5 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan5_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 12); +} + +/** + * @brief Get field chan6_raw from rc_channels message + * + * @return RC channel 6 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan6_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 14); +} + +/** + * @brief Get field chan7_raw from rc_channels message + * + * @return RC channel 7 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan7_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 16); +} + +/** + * @brief Get field chan8_raw from rc_channels message + * + * @return RC channel 8 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan8_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 18); +} + +/** + * @brief Get field chan9_raw from rc_channels message + * + * @return RC channel 9 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan9_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 20); +} + +/** + * @brief Get field chan10_raw from rc_channels message + * + * @return RC channel 10 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan10_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 22); +} + +/** + * @brief Get field chan11_raw from rc_channels message + * + * @return RC channel 11 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan11_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 24); +} + +/** + * @brief Get field chan12_raw from rc_channels message + * + * @return RC channel 12 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan12_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 26); +} + +/** + * @brief Get field chan13_raw from rc_channels message + * + * @return RC channel 13 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan13_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 28); +} + +/** + * @brief Get field chan14_raw from rc_channels message + * + * @return RC channel 14 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan14_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 30); +} + +/** + * @brief Get field chan15_raw from rc_channels message + * + * @return RC channel 15 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan15_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 32); +} + +/** + * @brief Get field chan16_raw from rc_channels message + * + * @return RC channel 16 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan16_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 34); +} + +/** + * @brief Get field chan17_raw from rc_channels message + * + * @return RC channel 17 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan17_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 36); +} + +/** + * @brief Get field chan18_raw from rc_channels message + * + * @return RC channel 18 value, in microseconds. A value of UINT16_MAX implies the channel is unused. + */ +static inline uint16_t mavlink_msg_rc_channels_get_chan18_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 38); +} + +/** + * @brief Get field rssi from rc_channels message + * + * @return Receive signal strength indicator, 0: 0%, 100: 100%, 255: invalid/unknown. + */ +static inline uint8_t mavlink_msg_rc_channels_get_rssi(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 41); +} + +/** + * @brief Decode a rc_channels message into a struct + * + * @param msg The message to decode + * @param rc_channels C-struct to decode the message contents into + */ +static inline void mavlink_msg_rc_channels_decode(const mavlink_message_t* msg, mavlink_rc_channels_t* rc_channels) +{ +#if MAVLINK_NEED_BYTE_SWAP + rc_channels->time_boot_ms = mavlink_msg_rc_channels_get_time_boot_ms(msg); + rc_channels->chan1_raw = mavlink_msg_rc_channels_get_chan1_raw(msg); + rc_channels->chan2_raw = mavlink_msg_rc_channels_get_chan2_raw(msg); + rc_channels->chan3_raw = mavlink_msg_rc_channels_get_chan3_raw(msg); + rc_channels->chan4_raw = mavlink_msg_rc_channels_get_chan4_raw(msg); + rc_channels->chan5_raw = mavlink_msg_rc_channels_get_chan5_raw(msg); + rc_channels->chan6_raw = mavlink_msg_rc_channels_get_chan6_raw(msg); + rc_channels->chan7_raw = mavlink_msg_rc_channels_get_chan7_raw(msg); + rc_channels->chan8_raw = mavlink_msg_rc_channels_get_chan8_raw(msg); + rc_channels->chan9_raw = mavlink_msg_rc_channels_get_chan9_raw(msg); + rc_channels->chan10_raw = mavlink_msg_rc_channels_get_chan10_raw(msg); + rc_channels->chan11_raw = mavlink_msg_rc_channels_get_chan11_raw(msg); + rc_channels->chan12_raw = mavlink_msg_rc_channels_get_chan12_raw(msg); + rc_channels->chan13_raw = mavlink_msg_rc_channels_get_chan13_raw(msg); + rc_channels->chan14_raw = mavlink_msg_rc_channels_get_chan14_raw(msg); + rc_channels->chan15_raw = mavlink_msg_rc_channels_get_chan15_raw(msg); + rc_channels->chan16_raw = mavlink_msg_rc_channels_get_chan16_raw(msg); + rc_channels->chan17_raw = mavlink_msg_rc_channels_get_chan17_raw(msg); + rc_channels->chan18_raw = mavlink_msg_rc_channels_get_chan18_raw(msg); + rc_channels->chancount = mavlink_msg_rc_channels_get_chancount(msg); + rc_channels->rssi = mavlink_msg_rc_channels_get_rssi(msg); +#else + memcpy(rc_channels, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_RC_CHANNELS_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_override.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_override.h index 0d8a1514b1..15d4c6f958 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_override.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_override.h @@ -245,6 +245,56 @@ static inline void mavlink_msg_rc_channels_override_send(mavlink_channel_t chan, #endif } +#if MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_rc_channels_override_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, chan1_raw); + _mav_put_uint16_t(buf, 2, chan2_raw); + _mav_put_uint16_t(buf, 4, chan3_raw); + _mav_put_uint16_t(buf, 6, chan4_raw); + _mav_put_uint16_t(buf, 8, chan5_raw); + _mav_put_uint16_t(buf, 10, chan6_raw); + _mav_put_uint16_t(buf, 12, chan7_raw); + _mav_put_uint16_t(buf, 14, chan8_raw); + _mav_put_uint8_t(buf, 16, target_system); + _mav_put_uint8_t(buf, 17, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, buf, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, buf, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN); +#endif +#else + mavlink_rc_channels_override_t *packet = (mavlink_rc_channels_override_t *)msgbuf; + packet->chan1_raw = chan1_raw; + packet->chan2_raw = chan2_raw; + packet->chan3_raw = chan3_raw; + packet->chan4_raw = chan4_raw; + packet->chan5_raw = chan5_raw; + packet->chan6_raw = chan6_raw; + packet->chan7_raw = chan7_raw; + packet->chan8_raw = chan8_raw; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, (const char *)packet, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, (const char *)packet, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE RC_CHANNELS_OVERRIDE UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_raw.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_raw.h index 3c0aed0202..d75a5934aa 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_raw.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_raw.h @@ -256,6 +256,58 @@ static inline void mavlink_msg_rc_channels_raw_send(mavlink_channel_t chan, uint #endif } +#if MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_rc_channels_raw_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t port, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint8_t rssi) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint16_t(buf, 4, chan1_raw); + _mav_put_uint16_t(buf, 6, chan2_raw); + _mav_put_uint16_t(buf, 8, chan3_raw); + _mav_put_uint16_t(buf, 10, chan4_raw); + _mav_put_uint16_t(buf, 12, chan5_raw); + _mav_put_uint16_t(buf, 14, chan6_raw); + _mav_put_uint16_t(buf, 16, chan7_raw); + _mav_put_uint16_t(buf, 18, chan8_raw); + _mav_put_uint8_t(buf, 20, port); + _mav_put_uint8_t(buf, 21, rssi); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_RAW, buf, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN, MAVLINK_MSG_ID_RC_CHANNELS_RAW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_RAW, buf, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN); +#endif +#else + mavlink_rc_channels_raw_t *packet = (mavlink_rc_channels_raw_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->chan1_raw = chan1_raw; + packet->chan2_raw = chan2_raw; + packet->chan3_raw = chan3_raw; + packet->chan4_raw = chan4_raw; + packet->chan5_raw = chan5_raw; + packet->chan6_raw = chan6_raw; + packet->chan7_raw = chan7_raw; + packet->chan8_raw = chan8_raw; + packet->port = port; + packet->rssi = rssi; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_RAW, (const char *)packet, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN, MAVLINK_MSG_ID_RC_CHANNELS_RAW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_RAW, (const char *)packet, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE RC_CHANNELS_RAW UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_scaled.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_scaled.h index 2153ab3928..f400f987dd 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_scaled.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_rc_channels_scaled.h @@ -256,6 +256,58 @@ static inline void mavlink_msg_rc_channels_scaled_send(mavlink_channel_t chan, u #endif } +#if MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_rc_channels_scaled_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t port, int16_t chan1_scaled, int16_t chan2_scaled, int16_t chan3_scaled, int16_t chan4_scaled, int16_t chan5_scaled, int16_t chan6_scaled, int16_t chan7_scaled, int16_t chan8_scaled, uint8_t rssi) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int16_t(buf, 4, chan1_scaled); + _mav_put_int16_t(buf, 6, chan2_scaled); + _mav_put_int16_t(buf, 8, chan3_scaled); + _mav_put_int16_t(buf, 10, chan4_scaled); + _mav_put_int16_t(buf, 12, chan5_scaled); + _mav_put_int16_t(buf, 14, chan6_scaled); + _mav_put_int16_t(buf, 16, chan7_scaled); + _mav_put_int16_t(buf, 18, chan8_scaled); + _mav_put_uint8_t(buf, 20, port); + _mav_put_uint8_t(buf, 21, rssi); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_SCALED, buf, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_SCALED, buf, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN); +#endif +#else + mavlink_rc_channels_scaled_t *packet = (mavlink_rc_channels_scaled_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->chan1_scaled = chan1_scaled; + packet->chan2_scaled = chan2_scaled; + packet->chan3_scaled = chan3_scaled; + packet->chan4_scaled = chan4_scaled; + packet->chan5_scaled = chan5_scaled; + packet->chan6_scaled = chan6_scaled; + packet->chan7_scaled = chan7_scaled; + packet->chan8_scaled = chan8_scaled; + packet->port = port; + packet->rssi = rssi; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_SCALED, (const char *)packet, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_SCALED, (const char *)packet, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE RC_CHANNELS_SCALED UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_request_data_stream.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_request_data_stream.h index c754ad8761..2ebcc54da5 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_request_data_stream.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_request_data_stream.h @@ -190,6 +190,46 @@ static inline void mavlink_msg_request_data_stream_send(mavlink_channel_t chan, #endif } +#if MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_request_data_stream_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t req_stream_id, uint16_t req_message_rate, uint8_t start_stop) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, req_message_rate); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, req_stream_id); + _mav_put_uint8_t(buf, 5, start_stop); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REQUEST_DATA_STREAM, buf, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REQUEST_DATA_STREAM, buf, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN); +#endif +#else + mavlink_request_data_stream_t *packet = (mavlink_request_data_stream_t *)msgbuf; + packet->req_message_rate = req_message_rate; + packet->target_system = target_system; + packet->target_component = target_component; + packet->req_stream_id = req_stream_id; + packet->start_stop = start_stop; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REQUEST_DATA_STREAM, (const char *)packet, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REQUEST_DATA_STREAM, (const char *)packet, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE REQUEST_DATA_STREAM UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint.h index ac3ef4fa9f..131b756b60 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint.h @@ -190,6 +190,46 @@ static inline void mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(mavlink #endif } +#if MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, float roll_rate, float pitch_rate, float yaw_rate, float thrust) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, roll_rate); + _mav_put_float(buf, 8, pitch_rate); + _mav_put_float(buf, 12, yaw_rate); + _mav_put_float(buf, 16, thrust); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, buf, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, buf, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN); +#endif +#else + mavlink_roll_pitch_yaw_rates_thrust_setpoint_t *packet = (mavlink_roll_pitch_yaw_rates_thrust_setpoint_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->roll_rate = roll_rate; + packet->pitch_rate = pitch_rate; + packet->yaw_rate = yaw_rate; + packet->thrust = thrust; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE ROLL_PITCH_YAW_RATES_THRUST_SETPOINT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h index 626477322b..757e6d138b 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h @@ -190,6 +190,46 @@ static inline void mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_send(mavlink #endif } +#if MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, float roll_speed, float pitch_speed, float yaw_speed, float thrust) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, roll_speed); + _mav_put_float(buf, 8, pitch_speed); + _mav_put_float(buf, 12, yaw_speed); + _mav_put_float(buf, 16, thrust); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, buf, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, buf, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN); +#endif +#else + mavlink_roll_pitch_yaw_speed_thrust_setpoint_t *packet = (mavlink_roll_pitch_yaw_speed_thrust_setpoint_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->roll_speed = roll_speed; + packet->pitch_speed = pitch_speed; + packet->yaw_speed = yaw_speed; + packet->thrust = thrust; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h index ffcdc547b4..f04eedcb36 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h @@ -190,6 +190,46 @@ static inline void mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(mavlink_chann #endif } +#if MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_roll_pitch_yaw_thrust_setpoint_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, float roll, float pitch, float yaw, float thrust) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, roll); + _mav_put_float(buf, 8, pitch); + _mav_put_float(buf, 12, yaw); + _mav_put_float(buf, 16, thrust); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT, buf, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT, buf, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN); +#endif +#else + mavlink_roll_pitch_yaw_thrust_setpoint_t *packet = (mavlink_roll_pitch_yaw_thrust_setpoint_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->roll = roll; + packet->pitch = pitch; + packet->yaw = yaw; + packet->thrust = thrust; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE ROLL_PITCH_YAW_THRUST_SETPOINT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_allowed_area.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_allowed_area.h index fcd54cbb78..0176dfcc8d 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_allowed_area.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_allowed_area.h @@ -212,6 +212,50 @@ static inline void mavlink_msg_safety_allowed_area_send(mavlink_channel_t chan, #endif } +#if MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_safety_allowed_area_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, p1x); + _mav_put_float(buf, 4, p1y); + _mav_put_float(buf, 8, p1z); + _mav_put_float(buf, 12, p2x); + _mav_put_float(buf, 16, p2y); + _mav_put_float(buf, 20, p2z); + _mav_put_uint8_t(buf, 24, frame); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA, buf, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA, buf, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN); +#endif +#else + mavlink_safety_allowed_area_t *packet = (mavlink_safety_allowed_area_t *)msgbuf; + packet->p1x = p1x; + packet->p1y = p1y; + packet->p1z = p1z; + packet->p2x = p2x; + packet->p2y = p2y; + packet->p2z = p2z; + packet->frame = frame; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA, (const char *)packet, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA, (const char *)packet, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SAFETY_ALLOWED_AREA UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_set_allowed_area.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_set_allowed_area.h index 61f6af1554..dafbc260b4 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_set_allowed_area.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_safety_set_allowed_area.h @@ -234,6 +234,54 @@ static inline void mavlink_msg_safety_set_allowed_area_send(mavlink_channel_t ch #endif } +#if MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_safety_set_allowed_area_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, p1x); + _mav_put_float(buf, 4, p1y); + _mav_put_float(buf, 8, p1z); + _mav_put_float(buf, 12, p2x); + _mav_put_float(buf, 16, p2y); + _mav_put_float(buf, 20, p2z); + _mav_put_uint8_t(buf, 24, target_system); + _mav_put_uint8_t(buf, 25, target_component); + _mav_put_uint8_t(buf, 26, frame); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA, buf, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA, buf, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN); +#endif +#else + mavlink_safety_set_allowed_area_t *packet = (mavlink_safety_set_allowed_area_t *)msgbuf; + packet->p1x = p1x; + packet->p1y = p1y; + packet->p1z = p1z; + packet->p2x = p2x; + packet->p2y = p2y; + packet->p2z = p2z; + packet->target_system = target_system; + packet->target_component = target_component; + packet->frame = frame; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA, (const char *)packet, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA, (const char *)packet, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SAFETY_SET_ALLOWED_AREA UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu.h index 3010d051af..1648a56f89 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu.h @@ -245,6 +245,56 @@ static inline void mavlink_msg_scaled_imu_send(mavlink_channel_t chan, uint32_t #endif } +#if MAVLINK_MSG_ID_SCALED_IMU_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_scaled_imu_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int16_t(buf, 4, xacc); + _mav_put_int16_t(buf, 6, yacc); + _mav_put_int16_t(buf, 8, zacc); + _mav_put_int16_t(buf, 10, xgyro); + _mav_put_int16_t(buf, 12, ygyro); + _mav_put_int16_t(buf, 14, zgyro); + _mav_put_int16_t(buf, 16, xmag); + _mav_put_int16_t(buf, 18, ymag); + _mav_put_int16_t(buf, 20, zmag); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU, buf, MAVLINK_MSG_ID_SCALED_IMU_LEN, MAVLINK_MSG_ID_SCALED_IMU_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU, buf, MAVLINK_MSG_ID_SCALED_IMU_LEN); +#endif +#else + mavlink_scaled_imu_t *packet = (mavlink_scaled_imu_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->xacc = xacc; + packet->yacc = yacc; + packet->zacc = zacc; + packet->xgyro = xgyro; + packet->ygyro = ygyro; + packet->zgyro = zgyro; + packet->xmag = xmag; + packet->ymag = ymag; + packet->zmag = zmag; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU, (const char *)packet, MAVLINK_MSG_ID_SCALED_IMU_LEN, MAVLINK_MSG_ID_SCALED_IMU_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU, (const char *)packet, MAVLINK_MSG_ID_SCALED_IMU_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SCALED_IMU UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu2.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu2.h index ea4dbbf81d..1dea121dde 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu2.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_imu2.h @@ -245,6 +245,56 @@ static inline void mavlink_msg_scaled_imu2_send(mavlink_channel_t chan, uint32_t #endif } +#if MAVLINK_MSG_ID_SCALED_IMU2_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_scaled_imu2_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int16_t(buf, 4, xacc); + _mav_put_int16_t(buf, 6, yacc); + _mav_put_int16_t(buf, 8, zacc); + _mav_put_int16_t(buf, 10, xgyro); + _mav_put_int16_t(buf, 12, ygyro); + _mav_put_int16_t(buf, 14, zgyro); + _mav_put_int16_t(buf, 16, xmag); + _mav_put_int16_t(buf, 18, ymag); + _mav_put_int16_t(buf, 20, zmag); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU2, buf, MAVLINK_MSG_ID_SCALED_IMU2_LEN, MAVLINK_MSG_ID_SCALED_IMU2_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU2, buf, MAVLINK_MSG_ID_SCALED_IMU2_LEN); +#endif +#else + mavlink_scaled_imu2_t *packet = (mavlink_scaled_imu2_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->xacc = xacc; + packet->yacc = yacc; + packet->zacc = zacc; + packet->xgyro = xgyro; + packet->ygyro = ygyro; + packet->zgyro = zgyro; + packet->xmag = xmag; + packet->ymag = ymag; + packet->zmag = zmag; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU2, (const char *)packet, MAVLINK_MSG_ID_SCALED_IMU2_LEN, MAVLINK_MSG_ID_SCALED_IMU2_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU2, (const char *)packet, MAVLINK_MSG_ID_SCALED_IMU2_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SCALED_IMU2 UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_pressure.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_pressure.h index 10324bc941..e0d6d87662 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_pressure.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_scaled_pressure.h @@ -179,6 +179,44 @@ static inline void mavlink_msg_scaled_pressure_send(mavlink_channel_t chan, uint #endif } +#if MAVLINK_MSG_ID_SCALED_PRESSURE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_scaled_pressure_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, press_abs); + _mav_put_float(buf, 8, press_diff); + _mav_put_int16_t(buf, 12, temperature); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE, buf, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE, buf, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN); +#endif +#else + mavlink_scaled_pressure_t *packet = (mavlink_scaled_pressure_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->press_abs = press_abs; + packet->press_diff = press_diff; + packet->temperature = temperature; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE, (const char *)packet, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE, (const char *)packet, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SCALED_PRESSURE UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_serial_control.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_serial_control.h new file mode 100644 index 0000000000..cbf72bae31 --- /dev/null +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_serial_control.h @@ -0,0 +1,321 @@ +// MESSAGE SERIAL_CONTROL PACKING + +#define MAVLINK_MSG_ID_SERIAL_CONTROL 126 + +typedef struct __mavlink_serial_control_t +{ + uint32_t baudrate; ///< Baudrate of transfer. Zero means no change. + uint16_t timeout; ///< Timeout for reply data in milliseconds + uint8_t device; ///< See SERIAL_CONTROL_DEV enum + uint8_t flags; ///< See SERIAL_CONTROL_FLAG enum + uint8_t count; ///< how many bytes in this transfer + uint8_t data[70]; ///< serial data +} mavlink_serial_control_t; + +#define MAVLINK_MSG_ID_SERIAL_CONTROL_LEN 79 +#define MAVLINK_MSG_ID_126_LEN 79 + +#define MAVLINK_MSG_ID_SERIAL_CONTROL_CRC 220 +#define MAVLINK_MSG_ID_126_CRC 220 + +#define MAVLINK_MSG_SERIAL_CONTROL_FIELD_DATA_LEN 70 + +#define MAVLINK_MESSAGE_INFO_SERIAL_CONTROL { \ + "SERIAL_CONTROL", \ + 6, \ + { { "baudrate", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_serial_control_t, baudrate) }, \ + { "timeout", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_serial_control_t, timeout) }, \ + { "device", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_serial_control_t, device) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_serial_control_t, flags) }, \ + { "count", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_serial_control_t, count) }, \ + { "data", NULL, MAVLINK_TYPE_UINT8_T, 70, 9, offsetof(mavlink_serial_control_t, data) }, \ + } \ +} + + +/** + * @brief Pack a serial_control message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param device See SERIAL_CONTROL_DEV enum + * @param flags See SERIAL_CONTROL_FLAG enum + * @param timeout Timeout for reply data in milliseconds + * @param baudrate Baudrate of transfer. Zero means no change. + * @param count how many bytes in this transfer + * @param data serial data + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_serial_control_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t device, uint8_t flags, uint16_t timeout, uint32_t baudrate, uint8_t count, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SERIAL_CONTROL_LEN]; + _mav_put_uint32_t(buf, 0, baudrate); + _mav_put_uint16_t(buf, 4, timeout); + _mav_put_uint8_t(buf, 6, device); + _mav_put_uint8_t(buf, 7, flags); + _mav_put_uint8_t(buf, 8, count); + _mav_put_uint8_t_array(buf, 9, data, 70); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); +#else + mavlink_serial_control_t packet; + packet.baudrate = baudrate; + packet.timeout = timeout; + packet.device = device; + packet.flags = flags; + packet.count = count; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*70); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SERIAL_CONTROL; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN, MAVLINK_MSG_ID_SERIAL_CONTROL_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); +#endif +} + +/** + * @brief Pack a serial_control message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param device See SERIAL_CONTROL_DEV enum + * @param flags See SERIAL_CONTROL_FLAG enum + * @param timeout Timeout for reply data in milliseconds + * @param baudrate Baudrate of transfer. Zero means no change. + * @param count how many bytes in this transfer + * @param data serial data + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_serial_control_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t device,uint8_t flags,uint16_t timeout,uint32_t baudrate,uint8_t count,const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SERIAL_CONTROL_LEN]; + _mav_put_uint32_t(buf, 0, baudrate); + _mav_put_uint16_t(buf, 4, timeout); + _mav_put_uint8_t(buf, 6, device); + _mav_put_uint8_t(buf, 7, flags); + _mav_put_uint8_t(buf, 8, count); + _mav_put_uint8_t_array(buf, 9, data, 70); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); +#else + mavlink_serial_control_t packet; + packet.baudrate = baudrate; + packet.timeout = timeout; + packet.device = device; + packet.flags = flags; + packet.count = count; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*70); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SERIAL_CONTROL; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN, MAVLINK_MSG_ID_SERIAL_CONTROL_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); +#endif +} + +/** + * @brief Encode a serial_control struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param serial_control C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_serial_control_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_serial_control_t* serial_control) +{ + return mavlink_msg_serial_control_pack(system_id, component_id, msg, serial_control->device, serial_control->flags, serial_control->timeout, serial_control->baudrate, serial_control->count, serial_control->data); +} + +/** + * @brief Encode a serial_control struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param serial_control C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_serial_control_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_serial_control_t* serial_control) +{ + return mavlink_msg_serial_control_pack_chan(system_id, component_id, chan, msg, serial_control->device, serial_control->flags, serial_control->timeout, serial_control->baudrate, serial_control->count, serial_control->data); +} + +/** + * @brief Send a serial_control message + * @param chan MAVLink channel to send the message + * + * @param device See SERIAL_CONTROL_DEV enum + * @param flags See SERIAL_CONTROL_FLAG enum + * @param timeout Timeout for reply data in milliseconds + * @param baudrate Baudrate of transfer. Zero means no change. + * @param count how many bytes in this transfer + * @param data serial data + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_serial_control_send(mavlink_channel_t chan, uint8_t device, uint8_t flags, uint16_t timeout, uint32_t baudrate, uint8_t count, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SERIAL_CONTROL_LEN]; + _mav_put_uint32_t(buf, 0, baudrate); + _mav_put_uint16_t(buf, 4, timeout); + _mav_put_uint8_t(buf, 6, device); + _mav_put_uint8_t(buf, 7, flags); + _mav_put_uint8_t(buf, 8, count); + _mav_put_uint8_t_array(buf, 9, data, 70); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_CONTROL, buf, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN, MAVLINK_MSG_ID_SERIAL_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_CONTROL, buf, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); +#endif +#else + mavlink_serial_control_t packet; + packet.baudrate = baudrate; + packet.timeout = timeout; + packet.device = device; + packet.flags = flags; + packet.count = count; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*70); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_CONTROL, (const char *)&packet, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN, MAVLINK_MSG_ID_SERIAL_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_CONTROL, (const char *)&packet, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); +#endif +#endif +} + +#if MAVLINK_MSG_ID_SERIAL_CONTROL_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_serial_control_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t device, uint8_t flags, uint16_t timeout, uint32_t baudrate, uint8_t count, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, baudrate); + _mav_put_uint16_t(buf, 4, timeout); + _mav_put_uint8_t(buf, 6, device); + _mav_put_uint8_t(buf, 7, flags); + _mav_put_uint8_t(buf, 8, count); + _mav_put_uint8_t_array(buf, 9, data, 70); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_CONTROL, buf, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN, MAVLINK_MSG_ID_SERIAL_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_CONTROL, buf, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); +#endif +#else + mavlink_serial_control_t *packet = (mavlink_serial_control_t *)msgbuf; + packet->baudrate = baudrate; + packet->timeout = timeout; + packet->device = device; + packet->flags = flags; + packet->count = count; + mav_array_memcpy(packet->data, data, sizeof(uint8_t)*70); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_CONTROL, (const char *)packet, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN, MAVLINK_MSG_ID_SERIAL_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_CONTROL, (const char *)packet, MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); +#endif +#endif +} +#endif + +#endif + +// MESSAGE SERIAL_CONTROL UNPACKING + + +/** + * @brief Get field device from serial_control message + * + * @return See SERIAL_CONTROL_DEV enum + */ +static inline uint8_t mavlink_msg_serial_control_get_device(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 6); +} + +/** + * @brief Get field flags from serial_control message + * + * @return See SERIAL_CONTROL_FLAG enum + */ +static inline uint8_t mavlink_msg_serial_control_get_flags(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 7); +} + +/** + * @brief Get field timeout from serial_control message + * + * @return Timeout for reply data in milliseconds + */ +static inline uint16_t mavlink_msg_serial_control_get_timeout(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 4); +} + +/** + * @brief Get field baudrate from serial_control message + * + * @return Baudrate of transfer. Zero means no change. + */ +static inline uint32_t mavlink_msg_serial_control_get_baudrate(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field count from serial_control message + * + * @return how many bytes in this transfer + */ +static inline uint8_t mavlink_msg_serial_control_get_count(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 8); +} + +/** + * @brief Get field data from serial_control message + * + * @return serial data + */ +static inline uint16_t mavlink_msg_serial_control_get_data(const mavlink_message_t* msg, uint8_t *data) +{ + return _MAV_RETURN_uint8_t_array(msg, data, 70, 9); +} + +/** + * @brief Decode a serial_control message into a struct + * + * @param msg The message to decode + * @param serial_control C-struct to decode the message contents into + */ +static inline void mavlink_msg_serial_control_decode(const mavlink_message_t* msg, mavlink_serial_control_t* serial_control) +{ +#if MAVLINK_NEED_BYTE_SWAP + serial_control->baudrate = mavlink_msg_serial_control_get_baudrate(msg); + serial_control->timeout = mavlink_msg_serial_control_get_timeout(msg); + serial_control->device = mavlink_msg_serial_control_get_device(msg); + serial_control->flags = mavlink_msg_serial_control_get_flags(msg); + serial_control->count = mavlink_msg_serial_control_get_count(msg); + mavlink_msg_serial_control_get_data(msg, serial_control->data); +#else + memcpy(serial_control, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_SERIAL_CONTROL_LEN); +#endif +} diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_servo_output_raw.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_servo_output_raw.h index 6a14e93ed3..e8408862ff 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_servo_output_raw.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_servo_output_raw.h @@ -245,6 +245,56 @@ static inline void mavlink_msg_servo_output_raw_send(mavlink_channel_t chan, uin #endif } +#if MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_servo_output_raw_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_usec, uint8_t port, uint16_t servo1_raw, uint16_t servo2_raw, uint16_t servo3_raw, uint16_t servo4_raw, uint16_t servo5_raw, uint16_t servo6_raw, uint16_t servo7_raw, uint16_t servo8_raw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_usec); + _mav_put_uint16_t(buf, 4, servo1_raw); + _mav_put_uint16_t(buf, 6, servo2_raw); + _mav_put_uint16_t(buf, 8, servo3_raw); + _mav_put_uint16_t(buf, 10, servo4_raw); + _mav_put_uint16_t(buf, 12, servo5_raw); + _mav_put_uint16_t(buf, 14, servo6_raw); + _mav_put_uint16_t(buf, 16, servo7_raw); + _mav_put_uint16_t(buf, 18, servo8_raw); + _mav_put_uint8_t(buf, 20, port); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, buf, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, buf, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN); +#endif +#else + mavlink_servo_output_raw_t *packet = (mavlink_servo_output_raw_t *)msgbuf; + packet->time_usec = time_usec; + packet->servo1_raw = servo1_raw; + packet->servo2_raw = servo2_raw; + packet->servo3_raw = servo3_raw; + packet->servo4_raw = servo4_raw; + packet->servo5_raw = servo5_raw; + packet->servo6_raw = servo6_raw; + packet->servo7_raw = servo7_raw; + packet->servo8_raw = servo8_raw; + packet->port = port; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, (const char *)packet, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, (const char *)packet, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SERVO_OUTPUT_RAW UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_global_position_setpoint_int.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_global_position_setpoint_int.h index 6f0d7a69da..94a7b432bf 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_global_position_setpoint_int.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_global_position_setpoint_int.h @@ -190,6 +190,46 @@ static inline void mavlink_msg_set_global_position_setpoint_int_send(mavlink_cha #endif } +#if MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_set_global_position_setpoint_int_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t coordinate_frame, int32_t latitude, int32_t longitude, int32_t altitude, int16_t yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int32_t(buf, 0, latitude); + _mav_put_int32_t(buf, 4, longitude); + _mav_put_int32_t(buf, 8, altitude); + _mav_put_int16_t(buf, 12, yaw); + _mav_put_uint8_t(buf, 14, coordinate_frame); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT, buf, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT, buf, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN); +#endif +#else + mavlink_set_global_position_setpoint_int_t *packet = (mavlink_set_global_position_setpoint_int_t *)msgbuf; + packet->latitude = latitude; + packet->longitude = longitude; + packet->altitude = altitude; + packet->yaw = yaw; + packet->coordinate_frame = coordinate_frame; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT, (const char *)packet, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT, (const char *)packet, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SET_GLOBAL_POSITION_SETPOINT_INT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_gps_global_origin.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_gps_global_origin.h index c444d8d52c..1c018cbee3 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_gps_global_origin.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_gps_global_origin.h @@ -179,6 +179,44 @@ static inline void mavlink_msg_set_gps_global_origin_send(mavlink_channel_t chan #endif } +#if MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_set_gps_global_origin_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, int32_t latitude, int32_t longitude, int32_t altitude) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int32_t(buf, 0, latitude); + _mav_put_int32_t(buf, 4, longitude); + _mav_put_int32_t(buf, 8, altitude); + _mav_put_uint8_t(buf, 12, target_system); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN, buf, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN, buf, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN); +#endif +#else + mavlink_set_gps_global_origin_t *packet = (mavlink_set_gps_global_origin_t *)msgbuf; + packet->latitude = latitude; + packet->longitude = longitude; + packet->altitude = altitude; + packet->target_system = target_system; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN, (const char *)packet, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN, (const char *)packet, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SET_GPS_GLOBAL_ORIGIN UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_local_position_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_local_position_setpoint.h index 6f2835e031..04dbe6e9a2 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_local_position_setpoint.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_local_position_setpoint.h @@ -212,6 +212,50 @@ static inline void mavlink_msg_set_local_position_setpoint_send(mavlink_channel_ #endif } +#if MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_set_local_position_setpoint_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t coordinate_frame, float x, float y, float z, float yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, x); + _mav_put_float(buf, 4, y); + _mav_put_float(buf, 8, z); + _mav_put_float(buf, 12, yaw); + _mav_put_uint8_t(buf, 16, target_system); + _mav_put_uint8_t(buf, 17, target_component); + _mav_put_uint8_t(buf, 18, coordinate_frame); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT, buf, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT, buf, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN); +#endif +#else + mavlink_set_local_position_setpoint_t *packet = (mavlink_set_local_position_setpoint_t *)msgbuf; + packet->x = x; + packet->y = y; + packet->z = z; + packet->yaw = yaw; + packet->target_system = target_system; + packet->target_component = target_component; + packet->coordinate_frame = coordinate_frame; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SET_LOCAL_POSITION_SETPOINT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_mode.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_mode.h index 1aff42cce3..4b60a4120b 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_mode.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_mode.h @@ -168,6 +168,42 @@ static inline void mavlink_msg_set_mode_send(mavlink_channel_t chan, uint8_t tar #endif } +#if MAVLINK_MSG_ID_SET_MODE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_set_mode_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t base_mode, uint32_t custom_mode) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, custom_mode); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, base_mode); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MODE, buf, MAVLINK_MSG_ID_SET_MODE_LEN, MAVLINK_MSG_ID_SET_MODE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MODE, buf, MAVLINK_MSG_ID_SET_MODE_LEN); +#endif +#else + mavlink_set_mode_t *packet = (mavlink_set_mode_t *)msgbuf; + packet->custom_mode = custom_mode; + packet->target_system = target_system; + packet->base_mode = base_mode; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MODE, (const char *)packet, MAVLINK_MSG_ID_SET_MODE_LEN, MAVLINK_MSG_ID_SET_MODE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MODE, (const char *)packet, MAVLINK_MSG_ID_SET_MODE_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SET_MODE UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_motors_setpoint.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_motors_setpoint.h index 8ceb8888fc..8c0e2a014a 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_motors_setpoint.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_motors_setpoint.h @@ -190,6 +190,46 @@ static inline void mavlink_msg_set_quad_motors_setpoint_send(mavlink_channel_t c #endif } +#if MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_set_quad_motors_setpoint_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint16_t motor_front_nw, uint16_t motor_right_ne, uint16_t motor_back_se, uint16_t motor_left_sw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, motor_front_nw); + _mav_put_uint16_t(buf, 2, motor_right_ne); + _mav_put_uint16_t(buf, 4, motor_back_se); + _mav_put_uint16_t(buf, 6, motor_left_sw); + _mav_put_uint8_t(buf, 8, target_system); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT, buf, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT, buf, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN); +#endif +#else + mavlink_set_quad_motors_setpoint_t *packet = (mavlink_set_quad_motors_setpoint_t *)msgbuf; + packet->motor_front_nw = motor_front_nw; + packet->motor_right_ne = motor_right_ne; + packet->motor_back_se = motor_back_se; + packet->motor_left_sw = motor_left_sw; + packet->target_system = target_system; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SET_QUAD_MOTORS_SETPOINT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust.h index 9ef294cc97..c9a084773c 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust.h @@ -234,6 +234,52 @@ static inline void mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_send(mav #endif } +#if MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_set_quad_swarm_led_roll_pitch_yaw_thrust_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t group, uint8_t mode, const uint8_t *led_red, const uint8_t *led_blue, const uint8_t *led_green, const int16_t *roll, const int16_t *pitch, const int16_t *yaw, const uint16_t *thrust) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 32, group); + _mav_put_uint8_t(buf, 33, mode); + _mav_put_int16_t_array(buf, 0, roll, 4); + _mav_put_int16_t_array(buf, 8, pitch, 4); + _mav_put_int16_t_array(buf, 16, yaw, 4); + _mav_put_uint16_t_array(buf, 24, thrust, 4); + _mav_put_uint8_t_array(buf, 34, led_red, 4); + _mav_put_uint8_t_array(buf, 38, led_blue, 4); + _mav_put_uint8_t_array(buf, 42, led_green, 4); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, buf, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, buf, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN); +#endif +#else + mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t *packet = (mavlink_set_quad_swarm_led_roll_pitch_yaw_thrust_t *)msgbuf; + packet->group = group; + packet->mode = mode; + mav_array_memcpy(packet->roll, roll, sizeof(int16_t)*4); + mav_array_memcpy(packet->pitch, pitch, sizeof(int16_t)*4); + mav_array_memcpy(packet->yaw, yaw, sizeof(int16_t)*4); + mav_array_memcpy(packet->thrust, thrust, sizeof(uint16_t)*4); + mav_array_memcpy(packet->led_red, led_red, sizeof(uint8_t)*4); + mav_array_memcpy(packet->led_blue, led_blue, sizeof(uint8_t)*4); + mav_array_memcpy(packet->led_green, led_green, sizeof(uint8_t)*4); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, (const char *)packet, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, (const char *)packet, MAVLINK_MSG_ID_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust.h index 7d8d526f8b..338bf0e438 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust.h @@ -198,6 +198,46 @@ static inline void mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_send(mavlink #endif } +#if MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t group, uint8_t mode, const int16_t *roll, const int16_t *pitch, const int16_t *yaw, const uint16_t *thrust) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 32, group); + _mav_put_uint8_t(buf, 33, mode); + _mav_put_int16_t_array(buf, 0, roll, 4); + _mav_put_int16_t_array(buf, 8, pitch, 4); + _mav_put_int16_t_array(buf, 16, yaw, 4); + _mav_put_uint16_t_array(buf, 24, thrust, 4); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, buf, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, buf, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN); +#endif +#else + mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t *packet = (mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t *)msgbuf; + packet->group = group; + packet->mode = mode; + mav_array_memcpy(packet->roll, roll, sizeof(int16_t)*4); + mav_array_memcpy(packet->pitch, pitch, sizeof(int16_t)*4); + mav_array_memcpy(packet->yaw, yaw, sizeof(int16_t)*4); + mav_array_memcpy(packet->thrust, thrust, sizeof(uint16_t)*4); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, (const char *)packet, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, (const char *)packet, MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h index 5846ba41f7..4d0e5931fa 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h @@ -201,6 +201,48 @@ static inline void mavlink_msg_set_roll_pitch_yaw_speed_thrust_send(mavlink_chan #endif } +#if MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_set_roll_pitch_yaw_speed_thrust_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float roll_speed, float pitch_speed, float yaw_speed, float thrust) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, roll_speed); + _mav_put_float(buf, 4, pitch_speed); + _mav_put_float(buf, 8, yaw_speed); + _mav_put_float(buf, 12, thrust); + _mav_put_uint8_t(buf, 16, target_system); + _mav_put_uint8_t(buf, 17, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST, buf, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST, buf, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN); +#endif +#else + mavlink_set_roll_pitch_yaw_speed_thrust_t *packet = (mavlink_set_roll_pitch_yaw_speed_thrust_t *)msgbuf; + packet->roll_speed = roll_speed; + packet->pitch_speed = pitch_speed; + packet->yaw_speed = yaw_speed; + packet->thrust = thrust; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST, (const char *)packet, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST, (const char *)packet, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SET_ROLL_PITCH_YAW_SPEED_THRUST UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_thrust.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_thrust.h index 334fd39e36..21e0a43f24 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_thrust.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_set_roll_pitch_yaw_thrust.h @@ -201,6 +201,48 @@ static inline void mavlink_msg_set_roll_pitch_yaw_thrust_send(mavlink_channel_t #endif } +#if MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_set_roll_pitch_yaw_thrust_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float roll, float pitch, float yaw, float thrust) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, roll); + _mav_put_float(buf, 4, pitch); + _mav_put_float(buf, 8, yaw); + _mav_put_float(buf, 12, thrust); + _mav_put_uint8_t(buf, 16, target_system); + _mav_put_uint8_t(buf, 17, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST, buf, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST, buf, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN); +#endif +#else + mavlink_set_roll_pitch_yaw_thrust_t *packet = (mavlink_set_roll_pitch_yaw_thrust_t *)msgbuf; + packet->roll = roll; + packet->pitch = pitch; + packet->yaw = yaw; + packet->thrust = thrust; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST, (const char *)packet, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST, (const char *)packet, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SET_ROLL_PITCH_YAW_THRUST UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_6dof.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_6dof.h index 93ce345b65..90b63427b9 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_6dof.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_6dof.h @@ -212,6 +212,50 @@ static inline void mavlink_msg_setpoint_6dof_send(mavlink_channel_t chan, uint8_ #endif } +#if MAVLINK_MSG_ID_SETPOINT_6DOF_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_setpoint_6dof_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, float trans_x, float trans_y, float trans_z, float rot_x, float rot_y, float rot_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, trans_x); + _mav_put_float(buf, 4, trans_y); + _mav_put_float(buf, 8, trans_z); + _mav_put_float(buf, 12, rot_x); + _mav_put_float(buf, 16, rot_y); + _mav_put_float(buf, 20, rot_z); + _mav_put_uint8_t(buf, 24, target_system); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETPOINT_6DOF, buf, MAVLINK_MSG_ID_SETPOINT_6DOF_LEN, MAVLINK_MSG_ID_SETPOINT_6DOF_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETPOINT_6DOF, buf, MAVLINK_MSG_ID_SETPOINT_6DOF_LEN); +#endif +#else + mavlink_setpoint_6dof_t *packet = (mavlink_setpoint_6dof_t *)msgbuf; + packet->trans_x = trans_x; + packet->trans_y = trans_y; + packet->trans_z = trans_z; + packet->rot_x = rot_x; + packet->rot_y = rot_y; + packet->rot_z = rot_z; + packet->target_system = target_system; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETPOINT_6DOF, (const char *)packet, MAVLINK_MSG_ID_SETPOINT_6DOF_LEN, MAVLINK_MSG_ID_SETPOINT_6DOF_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETPOINT_6DOF, (const char *)packet, MAVLINK_MSG_ID_SETPOINT_6DOF_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SETPOINT_6DOF UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_8dof.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_8dof.h index de80e46459..308a211cde 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_8dof.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_setpoint_8dof.h @@ -234,6 +234,54 @@ static inline void mavlink_msg_setpoint_8dof_send(mavlink_channel_t chan, uint8_ #endif } +#if MAVLINK_MSG_ID_SETPOINT_8DOF_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_setpoint_8dof_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, float val1, float val2, float val3, float val4, float val5, float val6, float val7, float val8) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, val1); + _mav_put_float(buf, 4, val2); + _mav_put_float(buf, 8, val3); + _mav_put_float(buf, 12, val4); + _mav_put_float(buf, 16, val5); + _mav_put_float(buf, 20, val6); + _mav_put_float(buf, 24, val7); + _mav_put_float(buf, 28, val8); + _mav_put_uint8_t(buf, 32, target_system); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETPOINT_8DOF, buf, MAVLINK_MSG_ID_SETPOINT_8DOF_LEN, MAVLINK_MSG_ID_SETPOINT_8DOF_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETPOINT_8DOF, buf, MAVLINK_MSG_ID_SETPOINT_8DOF_LEN); +#endif +#else + mavlink_setpoint_8dof_t *packet = (mavlink_setpoint_8dof_t *)msgbuf; + packet->val1 = val1; + packet->val2 = val2; + packet->val3 = val3; + packet->val4 = val4; + packet->val5 = val5; + packet->val6 = val6; + packet->val7 = val7; + packet->val8 = val8; + packet->target_system = target_system; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETPOINT_8DOF, (const char *)packet, MAVLINK_MSG_ID_SETPOINT_8DOF_LEN, MAVLINK_MSG_ID_SETPOINT_8DOF_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETPOINT_8DOF, (const char *)packet, MAVLINK_MSG_ID_SETPOINT_8DOF_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SETPOINT_8DOF UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_sim_state.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_sim_state.h index ebd657cf38..db3a926424 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_sim_state.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_sim_state.h @@ -366,6 +366,78 @@ static inline void mavlink_msg_sim_state_send(mavlink_channel_t chan, float q1, #endif } +#if MAVLINK_MSG_ID_SIM_STATE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_sim_state_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float q1, float q2, float q3, float q4, float roll, float pitch, float yaw, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float lat, float lon, float alt, float std_dev_horz, float std_dev_vert, float vn, float ve, float vd) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, q1); + _mav_put_float(buf, 4, q2); + _mav_put_float(buf, 8, q3); + _mav_put_float(buf, 12, q4); + _mav_put_float(buf, 16, roll); + _mav_put_float(buf, 20, pitch); + _mav_put_float(buf, 24, yaw); + _mav_put_float(buf, 28, xacc); + _mav_put_float(buf, 32, yacc); + _mav_put_float(buf, 36, zacc); + _mav_put_float(buf, 40, xgyro); + _mav_put_float(buf, 44, ygyro); + _mav_put_float(buf, 48, zgyro); + _mav_put_float(buf, 52, lat); + _mav_put_float(buf, 56, lon); + _mav_put_float(buf, 60, alt); + _mav_put_float(buf, 64, std_dev_horz); + _mav_put_float(buf, 68, std_dev_vert); + _mav_put_float(buf, 72, vn); + _mav_put_float(buf, 76, ve); + _mav_put_float(buf, 80, vd); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIM_STATE, buf, MAVLINK_MSG_ID_SIM_STATE_LEN, MAVLINK_MSG_ID_SIM_STATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIM_STATE, buf, MAVLINK_MSG_ID_SIM_STATE_LEN); +#endif +#else + mavlink_sim_state_t *packet = (mavlink_sim_state_t *)msgbuf; + packet->q1 = q1; + packet->q2 = q2; + packet->q3 = q3; + packet->q4 = q4; + packet->roll = roll; + packet->pitch = pitch; + packet->yaw = yaw; + packet->xacc = xacc; + packet->yacc = yacc; + packet->zacc = zacc; + packet->xgyro = xgyro; + packet->ygyro = ygyro; + packet->zgyro = zgyro; + packet->lat = lat; + packet->lon = lon; + packet->alt = alt; + packet->std_dev_horz = std_dev_horz; + packet->std_dev_vert = std_dev_vert; + packet->vn = vn; + packet->ve = ve; + packet->vd = vd; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIM_STATE, (const char *)packet, MAVLINK_MSG_ID_SIM_STATE_LEN, MAVLINK_MSG_ID_SIM_STATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SIM_STATE, (const char *)packet, MAVLINK_MSG_ID_SIM_STATE_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SIM_STATE UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_state_correction.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_state_correction.h index 2db627b96a..be2629dd8a 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_state_correction.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_state_correction.h @@ -234,6 +234,54 @@ static inline void mavlink_msg_state_correction_send(mavlink_channel_t chan, flo #endif } +#if MAVLINK_MSG_ID_STATE_CORRECTION_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_state_correction_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float xErr, float yErr, float zErr, float rollErr, float pitchErr, float yawErr, float vxErr, float vyErr, float vzErr) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, xErr); + _mav_put_float(buf, 4, yErr); + _mav_put_float(buf, 8, zErr); + _mav_put_float(buf, 12, rollErr); + _mav_put_float(buf, 16, pitchErr); + _mav_put_float(buf, 20, yawErr); + _mav_put_float(buf, 24, vxErr); + _mav_put_float(buf, 28, vyErr); + _mav_put_float(buf, 32, vzErr); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATE_CORRECTION, buf, MAVLINK_MSG_ID_STATE_CORRECTION_LEN, MAVLINK_MSG_ID_STATE_CORRECTION_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATE_CORRECTION, buf, MAVLINK_MSG_ID_STATE_CORRECTION_LEN); +#endif +#else + mavlink_state_correction_t *packet = (mavlink_state_correction_t *)msgbuf; + packet->xErr = xErr; + packet->yErr = yErr; + packet->zErr = zErr; + packet->rollErr = rollErr; + packet->pitchErr = pitchErr; + packet->yawErr = yawErr; + packet->vxErr = vxErr; + packet->vyErr = vyErr; + packet->vzErr = vzErr; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATE_CORRECTION, (const char *)packet, MAVLINK_MSG_ID_STATE_CORRECTION_LEN, MAVLINK_MSG_ID_STATE_CORRECTION_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATE_CORRECTION, (const char *)packet, MAVLINK_MSG_ID_STATE_CORRECTION_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE STATE_CORRECTION UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_statustext.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_statustext.h index 536ca06349..c8c2547b31 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_statustext.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_statustext.h @@ -151,6 +151,38 @@ static inline void mavlink_msg_statustext_send(mavlink_channel_t chan, uint8_t s #endif } +#if MAVLINK_MSG_ID_STATUSTEXT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_statustext_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t severity, const char *text) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, severity); + _mav_put_char_array(buf, 1, text, 50); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, buf, MAVLINK_MSG_ID_STATUSTEXT_LEN, MAVLINK_MSG_ID_STATUSTEXT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, buf, MAVLINK_MSG_ID_STATUSTEXT_LEN); +#endif +#else + mavlink_statustext_t *packet = (mavlink_statustext_t *)msgbuf; + packet->severity = severity; + mav_array_memcpy(packet->text, text, sizeof(char)*50); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, (const char *)packet, MAVLINK_MSG_ID_STATUSTEXT_LEN, MAVLINK_MSG_ID_STATUSTEXT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, (const char *)packet, MAVLINK_MSG_ID_STATUSTEXT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE STATUSTEXT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_sys_status.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_sys_status.h index 101b366783..1b1730d5f5 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_sys_status.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_sys_status.h @@ -278,6 +278,62 @@ static inline void mavlink_msg_sys_status_send(mavlink_channel_t chan, uint32_t #endif } +#if MAVLINK_MSG_ID_SYS_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_sys_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t onboard_control_sensors_present, uint32_t onboard_control_sensors_enabled, uint32_t onboard_control_sensors_health, uint16_t load, uint16_t voltage_battery, int16_t current_battery, int8_t battery_remaining, uint16_t drop_rate_comm, uint16_t errors_comm, uint16_t errors_count1, uint16_t errors_count2, uint16_t errors_count3, uint16_t errors_count4) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, onboard_control_sensors_present); + _mav_put_uint32_t(buf, 4, onboard_control_sensors_enabled); + _mav_put_uint32_t(buf, 8, onboard_control_sensors_health); + _mav_put_uint16_t(buf, 12, load); + _mav_put_uint16_t(buf, 14, voltage_battery); + _mav_put_int16_t(buf, 16, current_battery); + _mav_put_uint16_t(buf, 18, drop_rate_comm); + _mav_put_uint16_t(buf, 20, errors_comm); + _mav_put_uint16_t(buf, 22, errors_count1); + _mav_put_uint16_t(buf, 24, errors_count2); + _mav_put_uint16_t(buf, 26, errors_count3); + _mav_put_uint16_t(buf, 28, errors_count4); + _mav_put_int8_t(buf, 30, battery_remaining); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_STATUS, buf, MAVLINK_MSG_ID_SYS_STATUS_LEN, MAVLINK_MSG_ID_SYS_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_STATUS, buf, MAVLINK_MSG_ID_SYS_STATUS_LEN); +#endif +#else + mavlink_sys_status_t *packet = (mavlink_sys_status_t *)msgbuf; + packet->onboard_control_sensors_present = onboard_control_sensors_present; + packet->onboard_control_sensors_enabled = onboard_control_sensors_enabled; + packet->onboard_control_sensors_health = onboard_control_sensors_health; + packet->load = load; + packet->voltage_battery = voltage_battery; + packet->current_battery = current_battery; + packet->drop_rate_comm = drop_rate_comm; + packet->errors_comm = errors_comm; + packet->errors_count1 = errors_count1; + packet->errors_count2 = errors_count2; + packet->errors_count3 = errors_count3; + packet->errors_count4 = errors_count4; + packet->battery_remaining = battery_remaining; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_STATUS, (const char *)packet, MAVLINK_MSG_ID_SYS_STATUS_LEN, MAVLINK_MSG_ID_SYS_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_STATUS, (const char *)packet, MAVLINK_MSG_ID_SYS_STATUS_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SYS_STATUS UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_system_time.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_system_time.h index 1807567aea..988c669c36 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_system_time.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_system_time.h @@ -157,6 +157,40 @@ static inline void mavlink_msg_system_time_send(mavlink_channel_t chan, uint64_t #endif } +#if MAVLINK_MSG_ID_SYSTEM_TIME_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_system_time_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_unix_usec, uint32_t time_boot_ms) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_unix_usec); + _mav_put_uint32_t(buf, 8, time_boot_ms); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYSTEM_TIME, buf, MAVLINK_MSG_ID_SYSTEM_TIME_LEN, MAVLINK_MSG_ID_SYSTEM_TIME_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYSTEM_TIME, buf, MAVLINK_MSG_ID_SYSTEM_TIME_LEN); +#endif +#else + mavlink_system_time_t *packet = (mavlink_system_time_t *)msgbuf; + packet->time_unix_usec = time_unix_usec; + packet->time_boot_ms = time_boot_ms; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYSTEM_TIME, (const char *)packet, MAVLINK_MSG_ID_SYSTEM_TIME_LEN, MAVLINK_MSG_ID_SYSTEM_TIME_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYSTEM_TIME, (const char *)packet, MAVLINK_MSG_ID_SYSTEM_TIME_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SYSTEM_TIME UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vfr_hud.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vfr_hud.h index 5b1093a3d7..b130ee50b5 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vfr_hud.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vfr_hud.h @@ -201,6 +201,48 @@ static inline void mavlink_msg_vfr_hud_send(mavlink_channel_t chan, float airspe #endif } +#if MAVLINK_MSG_ID_VFR_HUD_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_vfr_hud_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float airspeed, float groundspeed, int16_t heading, uint16_t throttle, float alt, float climb) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, airspeed); + _mav_put_float(buf, 4, groundspeed); + _mav_put_float(buf, 8, alt); + _mav_put_float(buf, 12, climb); + _mav_put_int16_t(buf, 16, heading); + _mav_put_uint16_t(buf, 18, throttle); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VFR_HUD, buf, MAVLINK_MSG_ID_VFR_HUD_LEN, MAVLINK_MSG_ID_VFR_HUD_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VFR_HUD, buf, MAVLINK_MSG_ID_VFR_HUD_LEN); +#endif +#else + mavlink_vfr_hud_t *packet = (mavlink_vfr_hud_t *)msgbuf; + packet->airspeed = airspeed; + packet->groundspeed = groundspeed; + packet->alt = alt; + packet->climb = climb; + packet->heading = heading; + packet->throttle = throttle; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VFR_HUD, (const char *)packet, MAVLINK_MSG_ID_VFR_HUD_LEN, MAVLINK_MSG_ID_VFR_HUD_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VFR_HUD, (const char *)packet, MAVLINK_MSG_ID_VFR_HUD_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE VFR_HUD UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vicon_position_estimate.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vicon_position_estimate.h index a254202e4e..b3fa7bc1c1 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vicon_position_estimate.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vicon_position_estimate.h @@ -212,6 +212,50 @@ static inline void mavlink_msg_vicon_position_estimate_send(mavlink_channel_t ch #endif } +#if MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_vicon_position_estimate_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + _mav_put_float(buf, 20, roll); + _mav_put_float(buf, 24, pitch); + _mav_put_float(buf, 28, yaw); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN); +#endif +#else + mavlink_vicon_position_estimate_t *packet = (mavlink_vicon_position_estimate_t *)msgbuf; + packet->usec = usec; + packet->x = x; + packet->y = y; + packet->z = z; + packet->roll = roll; + packet->pitch = pitch; + packet->yaw = yaw; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE VICON_POSITION_ESTIMATE UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_position_estimate.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_position_estimate.h index f7a741b09e..8f82fb6824 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_position_estimate.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_position_estimate.h @@ -212,6 +212,50 @@ static inline void mavlink_msg_vision_position_estimate_send(mavlink_channel_t c #endif } +#if MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_vision_position_estimate_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + _mav_put_float(buf, 20, roll); + _mav_put_float(buf, 24, pitch); + _mav_put_float(buf, 28, yaw); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN); +#endif +#else + mavlink_vision_position_estimate_t *packet = (mavlink_vision_position_estimate_t *)msgbuf; + packet->usec = usec; + packet->x = x; + packet->y = y; + packet->z = z; + packet->roll = roll; + packet->pitch = pitch; + packet->yaw = yaw; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE VISION_POSITION_ESTIMATE UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_speed_estimate.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_speed_estimate.h index 6602251283..7528014c76 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_speed_estimate.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_vision_speed_estimate.h @@ -179,6 +179,44 @@ static inline void mavlink_msg_vision_speed_estimate_send(mavlink_channel_t chan #endif } +#if MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_vision_speed_estimate_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t usec, float x, float y, float z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE, buf, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE, buf, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN); +#endif +#else + mavlink_vision_speed_estimate_t *packet = (mavlink_vision_speed_estimate_t *)msgbuf; + packet->usec = usec; + packet->x = x; + packet->y = y; + packet->z = z; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE VISION_SPEED_ESTIMATE UNPACKING diff --git a/mavlink/include/mavlink/v1.0/common/testsuite.h b/mavlink/include/mavlink/v1.0/common/testsuite.h index c5aa9ddf3a..de23fcb2a2 100644 --- a/mavlink/include/mavlink/v1.0/common/testsuite.h +++ b/mavlink/include/mavlink/v1.0/common/testsuite.h @@ -2805,6 +2805,89 @@ static void mavlink_test_state_correction(uint8_t system_id, uint8_t component_i MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } +static void mavlink_test_rc_channels(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_rc_channels_t packet_in = { + 963497464, + }17443, + }17547, + }17651, + }17755, + }17859, + }17963, + }18067, + }18171, + }18275, + }18379, + }18483, + }18587, + }18691, + }18795, + }18899, + }19003, + }19107, + }19211, + }125, + }192, + }; + mavlink_rc_channels_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_boot_ms = packet_in.time_boot_ms; + packet1.chan1_raw = packet_in.chan1_raw; + packet1.chan2_raw = packet_in.chan2_raw; + packet1.chan3_raw = packet_in.chan3_raw; + packet1.chan4_raw = packet_in.chan4_raw; + packet1.chan5_raw = packet_in.chan5_raw; + packet1.chan6_raw = packet_in.chan6_raw; + packet1.chan7_raw = packet_in.chan7_raw; + packet1.chan8_raw = packet_in.chan8_raw; + packet1.chan9_raw = packet_in.chan9_raw; + packet1.chan10_raw = packet_in.chan10_raw; + packet1.chan11_raw = packet_in.chan11_raw; + packet1.chan12_raw = packet_in.chan12_raw; + packet1.chan13_raw = packet_in.chan13_raw; + packet1.chan14_raw = packet_in.chan14_raw; + packet1.chan15_raw = packet_in.chan15_raw; + packet1.chan16_raw = packet_in.chan16_raw; + packet1.chan17_raw = packet_in.chan17_raw; + packet1.chan18_raw = packet_in.chan18_raw; + packet1.chancount = packet_in.chancount; + packet1.rssi = packet_in.rssi; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_rc_channels_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_rc_channels_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_rc_channels_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.chancount , packet1.chan1_raw , packet1.chan2_raw , packet1.chan3_raw , packet1.chan4_raw , packet1.chan5_raw , packet1.chan6_raw , packet1.chan7_raw , packet1.chan8_raw , packet1.chan9_raw , packet1.chan10_raw , packet1.chan11_raw , packet1.chan12_raw , packet1.chan13_raw , packet1.chan14_raw , packet1.chan15_raw , packet1.chan16_raw , packet1.chan17_raw , packet1.chan18_raw , packet1.rssi ); + mavlink_msg_rc_channels_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_rc_channels_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.chancount , packet1.chan1_raw , packet1.chan2_raw , packet1.chan3_raw , packet1.chan4_raw , packet1.chan5_raw , packet1.chan6_raw , packet1.chan7_raw , packet1.chan8_raw , packet1.chan9_raw , packet1.chan10_raw , packet1.chan11_raw , packet1.chan12_raw , packet1.chan13_raw , packet1.chan14_raw , packet1.chan15_raw , packet1.chan16_raw , packet1.chan17_raw , packet1.chan18_raw , packet1.rssi ); + mavlink_msg_rc_channels_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ MAV_CMD_DO_SET_ROI=201, /* 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| */ + MAV_CMD_DO_DIGICAM_CONFIGURE=202, /* Mission command to configure an on-board camera controller system. |Modes: P, TV, AV, M, Etc| Shutter speed: Divisor number for one second| Aperture: F stop number| ISO number e.g. 80, 100, 200, Etc| Exposure type enumerator| Command Identity| Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)| */ + MAV_CMD_DO_DIGICAM_CONTROL=203, /* Mission command to control an on-board camera controller system. |Session control e.g. show/hide lens| Zoom's absolute position| Zooming step value to offset zoom from the current position| Focus Locking, Unlocking or Re-locking| Shooting Command| Command Identity| Empty| */ + MAV_CMD_DO_MOUNT_CONFIGURE=204, /* Mission command to configure a camera or antenna mount |Mount operation mode (see MAV_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty| */ + MAV_CMD_DO_MOUNT_CONTROL=205, /* Mission command to control a camera or antenna mount |pitch or lat in degrees, depending on mount mode.| roll or lon in degrees depending on mount mode| yaw or alt (in meters) depending on mount mode| reserved| reserved| reserved| MAV_MOUNT_MODE enum value| */ + MAV_CMD_DO_SET_CAM_TRIGG_DIST=206, /* Mission command to set CAM_TRIGG_DIST for this flight |Camera trigger distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_FENCE_ENABLE=207, /* Mission command to enable the geofence |enable? (0=disable, 1=enable)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_PARACHUTE=208, /* Mission command to trigger a parachute |action (0=disable, 1=enable, 2=release, for some systems see PARACHUTE_ACTION enum, not in general message set.)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_MOUNT_CONTROL_QUAT=220, /* Mission command to control a camera or antenna mount, using a quaternion as reference. |q1 - quaternion param #1| q2 - quaternion param #2| q3 - quaternion param #3| q4 - quaternion param #4| Empty| Empty| Empty| */ MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty| */ + MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Compass/Motor interference calibration: 0: no, 1: yes| Empty| */ MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS=242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| */ MAV_CMD_PREFLIGHT_STORAGE=245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty| */ MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN=246, /* Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty| */ diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_airspeeds.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_airspeeds.h index bc47a547ad..c834a3df09 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_airspeeds.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_airspeeds.h @@ -212,6 +212,50 @@ static inline void mavlink_msg_airspeeds_send(mavlink_channel_t chan, uint32_t t #endif } +#if MAVLINK_MSG_ID_AIRSPEEDS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_airspeeds_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, int16_t airspeed_imu, int16_t airspeed_pitot, int16_t airspeed_hot_wire, int16_t airspeed_ultrasonic, int16_t aoa, int16_t aoy) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int16_t(buf, 4, airspeed_imu); + _mav_put_int16_t(buf, 6, airspeed_pitot); + _mav_put_int16_t(buf, 8, airspeed_hot_wire); + _mav_put_int16_t(buf, 10, airspeed_ultrasonic); + _mav_put_int16_t(buf, 12, aoa); + _mav_put_int16_t(buf, 14, aoy); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEEDS, buf, MAVLINK_MSG_ID_AIRSPEEDS_LEN, MAVLINK_MSG_ID_AIRSPEEDS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEEDS, buf, MAVLINK_MSG_ID_AIRSPEEDS_LEN); +#endif +#else + mavlink_airspeeds_t *packet = (mavlink_airspeeds_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->airspeed_imu = airspeed_imu; + packet->airspeed_pitot = airspeed_pitot; + packet->airspeed_hot_wire = airspeed_hot_wire; + packet->airspeed_ultrasonic = airspeed_ultrasonic; + packet->aoa = aoa; + packet->aoy = aoy; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEEDS, (const char *)packet, MAVLINK_MSG_ID_AIRSPEEDS_LEN, MAVLINK_MSG_ID_AIRSPEEDS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIRSPEEDS, (const char *)packet, MAVLINK_MSG_ID_AIRSPEEDS_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE AIRSPEEDS UNPACKING diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_altitudes.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_altitudes.h index e64787cc73..b009068ffc 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_altitudes.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_altitudes.h @@ -212,6 +212,50 @@ static inline void mavlink_msg_altitudes_send(mavlink_channel_t chan, uint32_t t #endif } +#if MAVLINK_MSG_ID_ALTITUDES_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_altitudes_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, int32_t alt_gps, int32_t alt_imu, int32_t alt_barometric, int32_t alt_optical_flow, int32_t alt_range_finder, int32_t alt_extra) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int32_t(buf, 4, alt_gps); + _mav_put_int32_t(buf, 8, alt_imu); + _mav_put_int32_t(buf, 12, alt_barometric); + _mav_put_int32_t(buf, 16, alt_optical_flow); + _mav_put_int32_t(buf, 20, alt_range_finder); + _mav_put_int32_t(buf, 24, alt_extra); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ALTITUDES, buf, MAVLINK_MSG_ID_ALTITUDES_LEN, MAVLINK_MSG_ID_ALTITUDES_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ALTITUDES, buf, MAVLINK_MSG_ID_ALTITUDES_LEN); +#endif +#else + mavlink_altitudes_t *packet = (mavlink_altitudes_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->alt_gps = alt_gps; + packet->alt_imu = alt_imu; + packet->alt_barometric = alt_barometric; + packet->alt_optical_flow = alt_optical_flow; + packet->alt_range_finder = alt_range_finder; + packet->alt_extra = alt_extra; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ALTITUDES, (const char *)packet, MAVLINK_MSG_ID_ALTITUDES_LEN, MAVLINK_MSG_ID_ALTITUDES_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ALTITUDES, (const char *)packet, MAVLINK_MSG_ID_ALTITUDES_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE ALTITUDES UNPACKING diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function.h index 581bd35dc5..e7b803a14b 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function.h @@ -206,6 +206,48 @@ static inline void mavlink_msg_flexifunction_buffer_function_send(mavlink_channe #endif } +#if MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_flexifunction_buffer_function_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t func_index, uint16_t func_count, uint16_t data_address, uint16_t data_size, const int8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, func_index); + _mav_put_uint16_t(buf, 2, func_count); + _mav_put_uint16_t(buf, 4, data_address); + _mav_put_uint16_t(buf, 6, data_size); + _mav_put_uint8_t(buf, 8, target_system); + _mav_put_uint8_t(buf, 9, target_component); + _mav_put_int8_t_array(buf, 10, data, 48); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN); +#endif +#else + mavlink_flexifunction_buffer_function_t *packet = (mavlink_flexifunction_buffer_function_t *)msgbuf; + packet->func_index = func_index; + packet->func_count = func_count; + packet->data_address = data_address; + packet->data_size = data_size; + packet->target_system = target_system; + packet->target_component = target_component; + mav_array_memcpy(packet->data, data, sizeof(int8_t)*48); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE FLEXIFUNCTION_BUFFER_FUNCTION UNPACKING diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function_ack.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function_ack.h index 790afd52b3..06631b40ed 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function_ack.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_buffer_function_ack.h @@ -179,6 +179,44 @@ static inline void mavlink_msg_flexifunction_buffer_function_ack_send(mavlink_ch #endif } +#if MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_flexifunction_buffer_function_ack_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t func_index, uint16_t result) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, func_index); + _mav_put_uint16_t(buf, 2, result); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN); +#endif +#else + mavlink_flexifunction_buffer_function_ack_t *packet = (mavlink_flexifunction_buffer_function_ack_t *)msgbuf; + packet->func_index = func_index; + packet->result = result; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_BUFFER_FUNCTION_ACK_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE FLEXIFUNCTION_BUFFER_FUNCTION_ACK UNPACKING diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command.h index ce722c8a4d..ac8366eefe 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command.h @@ -168,6 +168,42 @@ static inline void mavlink_msg_flexifunction_command_send(mavlink_channel_t chan #endif } +#if MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_flexifunction_command_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t command_type) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, command_type); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN); +#endif +#else + mavlink_flexifunction_command_t *packet = (mavlink_flexifunction_command_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + packet->command_type = command_type; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE FLEXIFUNCTION_COMMAND UNPACKING diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command_ack.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command_ack.h index 070dc4bf8d..c88e2dc39f 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command_ack.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_command_ack.h @@ -157,6 +157,40 @@ static inline void mavlink_msg_flexifunction_command_ack_send(mavlink_channel_t #endif } +#if MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_flexifunction_command_ack_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t command_type, uint16_t result) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, command_type); + _mav_put_uint16_t(buf, 2, result); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN); +#endif +#else + mavlink_flexifunction_command_ack_t *packet = (mavlink_flexifunction_command_ack_t *)msgbuf; + packet->command_type = command_type; + packet->result = result; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_COMMAND_ACK_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE FLEXIFUNCTION_COMMAND_ACK UNPACKING diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory.h index ef262a6b1d..619f810c55 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory.h @@ -195,6 +195,46 @@ static inline void mavlink_msg_flexifunction_directory_send(mavlink_channel_t ch #endif } +#if MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_flexifunction_directory_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t directory_type, uint8_t start_index, uint8_t count, const int8_t *directory_data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, directory_type); + _mav_put_uint8_t(buf, 3, start_index); + _mav_put_uint8_t(buf, 4, count); + _mav_put_int8_t_array(buf, 5, directory_data, 48); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN); +#endif +#else + mavlink_flexifunction_directory_t *packet = (mavlink_flexifunction_directory_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + packet->directory_type = directory_type; + packet->start_index = start_index; + packet->count = count; + mav_array_memcpy(packet->directory_data, directory_data, sizeof(int8_t)*48); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE FLEXIFUNCTION_DIRECTORY UNPACKING diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory_ack.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory_ack.h index d3a386ce54..0287183d85 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory_ack.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_directory_ack.h @@ -201,6 +201,48 @@ static inline void mavlink_msg_flexifunction_directory_ack_send(mavlink_channel_ #endif } +#if MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_flexifunction_directory_ack_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t directory_type, uint8_t start_index, uint8_t count, uint16_t result) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, result); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, directory_type); + _mav_put_uint8_t(buf, 5, start_index); + _mav_put_uint8_t(buf, 6, count); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN); +#endif +#else + mavlink_flexifunction_directory_ack_t *packet = (mavlink_flexifunction_directory_ack_t *)msgbuf; + packet->result = result; + packet->target_system = target_system; + packet->target_component = target_component; + packet->directory_type = directory_type; + packet->start_index = start_index; + packet->count = count; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_DIRECTORY_ACK_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE FLEXIFUNCTION_DIRECTORY_ACK UNPACKING diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_read_req.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_read_req.h index e50f77b080..570782e792 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_read_req.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_read_req.h @@ -179,6 +179,44 @@ static inline void mavlink_msg_flexifunction_read_req_send(mavlink_channel_t cha #endif } +#if MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_flexifunction_read_req_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int16_t read_req_type, int16_t data_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int16_t(buf, 0, read_req_type); + _mav_put_int16_t(buf, 2, data_index); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN); +#endif +#else + mavlink_flexifunction_read_req_t *packet = (mavlink_flexifunction_read_req_t *)msgbuf; + packet->read_req_type = read_req_type; + packet->data_index = data_index; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_READ_REQ_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE FLEXIFUNCTION_READ_REQ UNPACKING diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_set.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_set.h index 41afb3811d..c10000640d 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_set.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_flexifunction_set.h @@ -157,6 +157,40 @@ static inline void mavlink_msg_flexifunction_set_send(mavlink_channel_t chan, ui #endif } +#if MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_flexifunction_set_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_SET, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_SET, buf, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN); +#endif +#else + mavlink_flexifunction_set_t *packet = (mavlink_flexifunction_set_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_SET, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLEXIFUNCTION_SET, (const char *)packet, MAVLINK_MSG_ID_FLEXIFUNCTION_SET_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE FLEXIFUNCTION_SET UNPACKING diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f13.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f13.h index d21ab48168..bbb753a06b 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f13.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f13.h @@ -179,6 +179,44 @@ static inline void mavlink_msg_serial_udb_extra_f13_send(mavlink_channel_t chan, #endif } +#if MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_serial_udb_extra_f13_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, int16_t sue_week_no, int32_t sue_lat_origin, int32_t sue_lon_origin, int32_t sue_alt_origin) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int32_t(buf, 0, sue_lat_origin); + _mav_put_int32_t(buf, 4, sue_lon_origin); + _mav_put_int32_t(buf, 8, sue_alt_origin); + _mav_put_int16_t(buf, 12, sue_week_no); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN); +#endif +#else + mavlink_serial_udb_extra_f13_t *packet = (mavlink_serial_udb_extra_f13_t *)msgbuf; + packet->sue_lat_origin = sue_lat_origin; + packet->sue_lon_origin = sue_lon_origin; + packet->sue_alt_origin = sue_alt_origin; + packet->sue_week_no = sue_week_no; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F13_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SERIAL_UDB_EXTRA_F13 UNPACKING diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f14.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f14.h index 6ac12f28a6..112564d006 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f14.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f14.h @@ -256,6 +256,58 @@ static inline void mavlink_msg_serial_udb_extra_f14_send(mavlink_channel_t chan, #endif } +#if MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_serial_udb_extra_f14_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t sue_WIND_ESTIMATION, uint8_t sue_GPS_TYPE, uint8_t sue_DR, uint8_t sue_BOARD_TYPE, uint8_t sue_AIRFRAME, int16_t sue_RCON, int16_t sue_TRAP_FLAGS, uint32_t sue_TRAP_SOURCE, int16_t sue_osc_fail_count, uint8_t sue_CLOCK_CONFIG, uint8_t sue_FLIGHT_PLAN_TYPE) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, sue_TRAP_SOURCE); + _mav_put_int16_t(buf, 4, sue_RCON); + _mav_put_int16_t(buf, 6, sue_TRAP_FLAGS); + _mav_put_int16_t(buf, 8, sue_osc_fail_count); + _mav_put_uint8_t(buf, 10, sue_WIND_ESTIMATION); + _mav_put_uint8_t(buf, 11, sue_GPS_TYPE); + _mav_put_uint8_t(buf, 12, sue_DR); + _mav_put_uint8_t(buf, 13, sue_BOARD_TYPE); + _mav_put_uint8_t(buf, 14, sue_AIRFRAME); + _mav_put_uint8_t(buf, 15, sue_CLOCK_CONFIG); + _mav_put_uint8_t(buf, 16, sue_FLIGHT_PLAN_TYPE); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN); +#endif +#else + mavlink_serial_udb_extra_f14_t *packet = (mavlink_serial_udb_extra_f14_t *)msgbuf; + packet->sue_TRAP_SOURCE = sue_TRAP_SOURCE; + packet->sue_RCON = sue_RCON; + packet->sue_TRAP_FLAGS = sue_TRAP_FLAGS; + packet->sue_osc_fail_count = sue_osc_fail_count; + packet->sue_WIND_ESTIMATION = sue_WIND_ESTIMATION; + packet->sue_GPS_TYPE = sue_GPS_TYPE; + packet->sue_DR = sue_DR; + packet->sue_BOARD_TYPE = sue_BOARD_TYPE; + packet->sue_AIRFRAME = sue_AIRFRAME; + packet->sue_CLOCK_CONFIG = sue_CLOCK_CONFIG; + packet->sue_FLIGHT_PLAN_TYPE = sue_FLIGHT_PLAN_TYPE; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F14_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SERIAL_UDB_EXTRA_F14 UNPACKING diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f15.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f15.h index 10c3f4ca4f..77666407de 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f15.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f15.h @@ -158,6 +158,40 @@ static inline void mavlink_msg_serial_udb_extra_f15_send(mavlink_channel_t chan, #endif } +#if MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_serial_udb_extra_f15_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, const uint8_t *sue_ID_VEHICLE_MODEL_NAME, const uint8_t *sue_ID_VEHICLE_REGISTRATION) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + + _mav_put_uint8_t_array(buf, 0, sue_ID_VEHICLE_MODEL_NAME, 40); + _mav_put_uint8_t_array(buf, 40, sue_ID_VEHICLE_REGISTRATION, 20); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN); +#endif +#else + mavlink_serial_udb_extra_f15_t *packet = (mavlink_serial_udb_extra_f15_t *)msgbuf; + + mav_array_memcpy(packet->sue_ID_VEHICLE_MODEL_NAME, sue_ID_VEHICLE_MODEL_NAME, sizeof(uint8_t)*40); + mav_array_memcpy(packet->sue_ID_VEHICLE_REGISTRATION, sue_ID_VEHICLE_REGISTRATION, sizeof(uint8_t)*20); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F15_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SERIAL_UDB_EXTRA_F15 UNPACKING diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f16.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f16.h index 659e6b7c52..ca9f0556f7 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f16.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f16.h @@ -158,6 +158,40 @@ static inline void mavlink_msg_serial_udb_extra_f16_send(mavlink_channel_t chan, #endif } +#if MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_serial_udb_extra_f16_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, const uint8_t *sue_ID_LEAD_PILOT, const uint8_t *sue_ID_DIY_DRONES_URL) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + + _mav_put_uint8_t_array(buf, 0, sue_ID_LEAD_PILOT, 40); + _mav_put_uint8_t_array(buf, 40, sue_ID_DIY_DRONES_URL, 70); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN); +#endif +#else + mavlink_serial_udb_extra_f16_t *packet = (mavlink_serial_udb_extra_f16_t *)msgbuf; + + mav_array_memcpy(packet->sue_ID_LEAD_PILOT, sue_ID_LEAD_PILOT, sizeof(uint8_t)*40); + mav_array_memcpy(packet->sue_ID_DIY_DRONES_URL, sue_ID_DIY_DRONES_URL, sizeof(uint8_t)*70); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F16_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SERIAL_UDB_EXTRA_F16 UNPACKING diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_a.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_a.h index 15ba68a346..41b9f09c00 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_a.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_a.h @@ -443,6 +443,92 @@ static inline void mavlink_msg_serial_udb_extra_f2_a_send(mavlink_channel_t chan #endif } +#if MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_serial_udb_extra_f2_a_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t sue_time, uint8_t sue_status, int32_t sue_latitude, int32_t sue_longitude, int32_t sue_altitude, uint16_t sue_waypoint_index, int16_t sue_rmat0, int16_t sue_rmat1, int16_t sue_rmat2, int16_t sue_rmat3, int16_t sue_rmat4, int16_t sue_rmat5, int16_t sue_rmat6, int16_t sue_rmat7, int16_t sue_rmat8, uint16_t sue_cog, int16_t sue_sog, uint16_t sue_cpu_load, int16_t sue_voltage_milis, uint16_t sue_air_speed_3DIMU, int16_t sue_estimated_wind_0, int16_t sue_estimated_wind_1, int16_t sue_estimated_wind_2, int16_t sue_magFieldEarth0, int16_t sue_magFieldEarth1, int16_t sue_magFieldEarth2, int16_t sue_svs, int16_t sue_hdop) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, sue_time); + _mav_put_int32_t(buf, 4, sue_latitude); + _mav_put_int32_t(buf, 8, sue_longitude); + _mav_put_int32_t(buf, 12, sue_altitude); + _mav_put_uint16_t(buf, 16, sue_waypoint_index); + _mav_put_int16_t(buf, 18, sue_rmat0); + _mav_put_int16_t(buf, 20, sue_rmat1); + _mav_put_int16_t(buf, 22, sue_rmat2); + _mav_put_int16_t(buf, 24, sue_rmat3); + _mav_put_int16_t(buf, 26, sue_rmat4); + _mav_put_int16_t(buf, 28, sue_rmat5); + _mav_put_int16_t(buf, 30, sue_rmat6); + _mav_put_int16_t(buf, 32, sue_rmat7); + _mav_put_int16_t(buf, 34, sue_rmat8); + _mav_put_uint16_t(buf, 36, sue_cog); + _mav_put_int16_t(buf, 38, sue_sog); + _mav_put_uint16_t(buf, 40, sue_cpu_load); + _mav_put_int16_t(buf, 42, sue_voltage_milis); + _mav_put_uint16_t(buf, 44, sue_air_speed_3DIMU); + _mav_put_int16_t(buf, 46, sue_estimated_wind_0); + _mav_put_int16_t(buf, 48, sue_estimated_wind_1); + _mav_put_int16_t(buf, 50, sue_estimated_wind_2); + _mav_put_int16_t(buf, 52, sue_magFieldEarth0); + _mav_put_int16_t(buf, 54, sue_magFieldEarth1); + _mav_put_int16_t(buf, 56, sue_magFieldEarth2); + _mav_put_int16_t(buf, 58, sue_svs); + _mav_put_int16_t(buf, 60, sue_hdop); + _mav_put_uint8_t(buf, 62, sue_status); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN); +#endif +#else + mavlink_serial_udb_extra_f2_a_t *packet = (mavlink_serial_udb_extra_f2_a_t *)msgbuf; + packet->sue_time = sue_time; + packet->sue_latitude = sue_latitude; + packet->sue_longitude = sue_longitude; + packet->sue_altitude = sue_altitude; + packet->sue_waypoint_index = sue_waypoint_index; + packet->sue_rmat0 = sue_rmat0; + packet->sue_rmat1 = sue_rmat1; + packet->sue_rmat2 = sue_rmat2; + packet->sue_rmat3 = sue_rmat3; + packet->sue_rmat4 = sue_rmat4; + packet->sue_rmat5 = sue_rmat5; + packet->sue_rmat6 = sue_rmat6; + packet->sue_rmat7 = sue_rmat7; + packet->sue_rmat8 = sue_rmat8; + packet->sue_cog = sue_cog; + packet->sue_sog = sue_sog; + packet->sue_cpu_load = sue_cpu_load; + packet->sue_voltage_milis = sue_voltage_milis; + packet->sue_air_speed_3DIMU = sue_air_speed_3DIMU; + packet->sue_estimated_wind_0 = sue_estimated_wind_0; + packet->sue_estimated_wind_1 = sue_estimated_wind_1; + packet->sue_estimated_wind_2 = sue_estimated_wind_2; + packet->sue_magFieldEarth0 = sue_magFieldEarth0; + packet->sue_magFieldEarth1 = sue_magFieldEarth1; + packet->sue_magFieldEarth2 = sue_magFieldEarth2; + packet->sue_svs = sue_svs; + packet->sue_hdop = sue_hdop; + packet->sue_status = sue_status; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_A_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SERIAL_UDB_EXTRA_F2_A UNPACKING diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_b.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_b.h index 7cb8c87da5..3e1757f743 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_b.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f2_b.h @@ -498,6 +498,102 @@ static inline void mavlink_msg_serial_udb_extra_f2_b_send(mavlink_channel_t chan #endif } +#if MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_serial_udb_extra_f2_b_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t sue_time, int16_t sue_pwm_input_1, int16_t sue_pwm_input_2, int16_t sue_pwm_input_3, int16_t sue_pwm_input_4, int16_t sue_pwm_input_5, int16_t sue_pwm_input_6, int16_t sue_pwm_input_7, int16_t sue_pwm_input_8, int16_t sue_pwm_input_9, int16_t sue_pwm_input_10, int16_t sue_pwm_output_1, int16_t sue_pwm_output_2, int16_t sue_pwm_output_3, int16_t sue_pwm_output_4, int16_t sue_pwm_output_5, int16_t sue_pwm_output_6, int16_t sue_pwm_output_7, int16_t sue_pwm_output_8, int16_t sue_pwm_output_9, int16_t sue_pwm_output_10, int16_t sue_imu_location_x, int16_t sue_imu_location_y, int16_t sue_imu_location_z, uint32_t sue_flags, int16_t sue_osc_fails, int16_t sue_imu_velocity_x, int16_t sue_imu_velocity_y, int16_t sue_imu_velocity_z, int16_t sue_waypoint_goal_x, int16_t sue_waypoint_goal_y, int16_t sue_waypoint_goal_z, int16_t sue_memory_stack_free) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, sue_time); + _mav_put_uint32_t(buf, 4, sue_flags); + _mav_put_int16_t(buf, 8, sue_pwm_input_1); + _mav_put_int16_t(buf, 10, sue_pwm_input_2); + _mav_put_int16_t(buf, 12, sue_pwm_input_3); + _mav_put_int16_t(buf, 14, sue_pwm_input_4); + _mav_put_int16_t(buf, 16, sue_pwm_input_5); + _mav_put_int16_t(buf, 18, sue_pwm_input_6); + _mav_put_int16_t(buf, 20, sue_pwm_input_7); + _mav_put_int16_t(buf, 22, sue_pwm_input_8); + _mav_put_int16_t(buf, 24, sue_pwm_input_9); + _mav_put_int16_t(buf, 26, sue_pwm_input_10); + _mav_put_int16_t(buf, 28, sue_pwm_output_1); + _mav_put_int16_t(buf, 30, sue_pwm_output_2); + _mav_put_int16_t(buf, 32, sue_pwm_output_3); + _mav_put_int16_t(buf, 34, sue_pwm_output_4); + _mav_put_int16_t(buf, 36, sue_pwm_output_5); + _mav_put_int16_t(buf, 38, sue_pwm_output_6); + _mav_put_int16_t(buf, 40, sue_pwm_output_7); + _mav_put_int16_t(buf, 42, sue_pwm_output_8); + _mav_put_int16_t(buf, 44, sue_pwm_output_9); + _mav_put_int16_t(buf, 46, sue_pwm_output_10); + _mav_put_int16_t(buf, 48, sue_imu_location_x); + _mav_put_int16_t(buf, 50, sue_imu_location_y); + _mav_put_int16_t(buf, 52, sue_imu_location_z); + _mav_put_int16_t(buf, 54, sue_osc_fails); + _mav_put_int16_t(buf, 56, sue_imu_velocity_x); + _mav_put_int16_t(buf, 58, sue_imu_velocity_y); + _mav_put_int16_t(buf, 60, sue_imu_velocity_z); + _mav_put_int16_t(buf, 62, sue_waypoint_goal_x); + _mav_put_int16_t(buf, 64, sue_waypoint_goal_y); + _mav_put_int16_t(buf, 66, sue_waypoint_goal_z); + _mav_put_int16_t(buf, 68, sue_memory_stack_free); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN); +#endif +#else + mavlink_serial_udb_extra_f2_b_t *packet = (mavlink_serial_udb_extra_f2_b_t *)msgbuf; + packet->sue_time = sue_time; + packet->sue_flags = sue_flags; + packet->sue_pwm_input_1 = sue_pwm_input_1; + packet->sue_pwm_input_2 = sue_pwm_input_2; + packet->sue_pwm_input_3 = sue_pwm_input_3; + packet->sue_pwm_input_4 = sue_pwm_input_4; + packet->sue_pwm_input_5 = sue_pwm_input_5; + packet->sue_pwm_input_6 = sue_pwm_input_6; + packet->sue_pwm_input_7 = sue_pwm_input_7; + packet->sue_pwm_input_8 = sue_pwm_input_8; + packet->sue_pwm_input_9 = sue_pwm_input_9; + packet->sue_pwm_input_10 = sue_pwm_input_10; + packet->sue_pwm_output_1 = sue_pwm_output_1; + packet->sue_pwm_output_2 = sue_pwm_output_2; + packet->sue_pwm_output_3 = sue_pwm_output_3; + packet->sue_pwm_output_4 = sue_pwm_output_4; + packet->sue_pwm_output_5 = sue_pwm_output_5; + packet->sue_pwm_output_6 = sue_pwm_output_6; + packet->sue_pwm_output_7 = sue_pwm_output_7; + packet->sue_pwm_output_8 = sue_pwm_output_8; + packet->sue_pwm_output_9 = sue_pwm_output_9; + packet->sue_pwm_output_10 = sue_pwm_output_10; + packet->sue_imu_location_x = sue_imu_location_x; + packet->sue_imu_location_y = sue_imu_location_y; + packet->sue_imu_location_z = sue_imu_location_z; + packet->sue_osc_fails = sue_osc_fails; + packet->sue_imu_velocity_x = sue_imu_velocity_x; + packet->sue_imu_velocity_y = sue_imu_velocity_y; + packet->sue_imu_velocity_z = sue_imu_velocity_z; + packet->sue_waypoint_goal_x = sue_waypoint_goal_x; + packet->sue_waypoint_goal_y = sue_waypoint_goal_y; + packet->sue_waypoint_goal_z = sue_waypoint_goal_z; + packet->sue_memory_stack_free = sue_memory_stack_free; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F2_B_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SERIAL_UDB_EXTRA_F2_B UNPACKING diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f4.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f4.h index 77c616cc2e..edd1e924c8 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f4.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f4.h @@ -245,6 +245,56 @@ static inline void mavlink_msg_serial_udb_extra_f4_send(mavlink_channel_t chan, #endif } +#if MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_serial_udb_extra_f4_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t sue_ROLL_STABILIZATION_AILERONS, uint8_t sue_ROLL_STABILIZATION_RUDDER, uint8_t sue_PITCH_STABILIZATION, uint8_t sue_YAW_STABILIZATION_RUDDER, uint8_t sue_YAW_STABILIZATION_AILERON, uint8_t sue_AILERON_NAVIGATION, uint8_t sue_RUDDER_NAVIGATION, uint8_t sue_ALTITUDEHOLD_STABILIZED, uint8_t sue_ALTITUDEHOLD_WAYPOINT, uint8_t sue_RACING_MODE) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, sue_ROLL_STABILIZATION_AILERONS); + _mav_put_uint8_t(buf, 1, sue_ROLL_STABILIZATION_RUDDER); + _mav_put_uint8_t(buf, 2, sue_PITCH_STABILIZATION); + _mav_put_uint8_t(buf, 3, sue_YAW_STABILIZATION_RUDDER); + _mav_put_uint8_t(buf, 4, sue_YAW_STABILIZATION_AILERON); + _mav_put_uint8_t(buf, 5, sue_AILERON_NAVIGATION); + _mav_put_uint8_t(buf, 6, sue_RUDDER_NAVIGATION); + _mav_put_uint8_t(buf, 7, sue_ALTITUDEHOLD_STABILIZED); + _mav_put_uint8_t(buf, 8, sue_ALTITUDEHOLD_WAYPOINT); + _mav_put_uint8_t(buf, 9, sue_RACING_MODE); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN); +#endif +#else + mavlink_serial_udb_extra_f4_t *packet = (mavlink_serial_udb_extra_f4_t *)msgbuf; + packet->sue_ROLL_STABILIZATION_AILERONS = sue_ROLL_STABILIZATION_AILERONS; + packet->sue_ROLL_STABILIZATION_RUDDER = sue_ROLL_STABILIZATION_RUDDER; + packet->sue_PITCH_STABILIZATION = sue_PITCH_STABILIZATION; + packet->sue_YAW_STABILIZATION_RUDDER = sue_YAW_STABILIZATION_RUDDER; + packet->sue_YAW_STABILIZATION_AILERON = sue_YAW_STABILIZATION_AILERON; + packet->sue_AILERON_NAVIGATION = sue_AILERON_NAVIGATION; + packet->sue_RUDDER_NAVIGATION = sue_RUDDER_NAVIGATION; + packet->sue_ALTITUDEHOLD_STABILIZED = sue_ALTITUDEHOLD_STABILIZED; + packet->sue_ALTITUDEHOLD_WAYPOINT = sue_ALTITUDEHOLD_WAYPOINT; + packet->sue_RACING_MODE = sue_RACING_MODE; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F4_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SERIAL_UDB_EXTRA_F4 UNPACKING diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f5.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f5.h index e115a9b445..a821f65bb7 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f5.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f5.h @@ -201,6 +201,48 @@ static inline void mavlink_msg_serial_udb_extra_f5_send(mavlink_channel_t chan, #endif } +#if MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_serial_udb_extra_f5_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float sue_YAWKP_AILERON, float sue_YAWKD_AILERON, float sue_ROLLKP, float sue_ROLLKD, float sue_YAW_STABILIZATION_AILERON, float sue_AILERON_BOOST) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, sue_YAWKP_AILERON); + _mav_put_float(buf, 4, sue_YAWKD_AILERON); + _mav_put_float(buf, 8, sue_ROLLKP); + _mav_put_float(buf, 12, sue_ROLLKD); + _mav_put_float(buf, 16, sue_YAW_STABILIZATION_AILERON); + _mav_put_float(buf, 20, sue_AILERON_BOOST); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN); +#endif +#else + mavlink_serial_udb_extra_f5_t *packet = (mavlink_serial_udb_extra_f5_t *)msgbuf; + packet->sue_YAWKP_AILERON = sue_YAWKP_AILERON; + packet->sue_YAWKD_AILERON = sue_YAWKD_AILERON; + packet->sue_ROLLKP = sue_ROLLKP; + packet->sue_ROLLKD = sue_ROLLKD; + packet->sue_YAW_STABILIZATION_AILERON = sue_YAW_STABILIZATION_AILERON; + packet->sue_AILERON_BOOST = sue_AILERON_BOOST; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F5_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SERIAL_UDB_EXTRA_F5 UNPACKING diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f6.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f6.h index 656103d090..0eb24c9866 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f6.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f6.h @@ -190,6 +190,46 @@ static inline void mavlink_msg_serial_udb_extra_f6_send(mavlink_channel_t chan, #endif } +#if MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_serial_udb_extra_f6_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float sue_PITCHGAIN, float sue_PITCHKD, float sue_RUDDER_ELEV_MIX, float sue_ROLL_ELEV_MIX, float sue_ELEVATOR_BOOST) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, sue_PITCHGAIN); + _mav_put_float(buf, 4, sue_PITCHKD); + _mav_put_float(buf, 8, sue_RUDDER_ELEV_MIX); + _mav_put_float(buf, 12, sue_ROLL_ELEV_MIX); + _mav_put_float(buf, 16, sue_ELEVATOR_BOOST); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN); +#endif +#else + mavlink_serial_udb_extra_f6_t *packet = (mavlink_serial_udb_extra_f6_t *)msgbuf; + packet->sue_PITCHGAIN = sue_PITCHGAIN; + packet->sue_PITCHKD = sue_PITCHKD; + packet->sue_RUDDER_ELEV_MIX = sue_RUDDER_ELEV_MIX; + packet->sue_ROLL_ELEV_MIX = sue_ROLL_ELEV_MIX; + packet->sue_ELEVATOR_BOOST = sue_ELEVATOR_BOOST; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F6_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SERIAL_UDB_EXTRA_F6 UNPACKING diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f7.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f7.h index 51c98e6b7a..58de7d01c8 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f7.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f7.h @@ -201,6 +201,48 @@ static inline void mavlink_msg_serial_udb_extra_f7_send(mavlink_channel_t chan, #endif } +#if MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_serial_udb_extra_f7_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float sue_YAWKP_RUDDER, float sue_YAWKD_RUDDER, float sue_ROLLKP_RUDDER, float sue_ROLLKD_RUDDER, float sue_RUDDER_BOOST, float sue_RTL_PITCH_DOWN) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, sue_YAWKP_RUDDER); + _mav_put_float(buf, 4, sue_YAWKD_RUDDER); + _mav_put_float(buf, 8, sue_ROLLKP_RUDDER); + _mav_put_float(buf, 12, sue_ROLLKD_RUDDER); + _mav_put_float(buf, 16, sue_RUDDER_BOOST); + _mav_put_float(buf, 20, sue_RTL_PITCH_DOWN); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN); +#endif +#else + mavlink_serial_udb_extra_f7_t *packet = (mavlink_serial_udb_extra_f7_t *)msgbuf; + packet->sue_YAWKP_RUDDER = sue_YAWKP_RUDDER; + packet->sue_YAWKD_RUDDER = sue_YAWKD_RUDDER; + packet->sue_ROLLKP_RUDDER = sue_ROLLKP_RUDDER; + packet->sue_ROLLKD_RUDDER = sue_ROLLKD_RUDDER; + packet->sue_RUDDER_BOOST = sue_RUDDER_BOOST; + packet->sue_RTL_PITCH_DOWN = sue_RTL_PITCH_DOWN; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F7_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SERIAL_UDB_EXTRA_F7 UNPACKING diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f8.h b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f8.h index 8557a95e27..5cab816555 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f8.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/mavlink_msg_serial_udb_extra_f8.h @@ -212,6 +212,50 @@ static inline void mavlink_msg_serial_udb_extra_f8_send(mavlink_channel_t chan, #endif } +#if MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_serial_udb_extra_f8_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float sue_HEIGHT_TARGET_MAX, float sue_HEIGHT_TARGET_MIN, float sue_ALT_HOLD_THROTTLE_MIN, float sue_ALT_HOLD_THROTTLE_MAX, float sue_ALT_HOLD_PITCH_MIN, float sue_ALT_HOLD_PITCH_MAX, float sue_ALT_HOLD_PITCH_HIGH) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, sue_HEIGHT_TARGET_MAX); + _mav_put_float(buf, 4, sue_HEIGHT_TARGET_MIN); + _mav_put_float(buf, 8, sue_ALT_HOLD_THROTTLE_MIN); + _mav_put_float(buf, 12, sue_ALT_HOLD_THROTTLE_MAX); + _mav_put_float(buf, 16, sue_ALT_HOLD_PITCH_MIN); + _mav_put_float(buf, 20, sue_ALT_HOLD_PITCH_MAX); + _mav_put_float(buf, 24, sue_ALT_HOLD_PITCH_HIGH); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8, buf, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN); +#endif +#else + mavlink_serial_udb_extra_f8_t *packet = (mavlink_serial_udb_extra_f8_t *)msgbuf; + packet->sue_HEIGHT_TARGET_MAX = sue_HEIGHT_TARGET_MAX; + packet->sue_HEIGHT_TARGET_MIN = sue_HEIGHT_TARGET_MIN; + packet->sue_ALT_HOLD_THROTTLE_MIN = sue_ALT_HOLD_THROTTLE_MIN; + packet->sue_ALT_HOLD_THROTTLE_MAX = sue_ALT_HOLD_THROTTLE_MAX; + packet->sue_ALT_HOLD_PITCH_MIN = sue_ALT_HOLD_PITCH_MIN; + packet->sue_ALT_HOLD_PITCH_MAX = sue_ALT_HOLD_PITCH_MAX; + packet->sue_ALT_HOLD_PITCH_HIGH = sue_ALT_HOLD_PITCH_HIGH; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8, (const char *)packet, MAVLINK_MSG_ID_SERIAL_UDB_EXTRA_F8_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SERIAL_UDB_EXTRA_F8 UNPACKING diff --git a/mavlink/include/mavlink/v1.0/matrixpilot/version.h b/mavlink/include/mavlink/v1.0/matrixpilot/version.h index a8be16c424..50cbda6c78 100644 --- a/mavlink/include/mavlink/v1.0/matrixpilot/version.h +++ b/mavlink/include/mavlink/v1.0/matrixpilot/version.h @@ -5,7 +5,7 @@ #ifndef MAVLINK_VERSION_H #define MAVLINK_VERSION_H -#define MAVLINK_BUILD_DATE "Tue Feb 4 15:28:04 2014" +#define MAVLINK_BUILD_DATE "Sun Apr 13 09:38:32 2014" #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_attitude_control.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_attitude_control.h index ef5354d5e1..f1cac9f2cd 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_attitude_control.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_attitude_control.h @@ -234,6 +234,54 @@ static inline void mavlink_msg_attitude_control_send(mavlink_channel_t chan, uin #endif } +#if MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_attitude_control_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target, float roll, float pitch, float yaw, float thrust, uint8_t roll_manual, uint8_t pitch_manual, uint8_t yaw_manual, uint8_t thrust_manual) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, roll); + _mav_put_float(buf, 4, pitch); + _mav_put_float(buf, 8, yaw); + _mav_put_float(buf, 12, thrust); + _mav_put_uint8_t(buf, 16, target); + _mav_put_uint8_t(buf, 17, roll_manual); + _mav_put_uint8_t(buf, 18, pitch_manual); + _mav_put_uint8_t(buf, 19, yaw_manual); + _mav_put_uint8_t(buf, 20, thrust_manual); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_CONTROL, buf, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN, MAVLINK_MSG_ID_ATTITUDE_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_CONTROL, buf, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN); +#endif +#else + mavlink_attitude_control_t *packet = (mavlink_attitude_control_t *)msgbuf; + packet->roll = roll; + packet->pitch = pitch; + packet->yaw = yaw; + packet->thrust = thrust; + packet->target = target; + packet->roll_manual = roll_manual; + packet->pitch_manual = pitch_manual; + packet->yaw_manual = yaw_manual; + packet->thrust_manual = thrust_manual; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_CONTROL, (const char *)packet, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN, MAVLINK_MSG_ID_ATTITUDE_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_CONTROL, (const char *)packet, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE ATTITUDE_CONTROL UNPACKING diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_brief_feature.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_brief_feature.h index d41093792e..7bc27982a8 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_brief_feature.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_brief_feature.h @@ -217,6 +217,50 @@ static inline void mavlink_msg_brief_feature_send(mavlink_channel_t chan, float #endif } +#if MAVLINK_MSG_ID_BRIEF_FEATURE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_brief_feature_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float x, float y, float z, uint8_t orientation_assignment, uint16_t size, uint16_t orientation, const uint8_t *descriptor, float response) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, x); + _mav_put_float(buf, 4, y); + _mav_put_float(buf, 8, z); + _mav_put_float(buf, 12, response); + _mav_put_uint16_t(buf, 16, size); + _mav_put_uint16_t(buf, 18, orientation); + _mav_put_uint8_t(buf, 20, orientation_assignment); + _mav_put_uint8_t_array(buf, 21, descriptor, 32); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BRIEF_FEATURE, buf, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN, MAVLINK_MSG_ID_BRIEF_FEATURE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BRIEF_FEATURE, buf, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN); +#endif +#else + mavlink_brief_feature_t *packet = (mavlink_brief_feature_t *)msgbuf; + packet->x = x; + packet->y = y; + packet->z = z; + packet->response = response; + packet->size = size; + packet->orientation = orientation; + packet->orientation_assignment = orientation_assignment; + mav_array_memcpy(packet->descriptor, descriptor, sizeof(uint8_t)*32); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BRIEF_FEATURE, (const char *)packet, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN, MAVLINK_MSG_ID_BRIEF_FEATURE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BRIEF_FEATURE, (const char *)packet, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE BRIEF_FEATURE UNPACKING diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_available.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_available.h index ae4db825df..37270d49c0 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_available.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_available.h @@ -388,6 +388,82 @@ static inline void mavlink_msg_image_available_send(mavlink_channel_t chan, uint #endif } +#if MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_image_available_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t cam_id, uint8_t cam_no, uint64_t timestamp, uint64_t valid_until, uint32_t img_seq, uint32_t img_buf_index, uint16_t width, uint16_t height, uint16_t depth, uint8_t channels, uint32_t key, uint32_t exposure, float gain, float roll, float pitch, float yaw, float local_z, float lat, float lon, float alt, float ground_x, float ground_y, float ground_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, cam_id); + _mav_put_uint64_t(buf, 8, timestamp); + _mav_put_uint64_t(buf, 16, valid_until); + _mav_put_uint32_t(buf, 24, img_seq); + _mav_put_uint32_t(buf, 28, img_buf_index); + _mav_put_uint32_t(buf, 32, key); + _mav_put_uint32_t(buf, 36, exposure); + _mav_put_float(buf, 40, gain); + _mav_put_float(buf, 44, roll); + _mav_put_float(buf, 48, pitch); + _mav_put_float(buf, 52, yaw); + _mav_put_float(buf, 56, local_z); + _mav_put_float(buf, 60, lat); + _mav_put_float(buf, 64, lon); + _mav_put_float(buf, 68, alt); + _mav_put_float(buf, 72, ground_x); + _mav_put_float(buf, 76, ground_y); + _mav_put_float(buf, 80, ground_z); + _mav_put_uint16_t(buf, 84, width); + _mav_put_uint16_t(buf, 86, height); + _mav_put_uint16_t(buf, 88, depth); + _mav_put_uint8_t(buf, 90, cam_no); + _mav_put_uint8_t(buf, 91, channels); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_AVAILABLE, buf, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN, MAVLINK_MSG_ID_IMAGE_AVAILABLE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_AVAILABLE, buf, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN); +#endif +#else + mavlink_image_available_t *packet = (mavlink_image_available_t *)msgbuf; + packet->cam_id = cam_id; + packet->timestamp = timestamp; + packet->valid_until = valid_until; + packet->img_seq = img_seq; + packet->img_buf_index = img_buf_index; + packet->key = key; + packet->exposure = exposure; + packet->gain = gain; + packet->roll = roll; + packet->pitch = pitch; + packet->yaw = yaw; + packet->local_z = local_z; + packet->lat = lat; + packet->lon = lon; + packet->alt = alt; + packet->ground_x = ground_x; + packet->ground_y = ground_y; + packet->ground_z = ground_z; + packet->width = width; + packet->height = height; + packet->depth = depth; + packet->cam_no = cam_no; + packet->channels = channels; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_AVAILABLE, (const char *)packet, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN, MAVLINK_MSG_ID_IMAGE_AVAILABLE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_AVAILABLE, (const char *)packet, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE IMAGE_AVAILABLE UNPACKING diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_trigger_control.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_trigger_control.h index 664574be94..85f8eff556 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_trigger_control.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_trigger_control.h @@ -146,6 +146,38 @@ static inline void mavlink_msg_image_trigger_control_send(mavlink_channel_t chan #endif } +#if MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_image_trigger_control_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t enable) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, enable); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL, buf, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL, buf, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN); +#endif +#else + mavlink_image_trigger_control_t *packet = (mavlink_image_trigger_control_t *)msgbuf; + packet->enable = enable; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL, (const char *)packet, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL, (const char *)packet, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE IMAGE_TRIGGER_CONTROL UNPACKING diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_triggered.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_triggered.h index f3a2243afc..b438c74ea5 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_triggered.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_image_triggered.h @@ -267,6 +267,60 @@ static inline void mavlink_msg_image_triggered_send(mavlink_channel_t chan, uint #endif } +#if MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_image_triggered_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t timestamp, uint32_t seq, float roll, float pitch, float yaw, float local_z, float lat, float lon, float alt, float ground_x, float ground_y, float ground_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, timestamp); + _mav_put_uint32_t(buf, 8, seq); + _mav_put_float(buf, 12, roll); + _mav_put_float(buf, 16, pitch); + _mav_put_float(buf, 20, yaw); + _mav_put_float(buf, 24, local_z); + _mav_put_float(buf, 28, lat); + _mav_put_float(buf, 32, lon); + _mav_put_float(buf, 36, alt); + _mav_put_float(buf, 40, ground_x); + _mav_put_float(buf, 44, ground_y); + _mav_put_float(buf, 48, ground_z); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGERED, buf, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN, MAVLINK_MSG_ID_IMAGE_TRIGGERED_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGERED, buf, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN); +#endif +#else + mavlink_image_triggered_t *packet = (mavlink_image_triggered_t *)msgbuf; + packet->timestamp = timestamp; + packet->seq = seq; + packet->roll = roll; + packet->pitch = pitch; + packet->yaw = yaw; + packet->local_z = local_z; + packet->lat = lat; + packet->lon = lon; + packet->alt = alt; + packet->ground_x = ground_x; + packet->ground_y = ground_y; + packet->ground_z = ground_z; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGERED, (const char *)packet, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN, MAVLINK_MSG_ID_IMAGE_TRIGGERED_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGERED, (const char *)packet, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE IMAGE_TRIGGERED UNPACKING diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_marker.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_marker.h index 6bdcceaf9a..05452637ea 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_marker.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_marker.h @@ -212,6 +212,50 @@ static inline void mavlink_msg_marker_send(mavlink_channel_t chan, uint16_t id, #endif } +#if MAVLINK_MSG_ID_MARKER_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_marker_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t id, float x, float y, float z, float roll, float pitch, float yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, x); + _mav_put_float(buf, 4, y); + _mav_put_float(buf, 8, z); + _mav_put_float(buf, 12, roll); + _mav_put_float(buf, 16, pitch); + _mav_put_float(buf, 20, yaw); + _mav_put_uint16_t(buf, 24, id); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MARKER, buf, MAVLINK_MSG_ID_MARKER_LEN, MAVLINK_MSG_ID_MARKER_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MARKER, buf, MAVLINK_MSG_ID_MARKER_LEN); +#endif +#else + mavlink_marker_t *packet = (mavlink_marker_t *)msgbuf; + packet->x = x; + packet->y = y; + packet->z = z; + packet->roll = roll; + packet->pitch = pitch; + packet->yaw = yaw; + packet->id = id; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MARKER, (const char *)packet, MAVLINK_MSG_ID_MARKER_LEN, MAVLINK_MSG_ID_MARKER_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MARKER, (const char *)packet, MAVLINK_MSG_ID_MARKER_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE MARKER UNPACKING diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_pattern_detected.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_pattern_detected.h index aa7b827a36..bde400da9a 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_pattern_detected.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_pattern_detected.h @@ -173,6 +173,42 @@ static inline void mavlink_msg_pattern_detected_send(mavlink_channel_t chan, uin #endif } +#if MAVLINK_MSG_ID_PATTERN_DETECTED_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_pattern_detected_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t type, float confidence, const char *file, uint8_t detected) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, confidence); + _mav_put_uint8_t(buf, 4, type); + _mav_put_uint8_t(buf, 105, detected); + _mav_put_char_array(buf, 5, file, 100); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PATTERN_DETECTED, buf, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN, MAVLINK_MSG_ID_PATTERN_DETECTED_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PATTERN_DETECTED, buf, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN); +#endif +#else + mavlink_pattern_detected_t *packet = (mavlink_pattern_detected_t *)msgbuf; + packet->confidence = confidence; + packet->type = type; + packet->detected = detected; + mav_array_memcpy(packet->file, file, sizeof(char)*100); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PATTERN_DETECTED, (const char *)packet, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN, MAVLINK_MSG_ID_PATTERN_DETECTED_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PATTERN_DETECTED, (const char *)packet, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE PATTERN_DETECTED UNPACKING diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest.h index 913a52897e..86e9345194 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest.h @@ -217,6 +217,50 @@ static inline void mavlink_msg_point_of_interest_send(mavlink_channel_t chan, ui #endif } +#if MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_point_of_interest_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float x, float y, float z, const char *name) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, x); + _mav_put_float(buf, 4, y); + _mav_put_float(buf, 8, z); + _mav_put_uint16_t(buf, 12, timeout); + _mav_put_uint8_t(buf, 14, type); + _mav_put_uint8_t(buf, 15, color); + _mav_put_uint8_t(buf, 16, coordinate_system); + _mav_put_char_array(buf, 17, name, 26); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST, buf, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN, MAVLINK_MSG_ID_POINT_OF_INTEREST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST, buf, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN); +#endif +#else + mavlink_point_of_interest_t *packet = (mavlink_point_of_interest_t *)msgbuf; + packet->x = x; + packet->y = y; + packet->z = z; + packet->timeout = timeout; + packet->type = type; + packet->color = color; + packet->coordinate_system = coordinate_system; + mav_array_memcpy(packet->name, name, sizeof(char)*26); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST, (const char *)packet, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN, MAVLINK_MSG_ID_POINT_OF_INTEREST_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST, (const char *)packet, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE POINT_OF_INTEREST UNPACKING diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest_connection.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest_connection.h index e244364318..e5c04a851c 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest_connection.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_point_of_interest_connection.h @@ -250,6 +250,56 @@ static inline void mavlink_msg_point_of_interest_connection_send(mavlink_channel #endif } +#if MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_point_of_interest_connection_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float xp1, float yp1, float zp1, float xp2, float yp2, float zp2, const char *name) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, xp1); + _mav_put_float(buf, 4, yp1); + _mav_put_float(buf, 8, zp1); + _mav_put_float(buf, 12, xp2); + _mav_put_float(buf, 16, yp2); + _mav_put_float(buf, 20, zp2); + _mav_put_uint16_t(buf, 24, timeout); + _mav_put_uint8_t(buf, 26, type); + _mav_put_uint8_t(buf, 27, color); + _mav_put_uint8_t(buf, 28, coordinate_system); + _mav_put_char_array(buf, 29, name, 26); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION, buf, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION, buf, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN); +#endif +#else + mavlink_point_of_interest_connection_t *packet = (mavlink_point_of_interest_connection_t *)msgbuf; + packet->xp1 = xp1; + packet->yp1 = yp1; + packet->zp1 = zp1; + packet->xp2 = xp2; + packet->yp2 = yp2; + packet->zp2 = zp2; + packet->timeout = timeout; + packet->type = type; + packet->color = color; + packet->coordinate_system = coordinate_system; + mav_array_memcpy(packet->name, name, sizeof(char)*26); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION, (const char *)packet, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION, (const char *)packet, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE POINT_OF_INTEREST_CONNECTION UNPACKING diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_position_control_setpoint.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_position_control_setpoint.h index 6f4ca510ae..480004cefe 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_position_control_setpoint.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_position_control_setpoint.h @@ -190,6 +190,46 @@ static inline void mavlink_msg_position_control_setpoint_send(mavlink_channel_t #endif } +#if MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_position_control_setpoint_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t id, float x, float y, float z, float yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, x); + _mav_put_float(buf, 4, y); + _mav_put_float(buf, 8, z); + _mav_put_float(buf, 12, yaw); + _mav_put_uint16_t(buf, 16, id); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT, buf, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT, buf, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN); +#endif +#else + mavlink_position_control_setpoint_t *packet = (mavlink_position_control_setpoint_t *)msgbuf; + packet->x = x; + packet->y = y; + packet->z = z; + packet->yaw = yaw; + packet->id = id; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT, (const char *)packet, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE POSITION_CONTROL_SETPOINT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_raw_aux.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_raw_aux.h index 08cedbb4b2..41a5126482 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_raw_aux.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_raw_aux.h @@ -212,6 +212,50 @@ static inline void mavlink_msg_raw_aux_send(mavlink_channel_t chan, uint16_t adc #endif } +#if MAVLINK_MSG_ID_RAW_AUX_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_raw_aux_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t adc1, uint16_t adc2, uint16_t adc3, uint16_t adc4, uint16_t vbat, int16_t temp, int32_t baro) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int32_t(buf, 0, baro); + _mav_put_uint16_t(buf, 4, adc1); + _mav_put_uint16_t(buf, 6, adc2); + _mav_put_uint16_t(buf, 8, adc3); + _mav_put_uint16_t(buf, 10, adc4); + _mav_put_uint16_t(buf, 12, vbat); + _mav_put_int16_t(buf, 14, temp); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_AUX, buf, MAVLINK_MSG_ID_RAW_AUX_LEN, MAVLINK_MSG_ID_RAW_AUX_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_AUX, buf, MAVLINK_MSG_ID_RAW_AUX_LEN); +#endif +#else + mavlink_raw_aux_t *packet = (mavlink_raw_aux_t *)msgbuf; + packet->baro = baro; + packet->adc1 = adc1; + packet->adc2 = adc2; + packet->adc3 = adc3; + packet->adc4 = adc4; + packet->vbat = vbat; + packet->temp = temp; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_AUX, (const char *)packet, MAVLINK_MSG_ID_RAW_AUX_LEN, MAVLINK_MSG_ID_RAW_AUX_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_AUX, (const char *)packet, MAVLINK_MSG_ID_RAW_AUX_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE RAW_AUX UNPACKING diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_cam_shutter.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_cam_shutter.h index b26d748c25..8fe14dd546 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_cam_shutter.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_cam_shutter.h @@ -201,6 +201,48 @@ static inline void mavlink_msg_set_cam_shutter_send(mavlink_channel_t chan, uint #endif } +#if MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_set_cam_shutter_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t cam_no, uint8_t cam_mode, uint8_t trigger_pin, uint16_t interval, uint16_t exposure, float gain) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, gain); + _mav_put_uint16_t(buf, 4, interval); + _mav_put_uint16_t(buf, 6, exposure); + _mav_put_uint8_t(buf, 8, cam_no); + _mav_put_uint8_t(buf, 9, cam_mode); + _mav_put_uint8_t(buf, 10, trigger_pin); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_CAM_SHUTTER, buf, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN, MAVLINK_MSG_ID_SET_CAM_SHUTTER_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_CAM_SHUTTER, buf, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN); +#endif +#else + mavlink_set_cam_shutter_t *packet = (mavlink_set_cam_shutter_t *)msgbuf; + packet->gain = gain; + packet->interval = interval; + packet->exposure = exposure; + packet->cam_no = cam_no; + packet->cam_mode = cam_mode; + packet->trigger_pin = trigger_pin; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_CAM_SHUTTER, (const char *)packet, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN, MAVLINK_MSG_ID_SET_CAM_SHUTTER_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_CAM_SHUTTER, (const char *)packet, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SET_CAM_SHUTTER UNPACKING diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_position_control_offset.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_position_control_offset.h index f1f9e698f8..c4e4fc19a2 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_position_control_offset.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_set_position_control_offset.h @@ -201,6 +201,48 @@ static inline void mavlink_msg_set_position_control_offset_send(mavlink_channel_ #endif } +#if MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_set_position_control_offset_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float x, float y, float z, float yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, x); + _mav_put_float(buf, 4, y); + _mav_put_float(buf, 8, z); + _mav_put_float(buf, 12, yaw); + _mav_put_uint8_t(buf, 16, target_system); + _mav_put_uint8_t(buf, 17, target_component); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET, buf, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET, buf, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN); +#endif +#else + mavlink_set_position_control_offset_t *packet = (mavlink_set_position_control_offset_t *)msgbuf; + packet->x = x; + packet->y = y; + packet->z = z; + packet->yaw = yaw; + packet->target_system = target_system; + packet->target_component = target_component; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET, (const char *)packet, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET, (const char *)packet, MAVLINK_MSG_ID_SET_POSITION_CONTROL_OFFSET_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SET_POSITION_CONTROL_OFFSET UNPACKING diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_command.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_command.h index 9458087da9..d71a2ed2e3 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_command.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_command.h @@ -179,6 +179,44 @@ static inline void mavlink_msg_watchdog_command_send(mavlink_channel_t chan, uin #endif } +#if MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_watchdog_command_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system_id, uint16_t watchdog_id, uint16_t process_id, uint8_t command_id) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, watchdog_id); + _mav_put_uint16_t(buf, 2, process_id); + _mav_put_uint8_t(buf, 4, target_system_id); + _mav_put_uint8_t(buf, 5, command_id); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_COMMAND, buf, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN, MAVLINK_MSG_ID_WATCHDOG_COMMAND_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_COMMAND, buf, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN); +#endif +#else + mavlink_watchdog_command_t *packet = (mavlink_watchdog_command_t *)msgbuf; + packet->watchdog_id = watchdog_id; + packet->process_id = process_id; + packet->target_system_id = target_system_id; + packet->command_id = command_id; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_COMMAND, (const char *)packet, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN, MAVLINK_MSG_ID_WATCHDOG_COMMAND_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_COMMAND, (const char *)packet, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE WATCHDOG_COMMAND UNPACKING diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_heartbeat.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_heartbeat.h index 3f4295ee5d..a8e49b6426 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_heartbeat.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_heartbeat.h @@ -157,6 +157,40 @@ static inline void mavlink_msg_watchdog_heartbeat_send(mavlink_channel_t chan, u #endif } +#if MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_watchdog_heartbeat_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t watchdog_id, uint16_t process_count) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, watchdog_id); + _mav_put_uint16_t(buf, 2, process_count); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT, buf, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT, buf, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN); +#endif +#else + mavlink_watchdog_heartbeat_t *packet = (mavlink_watchdog_heartbeat_t *)msgbuf; + packet->watchdog_id = watchdog_id; + packet->process_count = process_count; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT, (const char *)packet, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT, (const char *)packet, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE WATCHDOG_HEARTBEAT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_info.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_info.h index 55853cdde4..a5e0cc67e4 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_info.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_info.h @@ -185,6 +185,44 @@ static inline void mavlink_msg_watchdog_process_info_send(mavlink_channel_t chan #endif } +#if MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_watchdog_process_info_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t watchdog_id, uint16_t process_id, const char *name, const char *arguments, int32_t timeout) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int32_t(buf, 0, timeout); + _mav_put_uint16_t(buf, 4, watchdog_id); + _mav_put_uint16_t(buf, 6, process_id); + _mav_put_char_array(buf, 8, name, 100); + _mav_put_char_array(buf, 108, arguments, 147); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO, buf, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO, buf, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN); +#endif +#else + mavlink_watchdog_process_info_t *packet = (mavlink_watchdog_process_info_t *)msgbuf; + packet->timeout = timeout; + packet->watchdog_id = watchdog_id; + packet->process_id = process_id; + mav_array_memcpy(packet->name, name, sizeof(char)*100); + mav_array_memcpy(packet->arguments, arguments, sizeof(char)*147); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO, (const char *)packet, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO, (const char *)packet, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE WATCHDOG_PROCESS_INFO UNPACKING diff --git a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_status.h b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_status.h index a0410d803e..15c9598e40 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_status.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_watchdog_process_status.h @@ -201,6 +201,48 @@ static inline void mavlink_msg_watchdog_process_status_send(mavlink_channel_t ch #endif } +#if MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_watchdog_process_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t watchdog_id, uint16_t process_id, uint8_t state, uint8_t muted, int32_t pid, uint16_t crashes) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int32_t(buf, 0, pid); + _mav_put_uint16_t(buf, 4, watchdog_id); + _mav_put_uint16_t(buf, 6, process_id); + _mav_put_uint16_t(buf, 8, crashes); + _mav_put_uint8_t(buf, 10, state); + _mav_put_uint8_t(buf, 11, muted); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS, buf, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS, buf, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN); +#endif +#else + mavlink_watchdog_process_status_t *packet = (mavlink_watchdog_process_status_t *)msgbuf; + packet->pid = pid; + packet->watchdog_id = watchdog_id; + packet->process_id = process_id; + packet->crashes = crashes; + packet->state = state; + packet->muted = muted; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS, (const char *)packet, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS, (const char *)packet, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE WATCHDOG_PROCESS_STATUS UNPACKING diff --git a/mavlink/include/mavlink/v1.0/pixhawk/pixhawk.h b/mavlink/include/mavlink/v1.0/pixhawk/pixhawk.h index 428619eedb..a624a25797 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/pixhawk.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/pixhawk.h @@ -12,15 +12,15 @@ extern "C" { // MESSAGE LENGTHS AND CRCS #ifndef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 0, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 22, 6, 14, 12, 97, 2, 2, 113, 35, 0, 0, 0, 0, 0, 13, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 24, 33, 25, 0, 11, 52, 1, 92, 0, 0, 0, 0, 0, 18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 18, 26, 16, 0, 0, 0, 0, 0, 0, 0, 4, 255, 12, 6, 0, 0, 0, 0, 0, 0, 106, 43, 55, 0, 0, 53, 0, 0, 0, 0, 21, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0} +#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 42, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 22, 6, 14, 12, 97, 2, 2, 113, 35, 6, 79, 0, 0, 0, 13, 255, 14, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 24, 33, 25, 0, 11, 52, 1, 92, 0, 0, 0, 0, 0, 18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 18, 26, 16, 0, 0, 0, 0, 0, 0, 0, 4, 255, 12, 6, 0, 0, 0, 0, 0, 0, 106, 43, 55, 0, 0, 53, 0, 0, 0, 0, 21, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0} #endif #ifndef MAVLINK_MESSAGE_CRCS -#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 0, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 0, 0, 0, 0, 0, 29, 223, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 0, 108, 86, 95, 224, 0, 0, 0, 0, 0, 22, 0, 0, 0, 0, 0, 0, 0, 0, 0, 28, 249, 182, 0, 0, 0, 0, 0, 0, 0, 153, 16, 29, 162, 0, 0, 0, 0, 0, 0, 90, 95, 36, 0, 0, 88, 0, 0, 0, 0, 254, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0} +#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 118, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 203, 220, 0, 0, 0, 29, 223, 85, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 0, 108, 86, 95, 224, 0, 0, 0, 0, 0, 22, 0, 0, 0, 0, 0, 0, 0, 0, 0, 28, 249, 182, 0, 0, 0, 0, 0, 0, 0, 153, 16, 29, 162, 0, 0, 0, 0, 0, 0, 90, 95, 36, 0, 0, 88, 0, 0, 0, 0, 254, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0} #endif #ifndef MAVLINK_MESSAGE_INFO -#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_CAM_SHUTTER, MAVLINK_MESSAGE_INFO_IMAGE_TRIGGERED, MAVLINK_MESSAGE_INFO_IMAGE_TRIGGER_CONTROL, MAVLINK_MESSAGE_INFO_IMAGE_AVAILABLE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_POSITION_CONTROL_OFFSET, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_POSITION_CONTROL_SETPOINT, MAVLINK_MESSAGE_INFO_MARKER, MAVLINK_MESSAGE_INFO_RAW_AUX, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_WATCHDOG_HEARTBEAT, MAVLINK_MESSAGE_INFO_WATCHDOG_PROCESS_INFO, MAVLINK_MESSAGE_INFO_WATCHDOG_PROCESS_STATUS, MAVLINK_MESSAGE_INFO_WATCHDOG_COMMAND, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PATTERN_DETECTED, MAVLINK_MESSAGE_INFO_POINT_OF_INTEREST, MAVLINK_MESSAGE_INFO_POINT_OF_INTEREST_CONNECTION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BRIEF_FEATURE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ATTITUDE_CONTROL, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} +#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_RC_CHANNELS, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, MAVLINK_MESSAGE_INFO_POWER_STATUS, MAVLINK_MESSAGE_INFO_SERIAL_CONTROL, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, MAVLINK_MESSAGE_INFO_DISTANCE_SENSOR, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_CAM_SHUTTER, MAVLINK_MESSAGE_INFO_IMAGE_TRIGGERED, MAVLINK_MESSAGE_INFO_IMAGE_TRIGGER_CONTROL, MAVLINK_MESSAGE_INFO_IMAGE_AVAILABLE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_POSITION_CONTROL_OFFSET, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_POSITION_CONTROL_SETPOINT, MAVLINK_MESSAGE_INFO_MARKER, MAVLINK_MESSAGE_INFO_RAW_AUX, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_WATCHDOG_HEARTBEAT, MAVLINK_MESSAGE_INFO_WATCHDOG_PROCESS_INFO, MAVLINK_MESSAGE_INFO_WATCHDOG_PROCESS_STATUS, MAVLINK_MESSAGE_INFO_WATCHDOG_COMMAND, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PATTERN_DETECTED, MAVLINK_MESSAGE_INFO_POINT_OF_INTEREST, MAVLINK_MESSAGE_INFO_POINT_OF_INTEREST_CONNECTION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BRIEF_FEATURE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ATTITUDE_CONTROL, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} #endif #include "../protocol.h" @@ -56,6 +56,7 @@ enum MAV_CMD MAV_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| */ MAV_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| */ MAV_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| */ + MAV_CMD_NAV_SPLINE_WAYPOINT=82, /* Navigate to MISSION using a spline path. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Empty| Empty| Empty| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */ MAV_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| */ MAV_CMD_CONDITION_DELAY=112, /* Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty| */ MAV_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| */ @@ -73,8 +74,16 @@ enum MAV_CMD MAV_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| */ MAV_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ MAV_CMD_DO_SET_ROI=201, /* 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| */ + MAV_CMD_DO_DIGICAM_CONFIGURE=202, /* Mission command to configure an on-board camera controller system. |Modes: P, TV, AV, M, Etc| Shutter speed: Divisor number for one second| Aperture: F stop number| ISO number e.g. 80, 100, 200, Etc| Exposure type enumerator| Command Identity| Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)| */ + MAV_CMD_DO_DIGICAM_CONTROL=203, /* Mission command to control an on-board camera controller system. |Session control e.g. show/hide lens| Zoom's absolute position| Zooming step value to offset zoom from the current position| Focus Locking, Unlocking or Re-locking| Shooting Command| Command Identity| Empty| */ + MAV_CMD_DO_MOUNT_CONFIGURE=204, /* Mission command to configure a camera or antenna mount |Mount operation mode (see MAV_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty| */ + MAV_CMD_DO_MOUNT_CONTROL=205, /* Mission command to control a camera or antenna mount |pitch or lat in degrees, depending on mount mode.| roll or lon in degrees depending on mount mode| yaw or alt (in meters) depending on mount mode| reserved| reserved| reserved| MAV_MOUNT_MODE enum value| */ + MAV_CMD_DO_SET_CAM_TRIGG_DIST=206, /* Mission command to set CAM_TRIGG_DIST for this flight |Camera trigger distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_FENCE_ENABLE=207, /* Mission command to enable the geofence |enable? (0=disable, 1=enable)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_PARACHUTE=208, /* Mission command to trigger a parachute |action (0=disable, 1=enable, 2=release, for some systems see PARACHUTE_ACTION enum, not in general message set.)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_MOUNT_CONTROL_QUAT=220, /* Mission command to control a camera or antenna mount, using a quaternion as reference. |q1 - quaternion param #1| q2 - quaternion param #2| q3 - quaternion param #3| q4 - quaternion param #4| Empty| Empty| Empty| */ MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty| */ + MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Compass/Motor interference calibration: 0: no, 1: yes| Empty| */ MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS=242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| */ MAV_CMD_PREFLIGHT_STORAGE=245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty| */ MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN=246, /* Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty| */ diff --git a/mavlink/include/mavlink/v1.0/pixhawk/version.h b/mavlink/include/mavlink/v1.0/pixhawk/version.h index aa79f6ffb5..6610a894c9 100644 --- a/mavlink/include/mavlink/v1.0/pixhawk/version.h +++ b/mavlink/include/mavlink/v1.0/pixhawk/version.h @@ -5,7 +5,7 @@ #ifndef MAVLINK_VERSION_H #define MAVLINK_VERSION_H -#define MAVLINK_BUILD_DATE "Tue Feb 4 15:28:16 2014" +#define MAVLINK_BUILD_DATE "Sun Apr 13 09:38:46 2014" #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_ack.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_ack.h index 73e9d75dbc..a87ce87d37 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_ack.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_ack.h @@ -173,6 +173,40 @@ static inline void mavlink_msg_cmd_airspeed_ack_send(mavlink_channel_t chan, flo #endif } +#if MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_cmd_airspeed_ack_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float spCmd, uint8_t ack) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, spCmd); + _mav_put_uint8_t(buf, 4, ack); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK, buf, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK, buf, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN); +#endif +#else + mavlink_cmd_airspeed_ack_t *packet = (mavlink_cmd_airspeed_ack_t *)msgbuf; + packet->spCmd = spCmd; + packet->ack = ack; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK, (const char *)packet, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK, (const char *)packet, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE CMD_AIRSPEED_ACK UNPACKING diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_chng.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_chng.h index b6b567f993..71d61e1e08 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_chng.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_chng.h @@ -173,6 +173,40 @@ static inline void mavlink_msg_cmd_airspeed_chng_send(mavlink_channel_t chan, ui #endif } +#if MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_cmd_airspeed_chng_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target, float spCmd) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, spCmd); + _mav_put_uint8_t(buf, 4, target); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG, buf, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG, buf, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN); +#endif +#else + mavlink_cmd_airspeed_chng_t *packet = (mavlink_cmd_airspeed_chng_t *)msgbuf; + packet->spCmd = spCmd; + packet->target = target; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG, (const char *)packet, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG, (const char *)packet, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE CMD_AIRSPEED_CHNG UNPACKING diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_filt_rot_vel.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_filt_rot_vel.h index 642f7aab93..40a3db25a0 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_filt_rot_vel.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_filt_rot_vel.h @@ -154,6 +154,38 @@ static inline void mavlink_msg_filt_rot_vel_send(mavlink_channel_t chan, const f #endif } +#if MAVLINK_MSG_ID_FILT_ROT_VEL_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_filt_rot_vel_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, const float *rotVel) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + + _mav_put_float_array(buf, 0, rotVel, 3); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILT_ROT_VEL, buf, MAVLINK_MSG_ID_FILT_ROT_VEL_LEN, MAVLINK_MSG_ID_FILT_ROT_VEL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILT_ROT_VEL, buf, MAVLINK_MSG_ID_FILT_ROT_VEL_LEN); +#endif +#else + mavlink_filt_rot_vel_t *packet = (mavlink_filt_rot_vel_t *)msgbuf; + + mav_array_memcpy(packet->rotVel, rotVel, sizeof(float)*3); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILT_ROT_VEL, (const char *)packet, MAVLINK_MSG_ID_FILT_ROT_VEL_LEN, MAVLINK_MSG_ID_FILT_ROT_VEL_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILT_ROT_VEL, (const char *)packet, MAVLINK_MSG_ID_FILT_ROT_VEL_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE FILT_ROT_VEL UNPACKING diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_llc_out.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_llc_out.h index 0f8369881a..bceed1013c 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_llc_out.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_llc_out.h @@ -174,6 +174,40 @@ static inline void mavlink_msg_llc_out_send(mavlink_channel_t chan, const int16_ #endif } +#if MAVLINK_MSG_ID_LLC_OUT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_llc_out_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, const int16_t *servoOut, const int16_t *MotorOut) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + + _mav_put_int16_t_array(buf, 0, servoOut, 4); + _mav_put_int16_t_array(buf, 8, MotorOut, 2); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LLC_OUT, buf, MAVLINK_MSG_ID_LLC_OUT_LEN, MAVLINK_MSG_ID_LLC_OUT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LLC_OUT, buf, MAVLINK_MSG_ID_LLC_OUT_LEN); +#endif +#else + mavlink_llc_out_t *packet = (mavlink_llc_out_t *)msgbuf; + + mav_array_memcpy(packet->servoOut, servoOut, sizeof(int16_t)*4); + mav_array_memcpy(packet->MotorOut, MotorOut, sizeof(int16_t)*2); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LLC_OUT, (const char *)packet, MAVLINK_MSG_ID_LLC_OUT_LEN, MAVLINK_MSG_ID_LLC_OUT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LLC_OUT, (const char *)packet, MAVLINK_MSG_ID_LLC_OUT_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE LLC_OUT UNPACKING diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_temp.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_temp.h index 5d93823262..f5a43e4fa4 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_temp.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_temp.h @@ -154,6 +154,38 @@ static inline void mavlink_msg_obs_air_temp_send(mavlink_channel_t chan, float a #endif } +#if MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_obs_air_temp_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float airT) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, airT); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_TEMP, buf, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN, MAVLINK_MSG_ID_OBS_AIR_TEMP_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_TEMP, buf, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN); +#endif +#else + mavlink_obs_air_temp_t *packet = (mavlink_obs_air_temp_t *)msgbuf; + packet->airT = airT; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_TEMP, (const char *)packet, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN, MAVLINK_MSG_ID_OBS_AIR_TEMP_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_TEMP, (const char *)packet, MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE OBS_AIR_TEMP UNPACKING diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_velocity.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_velocity.h index 35d813ca15..67d5ab2a22 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_velocity.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_velocity.h @@ -192,6 +192,42 @@ static inline void mavlink_msg_obs_air_velocity_send(mavlink_channel_t chan, flo #endif } +#if MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_obs_air_velocity_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float magnitude, float aoa, float slip) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, magnitude); + _mav_put_float(buf, 4, aoa); + _mav_put_float(buf, 8, slip); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_VELOCITY, buf, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_VELOCITY, buf, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN); +#endif +#else + mavlink_obs_air_velocity_t *packet = (mavlink_obs_air_velocity_t *)msgbuf; + packet->magnitude = magnitude; + packet->aoa = aoa; + packet->slip = slip; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_VELOCITY, (const char *)packet, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_VELOCITY, (const char *)packet, MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE OBS_AIR_VELOCITY UNPACKING diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_attitude.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_attitude.h index 9c80cd66eb..e1178e25fa 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_attitude.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_attitude.h @@ -154,6 +154,38 @@ static inline void mavlink_msg_obs_attitude_send(mavlink_channel_t chan, const d #endif } +#if MAVLINK_MSG_ID_OBS_ATTITUDE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_obs_attitude_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, const double *quat) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + + _mav_put_double_array(buf, 0, quat, 4); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_ATTITUDE, buf, MAVLINK_MSG_ID_OBS_ATTITUDE_LEN, MAVLINK_MSG_ID_OBS_ATTITUDE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_ATTITUDE, buf, MAVLINK_MSG_ID_OBS_ATTITUDE_LEN); +#endif +#else + mavlink_obs_attitude_t *packet = (mavlink_obs_attitude_t *)msgbuf; + + mav_array_memcpy(packet->quat, quat, sizeof(double)*4); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_ATTITUDE, (const char *)packet, MAVLINK_MSG_ID_OBS_ATTITUDE_LEN, MAVLINK_MSG_ID_OBS_ATTITUDE_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_ATTITUDE, (const char *)packet, MAVLINK_MSG_ID_OBS_ATTITUDE_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE OBS_ATTITUDE UNPACKING diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_bias.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_bias.h index 24dd43b579..b164687ff8 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_bias.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_bias.h @@ -174,6 +174,40 @@ static inline void mavlink_msg_obs_bias_send(mavlink_channel_t chan, const float #endif } +#if MAVLINK_MSG_ID_OBS_BIAS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_obs_bias_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, const float *accBias, const float *gyroBias) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + + _mav_put_float_array(buf, 0, accBias, 3); + _mav_put_float_array(buf, 12, gyroBias, 3); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_BIAS, buf, MAVLINK_MSG_ID_OBS_BIAS_LEN, MAVLINK_MSG_ID_OBS_BIAS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_BIAS, buf, MAVLINK_MSG_ID_OBS_BIAS_LEN); +#endif +#else + mavlink_obs_bias_t *packet = (mavlink_obs_bias_t *)msgbuf; + + mav_array_memcpy(packet->accBias, accBias, sizeof(float)*3); + mav_array_memcpy(packet->gyroBias, gyroBias, sizeof(float)*3); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_BIAS, (const char *)packet, MAVLINK_MSG_ID_OBS_BIAS_LEN, MAVLINK_MSG_ID_OBS_BIAS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_BIAS, (const char *)packet, MAVLINK_MSG_ID_OBS_BIAS_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE OBS_BIAS UNPACKING diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_position.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_position.h index cfc2fe7e10..f0ecef0e7e 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_position.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_position.h @@ -192,6 +192,42 @@ static inline void mavlink_msg_obs_position_send(mavlink_channel_t chan, int32_t #endif } +#if MAVLINK_MSG_ID_OBS_POSITION_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_obs_position_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, int32_t lon, int32_t lat, int32_t alt) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int32_t(buf, 0, lon); + _mav_put_int32_t(buf, 4, lat); + _mav_put_int32_t(buf, 8, alt); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_POSITION, buf, MAVLINK_MSG_ID_OBS_POSITION_LEN, MAVLINK_MSG_ID_OBS_POSITION_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_POSITION, buf, MAVLINK_MSG_ID_OBS_POSITION_LEN); +#endif +#else + mavlink_obs_position_t *packet = (mavlink_obs_position_t *)msgbuf; + packet->lon = lon; + packet->lat = lat; + packet->alt = alt; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_POSITION, (const char *)packet, MAVLINK_MSG_ID_OBS_POSITION_LEN, MAVLINK_MSG_ID_OBS_POSITION_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_POSITION, (const char *)packet, MAVLINK_MSG_ID_OBS_POSITION_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE OBS_POSITION UNPACKING diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_qff.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_qff.h index 24e272bf7a..31617c8839 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_qff.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_qff.h @@ -154,6 +154,38 @@ static inline void mavlink_msg_obs_qff_send(mavlink_channel_t chan, float qff) #endif } +#if MAVLINK_MSG_ID_OBS_QFF_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_obs_qff_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float qff) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, qff); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_QFF, buf, MAVLINK_MSG_ID_OBS_QFF_LEN, MAVLINK_MSG_ID_OBS_QFF_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_QFF, buf, MAVLINK_MSG_ID_OBS_QFF_LEN); +#endif +#else + mavlink_obs_qff_t *packet = (mavlink_obs_qff_t *)msgbuf; + packet->qff = qff; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_QFF, (const char *)packet, MAVLINK_MSG_ID_OBS_QFF_LEN, MAVLINK_MSG_ID_OBS_QFF_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_QFF, (const char *)packet, MAVLINK_MSG_ID_OBS_QFF_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE OBS_QFF UNPACKING diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_velocity.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_velocity.h index 6e3776632b..ec815d6929 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_velocity.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_velocity.h @@ -154,6 +154,38 @@ static inline void mavlink_msg_obs_velocity_send(mavlink_channel_t chan, const f #endif } +#if MAVLINK_MSG_ID_OBS_VELOCITY_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_obs_velocity_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, const float *vel) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + + _mav_put_float_array(buf, 0, vel, 3); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_VELOCITY, buf, MAVLINK_MSG_ID_OBS_VELOCITY_LEN, MAVLINK_MSG_ID_OBS_VELOCITY_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_VELOCITY, buf, MAVLINK_MSG_ID_OBS_VELOCITY_LEN); +#endif +#else + mavlink_obs_velocity_t *packet = (mavlink_obs_velocity_t *)msgbuf; + + mav_array_memcpy(packet->vel, vel, sizeof(float)*3); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_VELOCITY, (const char *)packet, MAVLINK_MSG_ID_OBS_VELOCITY_LEN, MAVLINK_MSG_ID_OBS_VELOCITY_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_VELOCITY, (const char *)packet, MAVLINK_MSG_ID_OBS_VELOCITY_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE OBS_VELOCITY UNPACKING diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_wind.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_wind.h index 55f7cb2ae1..c40391d441 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_wind.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_wind.h @@ -154,6 +154,38 @@ static inline void mavlink_msg_obs_wind_send(mavlink_channel_t chan, const float #endif } +#if MAVLINK_MSG_ID_OBS_WIND_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_obs_wind_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, const float *wind) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + + _mav_put_float_array(buf, 0, wind, 3); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_WIND, buf, MAVLINK_MSG_ID_OBS_WIND_LEN, MAVLINK_MSG_ID_OBS_WIND_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_WIND, buf, MAVLINK_MSG_ID_OBS_WIND_LEN); +#endif +#else + mavlink_obs_wind_t *packet = (mavlink_obs_wind_t *)msgbuf; + + mav_array_memcpy(packet->wind, wind, sizeof(float)*3); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_WIND, (const char *)packet, MAVLINK_MSG_ID_OBS_WIND_LEN, MAVLINK_MSG_ID_OBS_WIND_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_WIND, (const char *)packet, MAVLINK_MSG_ID_OBS_WIND_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE OBS_WIND UNPACKING diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_pm_elec.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_pm_elec.h index e0963ece79..70524f464c 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_pm_elec.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_pm_elec.h @@ -186,6 +186,40 @@ static inline void mavlink_msg_pm_elec_send(mavlink_channel_t chan, float PwCons #endif } +#if MAVLINK_MSG_ID_PM_ELEC_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_pm_elec_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, float PwCons, float BatStat, const float *PwGen) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, PwCons); + _mav_put_float(buf, 4, BatStat); + _mav_put_float_array(buf, 8, PwGen, 3); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PM_ELEC, buf, MAVLINK_MSG_ID_PM_ELEC_LEN, MAVLINK_MSG_ID_PM_ELEC_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PM_ELEC, buf, MAVLINK_MSG_ID_PM_ELEC_LEN); +#endif +#else + mavlink_pm_elec_t *packet = (mavlink_pm_elec_t *)msgbuf; + packet->PwCons = PwCons; + packet->BatStat = BatStat; + mav_array_memcpy(packet->PwGen, PwGen, sizeof(float)*3); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PM_ELEC, (const char *)packet, MAVLINK_MSG_ID_PM_ELEC_LEN, MAVLINK_MSG_ID_PM_ELEC_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PM_ELEC, (const char *)packet, MAVLINK_MSG_ID_PM_ELEC_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE PM_ELEC UNPACKING diff --git a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_sys_stat.h b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_sys_stat.h index 94f3e58bb9..72f8a8193b 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_sys_stat.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/mavlink_msg_sys_stat.h @@ -211,6 +211,44 @@ static inline void mavlink_msg_sys_stat_send(mavlink_channel_t chan, uint8_t gps #endif } +#if MAVLINK_MSG_ID_SYS_Stat_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_sys_stat_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t gps, uint8_t act, uint8_t mod, uint8_t commRssi) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, gps); + _mav_put_uint8_t(buf, 1, act); + _mav_put_uint8_t(buf, 2, mod); + _mav_put_uint8_t(buf, 3, commRssi); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_Stat, buf, MAVLINK_MSG_ID_SYS_Stat_LEN, MAVLINK_MSG_ID_SYS_Stat_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_Stat, buf, MAVLINK_MSG_ID_SYS_Stat_LEN); +#endif +#else + mavlink_sys_stat_t *packet = (mavlink_sys_stat_t *)msgbuf; + packet->gps = gps; + packet->act = act; + packet->mod = mod; + packet->commRssi = commRssi; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_Stat, (const char *)packet, MAVLINK_MSG_ID_SYS_Stat_LEN, MAVLINK_MSG_ID_SYS_Stat_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_Stat, (const char *)packet, MAVLINK_MSG_ID_SYS_Stat_LEN); +#endif +#endif +} +#endif + #endif // MESSAGE SYS_Stat UNPACKING diff --git a/mavlink/include/mavlink/v1.0/sensesoar/sensesoar.h b/mavlink/include/mavlink/v1.0/sensesoar/sensesoar.h index 26666b0cc7..2eab863546 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/sensesoar.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/sensesoar.h @@ -12,15 +12,15 @@ extern "C" { // MESSAGE LENGTHS AND CRCS #ifndef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 0, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 22, 6, 14, 12, 97, 2, 2, 113, 35, 0, 0, 0, 0, 0, 13, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 24, 33, 25, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 12, 0, 12, 0, 32, 0, 12, 0, 12, 0, 24, 0, 4, 4, 12, 0, 12, 0, 20, 0, 4, 0, 5, 0, 5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0} +#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 34, 26, 46, 36, 42, 6, 4, 0, 11, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 20, 22, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 54, 64, 84, 9, 254, 249, 9, 36, 26, 64, 22, 6, 14, 12, 97, 2, 2, 113, 35, 6, 79, 0, 0, 0, 13, 255, 14, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 24, 33, 25, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 12, 0, 12, 0, 32, 0, 12, 0, 12, 0, 24, 0, 4, 4, 12, 0, 12, 0, 20, 0, 4, 0, 5, 0, 5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0} #endif #ifndef MAVLINK_MESSAGE_CRCS -#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 0, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 0, 0, 0, 0, 0, 29, 223, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 15, 0, 108, 0, 146, 0, 16, 0, 32, 0, 159, 0, 24, 248, 79, 0, 5, 0, 170, 0, 157, 0, 209, 0, 243, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0} +#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 240, 183, 130, 130, 118, 148, 21, 0, 243, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 127, 106, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 211, 108, 32, 185, 235, 93, 124, 124, 119, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 203, 220, 0, 0, 0, 29, 223, 85, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177, 241, 15, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 15, 0, 108, 0, 146, 0, 16, 0, 32, 0, 159, 0, 24, 248, 79, 0, 5, 0, 170, 0, 157, 0, 209, 0, 243, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0} #endif #ifndef MAVLINK_MESSAGE_INFO -#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_POSITION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_VELOCITY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_ATTITUDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_WIND, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_AIR_VELOCITY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_BIAS, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_QFF, MAVLINK_MESSAGE_INFO_OBS_AIR_TEMP, MAVLINK_MESSAGE_INFO_FILT_ROT_VEL, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LLC_OUT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PM_ELEC, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SYS_Stat, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_CMD_AIRSPEED_CHNG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_CMD_AIRSPEED_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} +#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_LED_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_RC_CHANNELS, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OMNIDIRECTIONAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_START, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_DIR_LIST, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_RES, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, MAVLINK_MESSAGE_INFO_POWER_STATUS, MAVLINK_MESSAGE_INFO_SERIAL_CONTROL, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, MAVLINK_MESSAGE_INFO_DISTANCE_SENSOR, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_SETPOINT_8DOF, MAVLINK_MESSAGE_INFO_SETPOINT_6DOF, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_POSITION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_VELOCITY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_ATTITUDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_WIND, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_AIR_VELOCITY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_BIAS, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_QFF, MAVLINK_MESSAGE_INFO_OBS_AIR_TEMP, MAVLINK_MESSAGE_INFO_FILT_ROT_VEL, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LLC_OUT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PM_ELEC, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SYS_Stat, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_CMD_AIRSPEED_CHNG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_CMD_AIRSPEED_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} #endif #include "../protocol.h" diff --git a/mavlink/include/mavlink/v1.0/sensesoar/version.h b/mavlink/include/mavlink/v1.0/sensesoar/version.h index cdd683949e..78d5628039 100644 --- a/mavlink/include/mavlink/v1.0/sensesoar/version.h +++ b/mavlink/include/mavlink/v1.0/sensesoar/version.h @@ -5,7 +5,7 @@ #ifndef MAVLINK_VERSION_H #define MAVLINK_VERSION_H -#define MAVLINK_BUILD_DATE "Tue Feb 4 15:28:27 2014" +#define MAVLINK_BUILD_DATE "Sun Apr 13 09:39:00 2014" #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 From 80cd2e6c9b03eb078aafe2814ff1c9d2753ac073 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Apr 2014 23:29:12 +0200 Subject: [PATCH 373/381] Added fields to range finder clarifying sensor properties --- src/drivers/drv_range_finder.h | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/src/drivers/drv_range_finder.h b/src/drivers/drv_range_finder.h index cf91f70304..e45939b371 100644 --- a/src/drivers/drv_range_finder.h +++ b/src/drivers/drv_range_finder.h @@ -46,6 +46,10 @@ #define RANGE_FINDER_DEVICE_PATH "/dev/range_finder" +enum RANGE_FINDER_TYPE { + RANGE_FINDER_TYPE_LASER = 0, +}; + /** * range finder report structure. Reads from the device must be in multiples of this * structure. @@ -53,8 +57,11 @@ struct range_finder_report { uint64_t timestamp; uint64_t error_count; - float distance; /** in meters */ - uint8_t valid; /** 1 == within sensor range, 0 = outside sensor range */ + unsigned type; /**< type, following RANGE_FINDER_TYPE enum */ + float distance; /**< in meters */ + float minimum_distance; /**< minimum distance the sensor can measure */ + float maximum_distance; /**< maximum distance the sensor can measure */ + uint8_t valid; /**< 1 == within sensor range, 0 = outside sensor range */ }; /* From f84669039579aeef80e232c3c9a444d20bcbdf39 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Apr 2014 23:30:09 +0200 Subject: [PATCH 374/381] Added rangefinder message to MAVLink app --- src/modules/mavlink/mavlink_main.cpp | 1 + src/modules/mavlink/mavlink_messages.cpp | 47 ++++++++++++++++++++++++ src/modules/mavlink/module.mk | 2 + 3 files changed, 50 insertions(+) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 1ed3f40014..c90b4e2deb 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1942,6 +1942,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("NAMED_VALUE_FLOAT", 1.0f * rate_mult); configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f * rate_mult); configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f * rate_mult); + configure_stream("DISTANCE_SENSOR", 0.5f); break; case MAVLINK_MODE_CAMERA: diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 47603b3903..e608bf7877 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -72,6 +72,7 @@ #include #include #include +#include #include "mavlink_messages.h" @@ -1271,6 +1272,51 @@ protected: } }; +class MavlinkStreamDistanceSensor : public MavlinkStream +{ +public: + const char *get_name() + { + return "DISTANCE_SENSOR"; + } + + MavlinkStream *new_instance() + { + return new MavlinkStreamDistanceSensor(); + } + +private: + MavlinkOrbSubscription *range_sub; + struct range_finder_report *range; + +protected: + void subscribe(Mavlink *mavlink) + { + range_sub = mavlink->add_orb_subscription(ORB_ID(sensor_range_finder)); + range = (struct range_finder_report *)range_sub->get_data(); + } + + void send(const hrt_abstime t) + { + (void)range_sub->update(t); + + uint8_t type; + + switch (range->type) { + case RANGE_FINDER_TYPE_LASER: + type = MAV_DISTANCE_SENSOR_LASER; + break; + } + + uint8_t id = 0; + uint8_t orientation = 0; + uint8_t covariance = 20; + + mavlink_msg_distance_sensor_send(_channel, range->timestamp / 1000, type, id, orientation, + range->minimum_distance*100, range->maximum_distance*100, range->distance*100, covariance); + } +}; + MavlinkStream *streams_list[] = { new MavlinkStreamHeartbeat(), new MavlinkStreamSysStatus(), @@ -1297,5 +1343,6 @@ MavlinkStream *streams_list[] = { new MavlinkStreamAttitudeControls(), new MavlinkStreamNamedValueFloat(), new MavlinkStreamCameraCapture(), + new MavlinkStreamDistanceSensor(), nullptr }; diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index 515fbfadce..dcca11977f 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -46,3 +46,5 @@ SRCS += mavlink_main.cpp \ mavlink_commands.cpp INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink + +MAXOPTIMIZATION = -Os From 5c08a1aeaca2c86b66ee1ad05b0178ff214847e1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Apr 2014 23:32:52 +0200 Subject: [PATCH 375/381] Untangled local pos and distance messages, now sending distance messages only for actual distance measuring devices --- src/modules/sdlog2/sdlog2.c | 18 +++--------------- src/modules/sdlog2/sdlog2_messages.h | 5 ++++- 2 files changed, 7 insertions(+), 16 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 1bf6f1de3e..611491cf9b 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -908,9 +908,6 @@ int sdlog2_thread_main(int argc, char *argv[]) hrt_abstime barometer_timestamp = 0; hrt_abstime differential_pressure_timestamp = 0; - /* track changes in distance status */ - bool dist_bottom_present = false; - /* enable logging on start if needed */ if (log_on_start) { /* check GPS topic to get GPS time */ @@ -1099,6 +1096,8 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_LPOS.x = buf.local_pos.x; log_msg.body.log_LPOS.y = buf.local_pos.y; log_msg.body.log_LPOS.z = buf.local_pos.z; + log_msg.body.log_LPOS.ground_dist = buf.local_pos.dist_bottom; + log_msg.body.log_LPOS.ground_dist_rate = buf.local_pos.dist_bottom_rate; log_msg.body.log_LPOS.vx = buf.local_pos.vx; log_msg.body.log_LPOS.vy = buf.local_pos.vy; log_msg.body.log_LPOS.vz = buf.local_pos.vz; @@ -1108,19 +1107,8 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_LPOS.xy_flags = (buf.local_pos.xy_valid ? 1 : 0) | (buf.local_pos.v_xy_valid ? 2 : 0) | (buf.local_pos.xy_global ? 8 : 0); log_msg.body.log_LPOS.z_flags = (buf.local_pos.z_valid ? 1 : 0) | (buf.local_pos.v_z_valid ? 2 : 0) | (buf.local_pos.z_global ? 8 : 0); log_msg.body.log_LPOS.landed = buf.local_pos.landed; + log_msg.body.log_LPOS.ground_dist_flags = (buf.local_pos.dist_bottom_valid ? 1 : 0); LOGBUFFER_WRITE_AND_COUNT(LPOS); - - if (buf.local_pos.dist_bottom_valid) { - dist_bottom_present = true; - } - - if (dist_bottom_present) { - log_msg.msg_type = LOG_DIST_MSG; - log_msg.body.log_DIST.bottom = buf.local_pos.dist_bottom; - log_msg.body.log_DIST.bottom_rate = buf.local_pos.dist_bottom_rate; - log_msg.body.log_DIST.flags = (buf.local_pos.dist_bottom_valid ? 1 : 0); - LOGBUFFER_WRITE_AND_COUNT(DIST); - } } /* --- LOCAL POSITION SETPOINT --- */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index d0585df394..2538dcd2f3 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -101,6 +101,8 @@ struct log_LPOS_s { float x; float y; float z; + float ground_dist; + float ground_dist_rate; float vx; float vy; float vz; @@ -110,6 +112,7 @@ struct log_LPOS_s { uint8_t xy_flags; uint8_t z_flags; uint8_t landed; + uint8_t ground_dist_flags; }; /* --- LPSP - LOCAL POSITION SETPOINT --- */ @@ -343,7 +346,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), LOG_FORMAT(SENS, "fffff", "BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"), - LOG_FORMAT(LPOS, "ffffffLLfBBB", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt,XYFlags,ZFlags,Landed"), + LOG_FORMAT(LPOS, "ffffffffLLfBBBB", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt,XYFlags,ZFlags,Landed"), LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), From 9be0dcdab1820cd5cc6f6fc6a447f8c5a52784cc Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 14 Apr 2014 21:44:51 +0200 Subject: [PATCH 376/381] startup: cleanup of cox mixer files (Thanks Rune) --- ROMFS/px4fmu_common/init.d/11001_hexa_cox | 2 +- ROMFS/px4fmu_common/init.d/12001_octo_cox | 2 +- ROMFS/px4fmu_common/init.d/rcS | 2 +- ROMFS/px4fmu_common/mixers/{hexa_cox.mix => FMU_hexa_cox.mix} | 0 4 files changed, 3 insertions(+), 3 deletions(-) rename ROMFS/px4fmu_common/mixers/{hexa_cox.mix => FMU_hexa_cox.mix} (100%) diff --git a/ROMFS/px4fmu_common/init.d/11001_hexa_cox b/ROMFS/px4fmu_common/init.d/11001_hexa_cox index 38e65435b6..df2e609bc8 100644 --- a/ROMFS/px4fmu_common/init.d/11001_hexa_cox +++ b/ROMFS/px4fmu_common/init.d/11001_hexa_cox @@ -9,7 +9,7 @@ sh /etc/init.d/rc.mc_defaults -set MIXER hexa_cox +set MIXER FMU_hexa_cox # We only can run one channel group with one rate, so set all 8 channels set PWM_OUTPUTS 12345678 diff --git a/ROMFS/px4fmu_common/init.d/12001_octo_cox b/ROMFS/px4fmu_common/init.d/12001_octo_cox index 99f902a6dd..8703f5f2fe 100644 --- a/ROMFS/px4fmu_common/init.d/12001_octo_cox +++ b/ROMFS/px4fmu_common/init.d/12001_octo_cox @@ -7,6 +7,6 @@ sh /etc/init.d/rc.mc_defaults -set MIXER octo_cox +set MIXER FMU_octo_cox set PWM_OUTPUTS 12345678 diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 0cb6d281a2..2ab1e2e96f 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -514,7 +514,7 @@ then then set MAV_TYPE 13 fi - if [ $MIXER == hexa_cox ] + if [ $MIXER == FMU_hexa_cox ] then set MAV_TYPE 13 fi diff --git a/ROMFS/px4fmu_common/mixers/hexa_cox.mix b/ROMFS/px4fmu_common/mixers/FMU_hexa_cox.mix similarity index 100% rename from ROMFS/px4fmu_common/mixers/hexa_cox.mix rename to ROMFS/px4fmu_common/mixers/FMU_hexa_cox.mix From 282f40d16247db6ae2598e7f28cd5cc42c4600d0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 15 Apr 2014 19:25:07 +0200 Subject: [PATCH 377/381] Hotfix to PX4IO uploader. There are no known mishaps due to it, but very clearly the IO firmware flashing process should be verified after an upload. --- src/drivers/px4io/px4io_uploader.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index dd8abbac5a..28ec62356b 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -201,9 +201,14 @@ PX4IO_Uploader::upload(const char *filenames[]) continue; } - if (bl_rev <= 2) + if (bl_rev <= 2) { ret = verify_rev2(fw_size); - else if(bl_rev == 3) { + } else if(bl_rev == 3) { + ret = verify_rev3(fw_size); + } else { + /* verify rev 4 and higher still uses the same approach and + * every version *needs* to be verified. + */ ret = verify_rev3(fw_size); } From 967e9b68783746765a417f613b3ba2935bd6c953 Mon Sep 17 00:00:00 2001 From: TickTock- Date: Wed, 16 Apr 2014 21:41:00 -0700 Subject: [PATCH 378/381] Fixed blinkm state indication (was not properly reporting flight mode). Added flashing orange safety disarmed state indicator and solid blue failsafe indicator. Changed safety on state to solid cyan. Increased LiPo cellcount support to 6. --- src/drivers/blinkm/blinkm.cpp | 141 +++++++++++++++++++++++----------- 1 file changed, 95 insertions(+), 46 deletions(-) diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index 2361f4dd18..b75c2297fa 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -48,11 +48,14 @@ * The recognized number off cells, will be blinked 5 times in purple color. * 2 Cells = 2 blinks * ... - * 5 Cells = 5 blinks + * 6 Cells = 6 blinks * Now the Application will show the actual selected Flightmode, GPS-Fix and Battery Warnings and Alerts. * - * System disarmed: - * The BlinkM should lit solid red. + * System disarmed and safe: + * The BlinkM should light solid cyan. + * + * System safety off but not armed: + * The BlinkM should light flashing orange * * System armed: * One message is made of 4 Blinks and a pause in the same length as the 4 blinks. @@ -67,10 +70,10 @@ * (X = on, _=off) * * The first 3 blinks indicates the status of the GPS-Signal (red): - * 0-4 satellites = X-X-X-X-_-_-_-_-_-_- - * 5 satellites = X-X-_-X-_-_-_-_-_-_- - * 6 satellites = X-_-_-X-_-_-_-_-_-_- - * >=7 satellites = _-_-_-X-_-_-_-_-_-_- + * 0-4 satellites = X-X-X-X-X-_-_-_-_-_- + * 5 satellites = X-X-_-X-X-_-_-_-_-_- + * 6 satellites = X-_-_-X-X-_-_-_-_-_- + * >=7 satellites = _-_-_-X-X-_-_-_-_-_- * If no GPS is found the first 3 blinks are white * * The fourth Blink indicates the Flightmode: @@ -119,6 +122,7 @@ #include #include #include +#include static const float MAX_CELL_VOLTAGE = 4.3f; static const int LED_ONTIME = 120; @@ -166,10 +170,12 @@ private: enum ledColors { LED_OFF, LED_RED, + LED_ORANGE, LED_YELLOW, LED_PURPLE, LED_GREEN, LED_BLUE, + LED_CYAN, LED_WHITE, LED_AMBER }; @@ -380,6 +386,7 @@ BlinkM::led() static int vehicle_control_mode_sub_fd; static int vehicle_gps_position_sub_fd; static int actuator_armed_sub_fd; + static int safety_sub_fd; static int num_of_cells = 0; static int detected_cells_runcount = 0; @@ -402,16 +409,20 @@ BlinkM::led() if(!topic_initialized) { vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status)); - orb_set_interval(vehicle_status_sub_fd, 1000); + orb_set_interval(vehicle_status_sub_fd, 250); vehicle_control_mode_sub_fd = orb_subscribe(ORB_ID(vehicle_control_mode)); - orb_set_interval(vehicle_control_mode_sub_fd, 1000); + orb_set_interval(vehicle_control_mode_sub_fd, 250); actuator_armed_sub_fd = orb_subscribe(ORB_ID(actuator_armed)); - orb_set_interval(actuator_armed_sub_fd, 1000); + orb_set_interval(actuator_armed_sub_fd, 250); vehicle_gps_position_sub_fd = orb_subscribe(ORB_ID(vehicle_gps_position)); - orb_set_interval(vehicle_gps_position_sub_fd, 1000); + orb_set_interval(vehicle_gps_position_sub_fd, 250); + + /* Subscribe to safety topic */ + safety_sub_fd = orb_subscribe(ORB_ID(safety)); + orb_set_interval(safety_sub_fd, 250); topic_initialized = true; } @@ -433,7 +444,9 @@ BlinkM::led() if(num_of_cells > 4) { t_led_color[4] = LED_PURPLE; } - t_led_color[5] = LED_OFF; + if(num_of_cells > 5) { + t_led_color[5] = LED_PURPLE; + } t_led_color[6] = LED_OFF; t_led_color[7] = LED_OFF; t_led_blink = LED_BLINK; @@ -467,14 +480,17 @@ BlinkM::led() struct vehicle_control_mode_s vehicle_control_mode; struct actuator_armed_s actuator_armed; struct vehicle_gps_position_s vehicle_gps_position_raw; + struct safety_s safety; memset(&vehicle_status_raw, 0, sizeof(vehicle_status_raw)); memset(&vehicle_gps_position_raw, 0, sizeof(vehicle_gps_position_raw)); + memset(&safety, 0, sizeof(safety)); bool new_data_vehicle_status; bool new_data_vehicle_control_mode; bool new_data_actuator_armed; bool new_data_vehicle_gps_position; + bool new_data_safety; orb_check(vehicle_status_sub_fd, &new_data_vehicle_status); @@ -520,7 +536,12 @@ BlinkM::led() no_data_vehicle_gps_position = 3; } + /* update safety topic */ + orb_check(safety_sub_fd, &new_data_safety); + if (new_data_safety) { + orb_copy(ORB_ID(safety), safety_sub_fd, &safety); + } /* get number of used satellites in navigation */ num_of_used_sats = 0; @@ -541,19 +562,7 @@ BlinkM::led() printf(" cells found:%d\n", num_of_cells); } else { - if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_LOW) { - /* LED Pattern for battery low warning */ - led_color_1 = LED_YELLOW; - led_color_2 = LED_YELLOW; - led_color_3 = LED_YELLOW; - led_color_4 = LED_YELLOW; - led_color_5 = LED_YELLOW; - led_color_6 = LED_YELLOW; - led_color_7 = LED_YELLOW; - led_color_8 = LED_YELLOW; - led_blink = LED_BLINK; - - } else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) { + if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) { /* LED Pattern for battery critical alerting */ led_color_1 = LED_RED; led_color_2 = LED_RED; @@ -565,21 +574,56 @@ BlinkM::led() led_color_8 = LED_RED; led_blink = LED_BLINK; + } else if(vehicle_status_raw.rc_signal_lost) { + /* LED Pattern for FAILSAFE */ + led_color_1 = LED_BLUE; + led_color_2 = LED_BLUE; + led_color_3 = LED_BLUE; + led_color_4 = LED_BLUE; + led_color_5 = LED_BLUE; + led_color_6 = LED_BLUE; + led_color_7 = LED_BLUE; + led_color_8 = LED_BLUE; + led_blink = LED_BLINK; + + } else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_LOW) { + /* LED Pattern for battery low warning */ + led_color_1 = LED_YELLOW; + led_color_2 = LED_YELLOW; + led_color_3 = LED_YELLOW; + led_color_4 = LED_YELLOW; + led_color_5 = LED_YELLOW; + led_color_6 = LED_YELLOW; + led_color_7 = LED_YELLOW; + led_color_8 = LED_YELLOW; + led_blink = LED_BLINK; + } else { /* no battery warnings here */ if(actuator_armed.armed == false) { /* system not armed */ - led_color_1 = LED_RED; - led_color_2 = LED_RED; - led_color_3 = LED_RED; - led_color_4 = LED_RED; - led_color_5 = LED_RED; - led_color_6 = LED_RED; - led_color_7 = LED_RED; - led_color_8 = LED_RED; - led_blink = LED_NOBLINK; - + if(safety.safety_off){ + led_color_1 = LED_ORANGE; + led_color_2 = LED_ORANGE; + led_color_3 = LED_ORANGE; + led_color_4 = LED_ORANGE; + led_color_5 = LED_ORANGE; + led_color_6 = LED_ORANGE; + led_color_7 = LED_ORANGE; + led_color_8 = LED_ORANGE; + led_blink = LED_BLINK; + }else{ + led_color_1 = LED_CYAN; + led_color_2 = LED_CYAN; + led_color_3 = LED_CYAN; + led_color_4 = LED_CYAN; + led_color_5 = LED_CYAN; + led_color_6 = LED_CYAN; + led_color_7 = LED_CYAN; + led_color_8 = LED_CYAN; + led_blink = LED_NOBLINK; + } } else { /* armed system - initial led pattern */ led_color_1 = LED_RED; @@ -593,23 +637,22 @@ BlinkM::led() led_blink = LED_BLINK; if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) { - - //XXX please check - if (vehicle_control_mode.flag_control_position_enabled) + /* indicate main control state */ + if (vehicle_status_raw.main_state == MAIN_STATE_EASY) led_color_4 = LED_GREEN; - else if (vehicle_control_mode.flag_control_velocity_enabled) + else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO) led_color_4 = LED_BLUE; - else if (vehicle_control_mode.flag_control_attitude_enabled) + else if (vehicle_status_raw.main_state == MAIN_STATE_SEATBELT) led_color_4 = LED_YELLOW; - else if (vehicle_control_mode.flag_control_manual_enabled) - led_color_4 = LED_AMBER; + else if (vehicle_status_raw.main_state == MAIN_STATE_MANUAL) + led_color_4 = LED_WHITE; else led_color_4 = LED_OFF; - + led_color_5 = led_color_4; } if(new_data_vehicle_gps_position || no_data_vehicle_gps_position < 3) { - /* handling used sat�s */ + /* handling used satus */ if(num_of_used_sats >= 7) { led_color_1 = LED_OFF; led_color_2 = LED_OFF; @@ -690,8 +733,11 @@ void BlinkM::setLEDColor(int ledcolor) { case LED_RED: // red set_rgb(255,0,0); break; + case LED_ORANGE: // orange + set_rgb(255,150,0); + break; case LED_YELLOW: // yellow - set_rgb(255,70,0); + set_rgb(200,200,0); break; case LED_PURPLE: // purple set_rgb(255,0,255); @@ -702,11 +748,14 @@ void BlinkM::setLEDColor(int ledcolor) { case LED_BLUE: // blue set_rgb(0,0,255); break; + case LED_CYAN: // cyan + set_rgb(0,128,128); + break; case LED_WHITE: // white set_rgb(255,255,255); break; case LED_AMBER: // amber - set_rgb(255,20,0); + set_rgb(255,65,0); break; } } From a4b10bab3042d653c5f59e704cb58008f7551401 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 18 Apr 2014 11:15:40 +0200 Subject: [PATCH 379/381] navigator: wrong mission topic was copied, clearer naming of offboard mission now --- src/modules/mavlink/mavlink_main.cpp | 4 ++-- src/modules/navigator/navigator_main.cpp | 6 +++--- src/modules/uORB/objects_common.cpp | 2 +- src/modules/uORB/topics/mission.h | 2 +- 4 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index f4a4ce86c6..2416a12640 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -832,10 +832,10 @@ void Mavlink::publish_mission() { /* Initialize mission publication if necessary */ if (_mission_pub < 0) { - _mission_pub = orb_advertise(ORB_ID(mission), &mission); + _mission_pub = orb_advertise(ORB_ID(offboard_mission), &mission); } else { - orb_publish(ORB_ID(mission), _mission_pub, &mission); + orb_publish(ORB_ID(offboard_mission), _mission_pub, &mission); } } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 5a94b66717..bcb3c8d0a1 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -510,7 +510,7 @@ Navigator::offboard_mission_update(bool isrotaryWing) { struct mission_s offboard_mission; - if (orb_copy(ORB_ID(mission), _offboard_mission_sub, &offboard_mission) == OK) { + if (orb_copy(ORB_ID(offboard_mission), _offboard_mission_sub, &offboard_mission) == OK) { /* Check mission feasibility, for now do not handle the return value, * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */ @@ -543,7 +543,7 @@ Navigator::onboard_mission_update() { struct mission_s onboard_mission; - if (orb_copy(ORB_ID(mission), _onboard_mission_sub, &onboard_mission) == OK) { + if (orb_copy(ORB_ID(onboard_mission), _onboard_mission_sub, &onboard_mission) == OK) { _mission.set_onboard_mission_count(onboard_mission.count); _mission.set_current_onboard_mission_index(onboard_mission.current_index); @@ -611,7 +611,7 @@ Navigator::task_main() * do subscriptions */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - _offboard_mission_sub = orb_subscribe(ORB_ID(mission)); + _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_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)); diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index c8a589bb7c..90675bb2e9 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -127,7 +127,7 @@ ORB_DEFINE(position_setpoint_triplet, struct position_setpoint_triplet_s); ORB_DEFINE(vehicle_global_velocity_setpoint, struct vehicle_global_velocity_setpoint_s); #include "topics/mission.h" -ORB_DEFINE(mission, struct mission_s); +ORB_DEFINE(offboard_mission, struct mission_s); ORB_DEFINE(onboard_mission, struct mission_s); #include "topics/mission_result.h" diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index 9594c4c6a2..ef4bc1defd 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -105,7 +105,7 @@ struct mission_s */ /* register this as object request broker structure */ -ORB_DECLARE(mission); +ORB_DECLARE(offboard_mission); ORB_DECLARE(onboard_mission); #endif From f8232fa2698b37983f4c3f05926c9d6a7a107acf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Apr 2014 02:05:20 +0200 Subject: [PATCH 380/381] fw_config_fixes --- ROMFS/px4fmu_common/init.d/3031_phantom | 8 ++++---- ROMFS/px4fmu_common/init.d/3032_skywalker_x5 | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/3031_phantom b/ROMFS/px4fmu_common/init.d/3031_phantom index 6cbd236430..24372bddcf 100644 --- a/ROMFS/px4fmu_common/init.d/3031_phantom +++ b/ROMFS/px4fmu_common/init.d/3031_phantom @@ -10,8 +10,8 @@ sh /etc/init.d/rc.fw_defaults if [ $DO_AUTOCONFIG == yes ] then param set FW_AIRSPD_MIN 13 - param set FW_AIRSPD_TRIM 18 - param set FW_AIRSPD_MAX 40 + param set FW_AIRSPD_TRIM 15 + param set FW_AIRSPD_MAX 25 param set FW_ATT_TC 0.3 param set FW_L1_DAMPING 0.75 param set FW_L1_PERIOD 15 @@ -23,12 +23,12 @@ then 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 0 + param set FW_P_ROLLFF 1 param set FW_RR_FF 0.5 param set FW_RR_I 0.02 param set FW_RR_IMAX 0.2 param set FW_RR_P 0.08 - param set FW_R_LIM 70 + param set FW_R_LIM 50 param set FW_R_RMAX 0 param set FW_T_HRATE_P 0.01 param set FW_T_RLL2THR 15 diff --git a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 index bf5a87068f..465166f253 100644 --- a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 +++ b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 @@ -23,7 +23,7 @@ then 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_P_ROLLFF 1 param set FW_RR_FF 0.3 param set FW_RR_I 0 param set FW_RR_IMAX 0.2 From 671c7a115a7c01ed89266a6631fb3929af84ffcf Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 21 Apr 2014 16:20:20 +0200 Subject: [PATCH 381/381] simple underspeed protection for mtecs --- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 27 +++++++++---- src/modules/fw_pos_control_l1/mtecs/mTecs.h | 10 +++-- .../fw_pos_control_l1/mtecs/mTecs_params.c | 40 ++++++++++++++++++- 3 files changed, 65 insertions(+), 12 deletions(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index 63e60995e8..1fb3153e9e 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -50,6 +50,7 @@ mTecs::mTecs() : SuperBlock(NULL, "MT"), /* Parameters */ _mTecsEnabled(this, "ENABLED"), + _airspeedMin(this, "FW_AIRSPD_MIN", false), /* control blocks */ _controlTotalEnergy(this, "THR"), _controlEnergyDistribution(this, "PIT", true), @@ -58,11 +59,13 @@ mTecs::mTecs() : _airspeedDerivative(this, "AD"), _throttleSp(0.0f), _pitchSp(0.0f), - BlockOutputLimiterTakeoffThrottle(this, "TKF_THR"), - BlockOutputLimiterTakeoffPitch(this, "TKF_PIT", true), + _BlockOutputLimiterTakeoffThrottle(this, "TKF_THR"), + _BlockOutputLimiterTakeoffPitch(this, "TKF_PIT", true), + _BlockOutputLimiterUnderspeedThrottle(this, "USP_THR"), + _BlockOutputLimiterUnderspeedPitch(this, "USP_PIT", true), timestampLastIteration(hrt_absolute_time()), _firstIterationAfterReset(true), - dtCalculated(false), + _dtCalculated(false), _counter(0) { } @@ -133,12 +136,20 @@ void mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float fligh (double)energyDistributionRateSp, (double)energyDistributionRate, (double)energyDistributionRateError, (double)energyDistributionRateError2); } + /* Check airspeed: if below safe value switch to underspeed mode */ + if (airspeed < _airspeedMin.get()) { + mode = TECS_MODE_UNDERSPEED; + } + /* Set special ouput limiters if we are not in TECS_MODE_NORMAL */ BlockOutputLimiter *outputLimiterThrottle = NULL; // NULL --> use standard inflight limits BlockOutputLimiter *outputLimiterPitch = NULL; // NULL --> use standard inflight limits if (mode == TECS_MODE_TAKEOFF) { - outputLimiterThrottle = &BlockOutputLimiterTakeoffThrottle; //XXX: accept prelaunch values from launchdetector - outputLimiterPitch = &BlockOutputLimiterTakeoffPitch; + outputLimiterThrottle = &_BlockOutputLimiterTakeoffThrottle; //XXX: accept prelaunch values from launchdetector + outputLimiterPitch = &_BlockOutputLimiterTakeoffPitch; + } else if (mode == TECS_MODE_UNDERSPEED) { + outputLimiterThrottle = &_BlockOutputLimiterUnderspeedThrottle; + outputLimiterPitch = &_BlockOutputLimiterUnderspeedPitch; } /** update control blocks **/ @@ -159,7 +170,7 @@ void mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float fligh /* clean up */ _firstIterationAfterReset = false; - dtCalculated = false; + _dtCalculated = false; _counter++; } @@ -174,7 +185,7 @@ void mTecs::resetIntegrators() void mTecs::updateTimeMeasurement() { - if (!dtCalculated) { + if (!_dtCalculated) { float deltaTSeconds = 0.0f; if (!_firstIterationAfterReset) { hrt_abstime timestampNow = hrt_absolute_time(); @@ -183,7 +194,7 @@ void mTecs::updateTimeMeasurement() } setDt(deltaTSeconds); - dtCalculated = true; + _dtCalculated = true; } } diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h index 6c30a458a9..393dd294d4 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -60,6 +60,7 @@ public: typedef enum { TECS_MODE_NORMAL, + TECS_MODE_UNDERSPEED, TECS_MODE_TAKEOFF } tecs_mode; @@ -92,6 +93,7 @@ public: protected: /* parameters */ control::BlockParamInt _mTecsEnabled; /**< 1 if mTecs is enabled */ + control::BlockParamFloat _airspeedMin; /**< minimal airspeed */ /* control blocks */ BlockFFPILimitedCustom _controlTotalEnergy; /**< FFPI controller for total energy control: output is throttle */ @@ -107,14 +109,16 @@ protected: float _pitchSp; /**< Pitch Setpoint from -pi to pi */ /* Output Limits in special modes */ - BlockOutputLimiter BlockOutputLimiterTakeoffThrottle; /**< Throttle Limits during takeoff */ - BlockOutputLimiter BlockOutputLimiterTakeoffPitch; /**< Pitch Limit during takeoff */ + BlockOutputLimiter _BlockOutputLimiterTakeoffThrottle; /**< Throttle Limits during takeoff */ + BlockOutputLimiter _BlockOutputLimiterTakeoffPitch; /**< Pitch Limit during takeoff */ + BlockOutputLimiter _BlockOutputLimiterUnderspeedThrottle; /**< Throttle Limits during takeoff */ + BlockOutputLimiter _BlockOutputLimiterUnderspeedPitch; /**< Pitch Limit during takeoff */ /* Time measurements */ hrt_abstime timestampLastIteration; /**< Saves the result of hrt_absolute_time() of the last iteration */ bool _firstIterationAfterReset; /**< True during the first iteration after a reset */ - bool dtCalculated; /**< True if dt has been calculated in this iteration */ + bool _dtCalculated; /**< True if dt has been calculated in this iteration */ int _counter; diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c index 85a4f3d488..fd501c17ad 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c @@ -247,7 +247,6 @@ PARAM_DEFINE_FLOAT(MT_ACC_MAX, 40.0f); */ PARAM_DEFINE_FLOAT(MT_AD_LP, 1.0f); - /** * Minimal throttle during takeoff * @@ -285,3 +284,42 @@ PARAM_DEFINE_FLOAT(MT_TKF_PIT_MIN, 0.0f); * @group mTECS */ PARAM_DEFINE_FLOAT(MT_TKF_PIT_MAX, 45.0f); + +/** + * Minimal throttle in underspeed mode + * + * @min 0.0f + * @max 1.0f + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_USP_THR_MIN, 1.0f); + +/** + * Maximal throttle in underspeed mode + * + * @min 0.0f + * @max 1.0f + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_USP_THR_MAX, 1.0f); + +/** + * Minimal pitch in underspeed mode + * + * @min -90.0f + * @max 90.0f + * @unit deg + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_USP_PIT_MIN, -45.0f); + +/** + * Maximal pitch in underspeed mode + * + * @min -90.0f + * @max 90.0f + * @unit deg + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_USP_PIT_MAX, 0.0f); +