diff options
| -rw-r--r-- | ROMFS/px4fmu_common/init.d/3035_viper | 1 | ||||
| -rw-r--r-- | ROMFS/px4fmu_common/init.d/rc.autostart | 6 | ||||
| -rw-r--r-- | ROMFS/px4fmu_common/init.d/rc.fw_apps | 2 | ||||
| -rw-r--r-- | ROMFS/px4fmu_common/init.d/rc.sensors | 5 | ||||
| -rwxr-xr-x | ROMFS/px4fmu_common/mixers/FMU_FX79.mix | 13 | ||||
| -rwxr-xr-x | ROMFS/px4fmu_common/mixers/Viper.mix | 2 | ||||
| -rw-r--r-- | makefiles/config_px4fmu-v2_default.mk | 5 | ||||
| -rw-r--r-- | src/drivers/gps/ubx.cpp | 12 | ||||
| -rw-r--r-- | src/drivers/gps/ubx.h | 17 | ||||
| -rw-r--r-- | src/drivers/hil/hil.cpp | 3 | ||||
| -rw-r--r-- | src/lib/external_lgpl/tecs/tecs.cpp | 2 | ||||
| -rw-r--r-- | src/modules/bottle_drop/bottle_drop.cpp | 907 | ||||
| -rw-r--r-- | src/modules/bottle_drop/bottle_drop_params.c | 131 | ||||
| -rw-r--r-- | src/modules/bottle_drop/module.mk | 41 | ||||
| -rw-r--r-- | src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 2 | ||||
| -rw-r--r-- | src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 135 | ||||
| -rw-r--r-- | src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c | 26 | ||||
| -rw-r--r-- | src/modules/mavlink/mavlink_main.cpp | 5 | ||||
| -rw-r--r-- | src/modules/navigator/navigator_main.cpp | 2 |
19 files changed, 1225 insertions, 92 deletions
diff --git a/ROMFS/px4fmu_common/init.d/3035_viper b/ROMFS/px4fmu_common/init.d/3035_viper index 3714b612f..f3b0e8418 100644 --- a/ROMFS/px4fmu_common/init.d/3035_viper +++ b/ROMFS/px4fmu_common/init.d/3035_viper @@ -9,4 +9,3 @@ sh /etc/init.d/rc.fw_defaults set MIXER Viper -set FAILSAFE "-c567 -p 1000" diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 9de7d9ecd..78778d806 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -21,6 +21,12 @@ # Simulation setups # +if param compare SYS_AUTOSTART 901 +then + sh /etc/init.d/901_bottle_drop_test.hil + set MODE custom +fi + if param compare SYS_AUTOSTART 1000 then sh /etc/init.d/1000_rc_fw_easystar.hil diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps index 9aca3fc5f..c97b3477f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_apps +++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps @@ -13,3 +13,5 @@ ekf_att_pos_estimator start # fw_att_control start fw_pos_control_l1 start + +bottle_drop start diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index ecb408a54..739df7ac0 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -68,6 +68,11 @@ else fi fi +# Check for flow sensor +if px4flow start +then +fi + # # Start the sensor collection task. # IMPORTANT: this also loads param offsets diff --git a/ROMFS/px4fmu_common/mixers/FMU_FX79.mix b/ROMFS/px4fmu_common/mixers/FMU_FX79.mix index 112d9b891..b8879af9e 100755 --- a/ROMFS/px4fmu_common/mixers/FMU_FX79.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_FX79.mix @@ -52,21 +52,18 @@ M: 1 O: 10000 10000 0 -10000 10000 S: 0 3 0 20000 -10000 -10000 10000 -Gimbal / flaps / payload mixer for last four channels +Inputs to the mixer come from channel group 2 (payload), channels 0 +(bay servo 1), 1 (bay servo 2) and 3 (drop release). ----------------------------------------------------- M: 1 O: 10000 10000 0 -10000 10000 -S: 0 4 10000 10000 0 -10000 10000 +S: 2 0 10000 10000 0 -10000 10000 M: 1 O: 10000 10000 0 -10000 10000 -S: 0 5 10000 10000 0 -10000 10000 +S: 2 1 10000 10000 0 -10000 10000 M: 1 O: 10000 10000 0 -10000 10000 -S: 0 6 10000 10000 0 -10000 10000 - -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 7 10000 10000 0 -10000 10000 +S: 2 2 -10000 -10000 0 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/Viper.mix b/ROMFS/px4fmu_common/mixers/Viper.mix index 5a0381bd8..5aa3828f2 100755 --- a/ROMFS/px4fmu_common/mixers/Viper.mix +++ b/ROMFS/px4fmu_common/mixers/Viper.mix @@ -66,6 +66,6 @@ S: 2 1 10000 10000 0 -10000 10000 M: 1 O: 10000 10000 0 -10000 10000 -S: 2 2 -10000 -10000 0 -10000 10000 +S: 2 2 -8000 -8000 0 -10000 10000 diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 5f146686c..d17dff5bb 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -129,6 +129,11 @@ MODULES += lib/conversion MODULES += lib/launchdetection # +# OBC challenge +# +MODULES += modules/bottle_drop + +# # Demo apps # #MODULES += examples/math_demo diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index d0854f5e9..b0eb4ab66 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -189,6 +189,18 @@ UBX::configure(unsigned &baudrate) return 1; } +#ifdef UBX_CONFIGURE_SBAS + /* send a SBAS message to set the SBAS options */ + memset(&_buf.payload_tx_cfg_sbas, 0, sizeof(_buf.payload_tx_cfg_sbas)); + _buf.payload_tx_cfg_sbas.mode = UBX_TX_CFG_SBAS_MODE; + + send_message(UBX_MSG_CFG_SBAS, _buf.raw, sizeof(_buf.payload_tx_cfg_sbas)); + + if (wait_for_ack(UBX_MSG_CFG_SBAS, UBX_CONFIG_TIMEOUT, true) < 0) { + return 1; + } +#endif + /* configure message rates */ /* the last argument is divisor for measurement rate (set by CFG RATE), i.e. 1 means 5Hz */ diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index 219a5762a..c74eb9168 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -73,6 +73,7 @@ #define UBX_ID_CFG_MSG 0x01 #define UBX_ID_CFG_RATE 0x08 #define UBX_ID_CFG_NAV5 0x24 +#define UBX_ID_CFG_SBAS 0x16 #define UBX_ID_MON_VER 0x04 #define UBX_ID_MON_HW 0x09 @@ -89,6 +90,7 @@ #define UBX_MSG_CFG_MSG ((UBX_CLASS_CFG) | UBX_ID_CFG_MSG << 8) #define UBX_MSG_CFG_RATE ((UBX_CLASS_CFG) | UBX_ID_CFG_RATE << 8) #define UBX_MSG_CFG_NAV5 ((UBX_CLASS_CFG) | UBX_ID_CFG_NAV5 << 8) +#define UBX_MSG_CFG_SBAS ((UBX_CLASS_CFG) | UBX_ID_CFG_SBAS << 8) #define UBX_MSG_MON_HW ((UBX_CLASS_MON) | UBX_ID_MON_HW << 8) #define UBX_MSG_MON_VER ((UBX_CLASS_MON) | UBX_ID_MON_VER << 8) @@ -128,6 +130,11 @@ #define UBX_TX_CFG_NAV5_DYNMODEL 7 /**< 0 Portable, 2 Stationary, 3 Pedestrian, 4 Automotive, 5 Sea, 6 Airborne <1g, 7 Airborne <2g, 8 Airborne <4g */ #define UBX_TX_CFG_NAV5_FIXMODE 2 /**< 1 2D only, 2 3D only, 3 Auto 2D/3D */ +/* TX CFG-SBAS message contents */ +#define UBX_TX_CFG_SBAS_MODE_ENABLED 1 /**< SBAS enabled */ +#define UBX_TX_CFG_SBAS_MODE_DISABLED 0 /**< SBAS disabled */ +#define UBX_TX_CFG_SBAS_MODE UBX_TX_CFG_SBAS_MODE_DISABLED /**< SBAS enabled or disabled */ + /* TX CFG-MSG message contents */ #define UBX_TX_CFG_MSG_RATE1_5HZ 0x01 /**< {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */ #define UBX_TX_CFG_MSG_RATE1_1HZ 0x05 /**< {0x00, 0x05, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */ @@ -383,6 +390,15 @@ typedef struct { uint32_t reserved4; } ubx_payload_tx_cfg_nav5_t; +/* tx cfg-sbas */ +typedef struct { + uint8_t mode; + uint8_t usage; + uint8_t maxSBAS; + uint8_t scanmode2; + uint32_t scanmode1; +} ubx_payload_tx_cfg_sbas_t; + /* Tx CFG-MSG */ typedef struct { union { @@ -413,6 +429,7 @@ typedef union { ubx_payload_tx_cfg_prt_t payload_tx_cfg_prt; ubx_payload_tx_cfg_rate_t payload_tx_cfg_rate; ubx_payload_tx_cfg_nav5_t payload_tx_cfg_nav5; + ubx_payload_tx_cfg_sbas_t payload_tx_cfg_sbas; ubx_payload_tx_cfg_msg_t payload_tx_cfg_msg; uint8_t raw[]; } ubx_buf_t; diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index f17e99e9d..f0dc0c651 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -392,7 +392,8 @@ HIL::task_main() if (fds[0].revents & POLLIN) { /* get controls - must always do this to avoid spinning */ - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls); + orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : + ORB_ID(actuator_controls_1), _t_actuators, &_controls); /* can we mix? */ if (_mixers != nullptr) { diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index 023bd71bf..6a2a61b04 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -236,9 +236,9 @@ void TECS::_update_height_demand(float demand, float state) // // _hgt_rate_dem); _hgt_dem_adj = demand;//0.025f * demand + 0.975f * _hgt_dem_adj_last; + _hgt_rate_dem = (_hgt_dem_adj-state)*_heightrate_p + _heightrate_ff * (_hgt_dem_adj - _hgt_dem_adj_last)/_DT; _hgt_dem_adj_last = _hgt_dem_adj; - _hgt_rate_dem = (_hgt_dem_adj-state)*_heightrate_p + _heightrate_ff * (_hgt_dem_adj - _hgt_dem_adj_last)/_DT; // Limit height rate of change if (_hgt_rate_dem > _maxClimbRate) { _hgt_rate_dem = _maxClimbRate; diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp new file mode 100644 index 000000000..31c9157e1 --- /dev/null +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -0,0 +1,907 @@ +/**************************************************************************** + * + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file bottle_drop.cpp + * + * Bottle drop module for Outback Challenge 2014, Team Swiss Fang + * + * @author Dominik Juchli <juchlid@ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + */ + +#include <nuttx/config.h> +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <math.h> +#include <poll.h> +#include <time.h> +#include <sys/ioctl.h> +#include <drivers/device/device.h> +#include <drivers/drv_hrt.h> +#include <arch/board/board.h> +#include <uORB/uORB.h> +#include <uORB/topics/vehicle_command.h> +#include <uORB/topics/actuator_controls.h> +#include <uORB/topics/wind_estimate.h> +#include <uORB/topics/parameter_update.h> +#include <systemlib/systemlib.h> +#include <systemlib/param/param.h> +#include <systemlib/err.h> +#include <geo/geo.h> +#include <dataman/dataman.h> +#include <mathlib/mathlib.h> +#include <mavlink/mavlink_log.h> + + +/** + * bottle_drop app start / stop handling function + * + * @ingroup apps + */ +extern "C" __EXPORT int bottle_drop_main(int argc, char *argv[]); + +class BottleDrop +{ +public: + /** + * Constructor + */ + BottleDrop(); + + /** + * Destructor, also kills task. + */ + ~BottleDrop(); + + /** + * Start the task. + * + * @return OK on success. + */ + int start(); + + /** + * Display status. + */ + void status(); + + void open_bay(); + void close_bay(); + void drop(); + void lock_release(); + +private: + bool _task_should_exit; /**< if true, task should exit */ + int _main_task; /**< handle for task */ + int _mavlink_fd; + + int _command_sub; + int _wind_estimate_sub; + struct vehicle_command_s _command; + struct vehicle_global_position_s _global_pos; + map_projection_reference_s ref; + + orb_advert_t _actuator_pub; + struct actuator_controls_s _actuators; + + bool _drop_approval; + hrt_abstime _doors_opened; + hrt_abstime _drop_time; + + float _alt_clearance; + + struct position_s { + double lat; ///< degrees + double lon; ///< degrees + float alt; ///< m + } _target_position, _drop_position; + + enum DROP_STATE { + DROP_STATE_INIT = 0, + DROP_STATE_TARGET_VALID, + DROP_STATE_TARGET_SET, + DROP_STATE_BAY_OPEN, + DROP_STATE_DROPPED, + DROP_STATE_BAY_CLOSED + } _drop_state; + + struct mission_s _onboard_mission; + orb_advert_t _onboard_mission_pub; + + void task_main(); + + void handle_command(struct vehicle_command_s *cmd); + + void answer_command(struct vehicle_command_s *cmd, enum VEHICLE_CMD_RESULT result); + + /** + * Set the actuators + */ + int actuators_publish(); + + /** + * Shim for calling task_main from task_create. + */ + static void task_main_trampoline(int argc, char *argv[]); +}; + +namespace bottle_drop +{ +BottleDrop *g_bottle_drop; +} + +BottleDrop::BottleDrop() : + + _task_should_exit(false), + _main_task(-1), + _mavlink_fd(-1), + _command_sub(-1), + _wind_estimate_sub(-1), + _command {}, + _global_pos {}, + ref {}, + _actuator_pub(-1), + _actuators {}, + _drop_approval(false), + _doors_opened(0), + _drop_time(0), + _alt_clearance(70.0f), + _target_position {}, + _drop_position {}, + _drop_state(DROP_STATE_INIT), + _onboard_mission {}, + _onboard_mission_pub(-1) +{ +} + +BottleDrop::~BottleDrop() +{ + if (_main_task != -1) { + + /* task wakes up every 100ms or so at the longest */ + _task_should_exit = true; + + /* wait for a second for the task to quit at our request */ + unsigned i = 0; + + do { + /* wait 20ms */ + usleep(20000); + + /* if we have given up, kill it */ + if (++i > 50) { + task_delete(_main_task); + break; + } + } while (_main_task != -1); + } + + bottle_drop::g_bottle_drop = nullptr; +} + +int +BottleDrop::start() +{ + ASSERT(_main_task == -1); + + /* start the task */ + _main_task = task_spawn_cmd("bottle_drop", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT + 15, + 2048, + (main_t)&BottleDrop::task_main_trampoline, + nullptr); + + if (_main_task < 0) { + warn("task start failed"); + return -errno; + } + + return OK; +} + + +void +BottleDrop::status() +{ + warnx("drop state: %d", _drop_state); +} + +void +BottleDrop::open_bay() +{ + _actuators.control[0] = -1.0f; + _actuators.control[1] = 1.0f; + + if (_doors_opened == 0) { + _doors_opened = hrt_absolute_time(); + } + warnx("open doors"); + + actuators_publish(); + + usleep(500 * 1000); +} + +void +BottleDrop::close_bay() +{ + // closed door and locked survival kit + _actuators.control[0] = 1.0f; + _actuators.control[1] = -1.0f; + + _doors_opened = 0; + + actuators_publish(); + + // delay until the bay is closed + usleep(500 * 1000); +} + +void +BottleDrop::drop() +{ + + // update drop actuator, wait 0.5s until the doors are open before dropping + hrt_abstime starttime = hrt_absolute_time(); + + // force the door open if we have to + if (_doors_opened == 0) { + open_bay(); + warnx("bay not ready, forced open"); + } + + while (hrt_elapsed_time(&_doors_opened) < 500 * 1000 && hrt_elapsed_time(&starttime) < 2000000) { + usleep(50000); + warnx("delayed by door!"); + } + + _actuators.control[2] = 1.0f; + + _drop_time = hrt_absolute_time(); + actuators_publish(); + + warnx("dropping now"); + + // Give it time to drop + usleep(1000 * 1000); +} + +void +BottleDrop::lock_release() +{ + _actuators.control[2] = -1.0f; + actuators_publish(); + + warnx("closing release"); +} + +int +BottleDrop::actuators_publish() +{ + _actuators.timestamp = hrt_absolute_time(); + + // lazily publish _actuators only once available + if (_actuator_pub > 0) { + return orb_publish(ORB_ID(actuator_controls_2), _actuator_pub, &_actuators); + + } else { + _actuator_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators); + if (_actuator_pub > 0) { + return OK; + } else { + return -1; + } + } +} + +void +BottleDrop::task_main() +{ + + _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + mavlink_log_info(_mavlink_fd, "[bottle_drop] started"); + + _command_sub = orb_subscribe(ORB_ID(vehicle_command)); + _wind_estimate_sub = orb_subscribe(ORB_ID(wind_estimate)); + + bool updated = false; + + float z_0; // ground properties + float turn_radius; // turn radius of the UAV + float precision; // Expected precision of the UAV + + float ground_distance = _alt_clearance; // Replace by closer estimate in loop + + // constant + float g = CONSTANTS_ONE_G; // constant of gravity [m/s^2] + float m = 0.5f; // mass of bottle [kg] + float rho = 1.2f; // air density [kg/m^3] + float A = ((0.063f * 0.063f) / 4.0f * M_PI_F); // Bottle cross section [m^2] + float dt_freefall_prediction = 0.01f; // step size of the free fall prediction [s] + + // Has to be estimated by experiment + float cd = 0.86f; // Drag coefficient for a cylinder with a d/l ratio of 1/3 [] + float t_signal = + 0.084f; // Time span between sending the signal and the bottle top reaching level height with the bottom of the plane [s] + float t_door = + 0.7f; // The time the system needs to open the door + safety, is also the time the palyload needs to safely escape the shaft [s] + + + // Definition + float h_0; // height over target + float az; // acceleration in z direction[m/s^2] + float vz; // velocity in z direction [m/s] + float z; // fallen distance [m] + float h; // height over target [m] + float ax; // acceleration in x direction [m/s^2] + float vx; // ground speed in x direction [m/s] + float x; // traveled distance in x direction [m] + float vw; // wind speed [m/s] + float vrx; // relative velocity in x direction [m/s] + float v; // relative speed vector [m/s] + float Fd; // Drag force [N] + float Fdx; // Drag force in x direction [N] + float Fdz; // Drag force in z direction [N] + float x_drop, y_drop; // coordinates of the drop point in reference to the target (projection of NED) + float x_t, y_t; // coordinates of the target in reference to the target x_t = 0, y_t = 0 (projection of NED) + float x_l, y_l; // local position in projected coordinates + float x_f, y_f; // to-be position of the UAV after dt_runs seconds in projected coordinates + double x_f_NED, y_f_NED; // to-be position of the UAV after dt_runs seconds in NED + float distance_open_door; // The distance the UAV travels during its doors open [m] + float approach_error = 0.0f; // The error in radians between current ground vector and desired ground vector + float distance_real = 0; // The distance between the UAVs position and the drop point [m] + float future_distance = 0; // The distance between the UAVs to-be position and the drop point [m] + + unsigned counter = 0; + + param_t param_gproperties = param_find("BD_GPROPERTIES"); + param_t param_turn_radius = param_find("BD_TURNRADIUS"); + param_t param_precision = param_find("BD_PRECISION"); + param_t param_cd = param_find("BD_OBJ_CD"); + param_t param_mass = param_find("BD_OBJ_MASS"); + param_t param_surface = param_find("BD_OBJ_SURFACE"); + + + param_get(param_precision, &precision); + param_get(param_turn_radius, &turn_radius); + param_get(param_gproperties, &z_0); + param_get(param_cd, &cd); + param_get(param_mass, &m); + param_get(param_surface, &A); + + int vehicle_global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + + struct parameter_update_s update; + memset(&update, 0, sizeof(update)); + int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); + + struct mission_item_s flight_vector_s {}; + struct mission_item_s flight_vector_e {}; + + flight_vector_s.nav_cmd = NAV_CMD_WAYPOINT; + flight_vector_s.acceptance_radius = 50; // TODO: make parameter + flight_vector_s.autocontinue = true; + flight_vector_s.altitude_is_relative = false; + + flight_vector_e.nav_cmd = NAV_CMD_WAYPOINT; + flight_vector_e.acceptance_radius = 50; // TODO: make parameter + flight_vector_e.autocontinue = true; + flight_vector_s.altitude_is_relative = false; + + struct wind_estimate_s wind; + + // wakeup source(s) + struct pollfd fds[1]; + + // Setup of loop + fds[0].fd = _command_sub; + fds[0].events = POLLIN; + + // Whatever state the bay is in, we want it closed on startup + lock_release(); + close_bay(); + + while (!_task_should_exit) { + + /* wait for up to 100ms for data */ + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 50); + + /* this is undesirable but not much we can do - might want to flag unhappy status */ + if (pret < 0) { + warn("poll error %d, %d", pret, errno); + continue; + } + + /* vehicle commands updated */ + if (fds[0].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_command), _command_sub, &_command); + handle_command(&_command); + } + + orb_check(vehicle_global_position_sub, &updated); + if (updated) { + /* copy global position */ + orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &_global_pos); + } + + if (_global_pos.timestamp == 0) { + continue; + } + + const unsigned sleeptime_us = 9500; + + hrt_abstime last_run = hrt_absolute_time(); + float dt_runs = sleeptime_us / 1e6f; + + // switch to faster updates during the drop + while (_drop_state > DROP_STATE_INIT) { + + // Get wind estimate + orb_check(_wind_estimate_sub, &updated); + if (updated) { + orb_copy(ORB_ID(wind_estimate), _wind_estimate_sub, &wind); + } + + // Get vehicle position + orb_check(vehicle_global_position_sub, &updated); + if (updated) { + // copy global position + orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &_global_pos); + } + + // Get parameter updates + orb_check(parameter_update_sub, &updated); + if (updated) { + // copy global position + orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); + + // update all parameters + param_get(param_gproperties, &z_0); + param_get(param_turn_radius, &turn_radius); + param_get(param_precision, &precision); + } + + orb_check(_command_sub, &updated); + if (updated) { + orb_copy(ORB_ID(vehicle_command), _command_sub, &_command); + handle_command(&_command); + } + + + float windspeed_norm = sqrtf(wind.windspeed_north * wind.windspeed_north + wind.windspeed_east * wind.windspeed_east); + float groundspeed_body = sqrtf(_global_pos.vel_n * _global_pos.vel_n + _global_pos.vel_e * _global_pos.vel_e); + ground_distance = _global_pos.alt - _target_position.alt; + + // Distance to drop position and angle error to approach vector + // are relevant in all states greater than target valid (which calculates these positions) + if (_drop_state > DROP_STATE_TARGET_VALID) { + distance_real = fabsf(get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, _drop_position.lat, _drop_position.lon)); + + float ground_direction = atan2f(_global_pos.vel_e, _global_pos.vel_n); + float approach_direction = get_bearing_to_next_waypoint(flight_vector_s.lat, flight_vector_s.lon, flight_vector_e.lat, flight_vector_e.lon); + + approach_error = _wrap_pi(ground_direction - approach_direction); + + if (counter % 90 == 0) { + mavlink_log_info(_mavlink_fd, "drop distance %u, heading error %u", (unsigned)distance_real, (unsigned)math::degrees(approach_error)); + } + } + + switch (_drop_state) { + + case DROP_STATE_TARGET_VALID: + { + + az = g; // acceleration in z direction[m/s^2] + vz = 0; // velocity in z direction [m/s] + z = 0; // fallen distance [m] + h_0 = _global_pos.alt - _target_position.alt; // height over target at start[m] + h = h_0; // height over target [m] + ax = 0; // acceleration in x direction [m/s^2] + vx = groundspeed_body;// XXX project // ground speed in x direction [m/s] + x = 0; // traveled distance in x direction [m] + vw = 0; // wind speed [m/s] + vrx = 0; // relative velocity in x direction [m/s] + v = groundspeed_body; // relative speed vector [m/s] + Fd = 0; // Drag force [N] + Fdx = 0; // Drag force in x direction [N] + Fdz = 0; // Drag force in z direction [N] + + + // Compute the distance the bottle will travel after it is dropped in body frame coordinates --> x + while (h > 0.05f) { + // z-direction + vz = vz + az * dt_freefall_prediction; + z = z + vz * dt_freefall_prediction; + h = h_0 - z; + + // x-direction + vw = windspeed_norm * logf(h / z_0) / logf(ground_distance / z_0); + vx = vx + ax * dt_freefall_prediction; + x = x + vx * dt_freefall_prediction; + vrx = vx + vw; + + // drag force + v = sqrtf(vz * vz + vrx * vrx); + Fd = 0.5f * rho * A * cd * (v * v); + Fdx = Fd * vrx / v; + Fdz = Fd * vz / v; + + // acceleration + az = g - Fdz / m; + ax = -Fdx / m; + } + + // compute drop vector + x = groundspeed_body * t_signal + x; + + x_t = 0.0f; + y_t = 0.0f; + + float wind_direction_n, wind_direction_e; + + if (windspeed_norm < 0.5f) { // If there is no wind, an arbitrarily direction is chosen + wind_direction_n = 1.0f; + wind_direction_e = 0.0f; + + } else { + wind_direction_n = wind.windspeed_north / windspeed_norm; + wind_direction_e = wind.windspeed_east / windspeed_norm; + } + + x_drop = x_t + x * wind_direction_n; + y_drop = y_t + x * wind_direction_e; + map_projection_reproject(&ref, x_drop, y_drop, &_drop_position.lat, &_drop_position.lon); + _drop_position.alt = _target_position.alt + _alt_clearance; + + // Compute flight vector + map_projection_reproject(&ref, x_drop + 2 * turn_radius * wind_direction_n, y_drop + 2 * turn_radius * wind_direction_e, + &(flight_vector_s.lat), &(flight_vector_s.lon)); + flight_vector_s.altitude = _drop_position.alt; + map_projection_reproject(&ref, x_drop - turn_radius * wind_direction_n, y_drop - turn_radius * wind_direction_e, + &flight_vector_e.lat, &flight_vector_e.lon); + flight_vector_e.altitude = _drop_position.alt; + + // Save WPs in datamanager + const ssize_t len = sizeof(struct mission_item_s); + + if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 0, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_s, len) != len) { + warnx("ERROR: could not save onboard WP"); + } + + if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 1, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_e, len) != len) { + warnx("ERROR: could not save onboard WP"); + } + + _onboard_mission.count = 2; + _onboard_mission.current_seq = 0; + + if (_onboard_mission_pub > 0) { + orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); + + } else { + _onboard_mission_pub = orb_advertise(ORB_ID(onboard_mission), &_onboard_mission); + } + + float approach_direction = get_bearing_to_next_waypoint(flight_vector_s.lat, flight_vector_s.lon, flight_vector_e.lat, flight_vector_e.lon); + mavlink_log_critical(_mavlink_fd, "position set, approach heading: %u", (unsigned)distance_real, (unsigned)math::degrees(approach_direction + M_PI_F)); + + _drop_state = DROP_STATE_TARGET_SET; + } + break; + + case DROP_STATE_TARGET_SET: + { + float distance_wp2 = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, flight_vector_e.lat, flight_vector_e.lon); + + if (distance_wp2 < distance_real) { + _onboard_mission.current_seq = 0; + orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); + } else { + + // We're close enough - open the bay + distance_open_door = math::max(10.0f, 3.0f * fabsf(t_door * groundspeed_body)); + + if (isfinite(distance_real) && distance_real < distance_open_door && + fabsf(approach_error) < math::radians(20.0f)) { + open_bay(); + _drop_state = DROP_STATE_BAY_OPEN; + mavlink_log_info(_mavlink_fd, "#audio: opening bay"); + } + } + } + break; + + case DROP_STATE_BAY_OPEN: + { + if (_drop_approval) { + map_projection_project(&ref, _global_pos.lat, _global_pos.lon, &x_l, &y_l); + x_f = x_l + _global_pos.vel_n * dt_runs; + y_f = y_l + _global_pos.vel_e * dt_runs; + map_projection_reproject(&ref, x_f, y_f, &x_f_NED, &y_f_NED); + future_distance = get_distance_to_next_waypoint(x_f_NED, y_f_NED, _drop_position.lat, _drop_position.lon); + + if (isfinite(distance_real) && + (distance_real < precision) && ((distance_real < future_distance))) { + drop(); + _drop_state = DROP_STATE_DROPPED; + mavlink_log_info(_mavlink_fd, "#audio: payload dropped"); + } else { + + float distance_wp2 = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, flight_vector_e.lat, flight_vector_e.lon); + + if (distance_wp2 < distance_real) { + _onboard_mission.current_seq = 0; + orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); + } + } + } + } + break; + + case DROP_STATE_DROPPED: + /* 2s after drop, reset and close everything again */ + if ((hrt_elapsed_time(&_doors_opened) > 2 * 1000 * 1000)) { + _drop_state = DROP_STATE_INIT; + _drop_approval = false; + lock_release(); + close_bay(); + mavlink_log_info(_mavlink_fd, "#audio: closing bay"); + + // remove onboard mission + _onboard_mission.current_seq = -1; + _onboard_mission.count = 0; + orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); + } + break; + } + + counter++; + + // update_actuators(); + + // run at roughly 100 Hz + usleep(sleeptime_us); + + dt_runs = hrt_elapsed_time(&last_run) / 1e6f; + last_run = hrt_absolute_time(); + } + } + + warnx("exiting."); + + _main_task = -1; + _exit(0); +} + +void +BottleDrop::handle_command(struct vehicle_command_s *cmd) +{ + switch (cmd->command) { + case VEHICLE_CMD_CUSTOM_0: + /* + * param1 and param2 set to 1: open and drop + * param1 set to 1: open + * else: close (and don't drop) + */ + if (cmd->param1 > 0.5f && cmd->param2 > 0.5f) { + open_bay(); + drop(); + mavlink_log_info(_mavlink_fd, "#audio: drop bottle"); + + } else if (cmd->param1 > 0.5f) { + open_bay(); + mavlink_log_info(_mavlink_fd, "#audio: opening bay"); + + } else { + lock_release(); + close_bay(); + mavlink_log_info(_mavlink_fd, "#audio: closing bay"); + } + + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + break; + + case VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY: + + switch ((int)(cmd->param1 + 0.5f)) { + case 0: + _drop_approval = false; + mavlink_log_info(_mavlink_fd, "#audio: got drop position, no approval"); + break; + + case 1: + _drop_approval = true; + mavlink_log_info(_mavlink_fd, "#audio: got drop position and approval"); + break; + + default: + _drop_approval = false; + warnx("param1 val unknown"); + break; + } + + // XXX check all fields (2-3) + _alt_clearance = cmd->param4; + _target_position.lat = cmd->param5; + _target_position.lon = cmd->param6; + _target_position.alt = cmd->param7; + _drop_state = DROP_STATE_TARGET_VALID; + mavlink_log_info(_mavlink_fd, "got target: %8.4f, %8.4f, %8.4f", (double)_target_position.lat, + (double)_target_position.lon, (double)_target_position.alt); + map_projection_init(&ref, _target_position.lat, _target_position.lon); + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + break; + + case VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY: + + if (cmd->param1 < 0) { + + // Clear internal states + _drop_approval = false; + _drop_state = DROP_STATE_INIT; + + // Abort if mission is present + _onboard_mission.current_seq = -1; + + if (_onboard_mission_pub > 0) { + orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); + } + + } else { + switch ((int)(cmd->param1 + 0.5f)) { + case 0: + _drop_approval = false; + break; + + case 1: + _drop_approval = true; + mavlink_log_info(_mavlink_fd, "#audio: got drop approval"); + break; + + default: + _drop_approval = false; + break; + // XXX handle other values + } + } + + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + break; + + default: + break; + } +} + +void +BottleDrop::answer_command(struct vehicle_command_s *cmd, enum VEHICLE_CMD_RESULT result) +{ + switch (result) { + case VEHICLE_CMD_RESULT_ACCEPTED: + break; + + case VEHICLE_CMD_RESULT_DENIED: + mavlink_log_critical(_mavlink_fd, "#audio: command denied: %u", cmd->command); + break; + + case VEHICLE_CMD_RESULT_FAILED: + mavlink_log_critical(_mavlink_fd, "#audio: command failed: %u", cmd->command); + break; + + case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED: + mavlink_log_critical(_mavlink_fd, "#audio: command temporarily rejected: %u", cmd->command); + break; + + case VEHICLE_CMD_RESULT_UNSUPPORTED: + mavlink_log_critical(_mavlink_fd, "#audio: command unsupported: %u", cmd->command); + break; + + default: + break; + } +} + +void +BottleDrop::task_main_trampoline(int argc, char *argv[]) +{ + bottle_drop::g_bottle_drop->task_main(); +} + +static void usage() +{ + errx(1, "usage: bottle_drop {start|stop|status}"); +} + +int bottle_drop_main(int argc, char *argv[]) +{ + if (argc < 2) { + usage(); + } + + if (!strcmp(argv[1], "start")) { + + if (bottle_drop::g_bottle_drop != nullptr) { + errx(1, "already running"); + } + + bottle_drop::g_bottle_drop = new BottleDrop; + + if (bottle_drop::g_bottle_drop == nullptr) { + errx(1, "alloc failed"); + } + + if (OK != bottle_drop::g_bottle_drop->start()) { + delete bottle_drop::g_bottle_drop; + bottle_drop::g_bottle_drop = nullptr; + err(1, "start failed"); + } + + return 0; + } + + if (bottle_drop::g_bottle_drop == nullptr) { + errx(1, "not running"); + } + + if (!strcmp(argv[1], "stop")) { + delete bottle_drop::g_bottle_drop; + bottle_drop::g_bottle_drop = nullptr; + + } else if (!strcmp(argv[1], "status")) { + bottle_drop::g_bottle_drop->status(); + + } else if (!strcmp(argv[1], "drop")) { + bottle_drop::g_bottle_drop->drop(); + + } else if (!strcmp(argv[1], "open")) { + bottle_drop::g_bottle_drop->open_bay(); + + } else if (!strcmp(argv[1], "close")) { + bottle_drop::g_bottle_drop->close_bay(); + + } else if (!strcmp(argv[1], "lock")) { + bottle_drop::g_bottle_drop->lock_release(); + + } else { + usage(); + } + + return 0; +} diff --git a/src/modules/bottle_drop/bottle_drop_params.c b/src/modules/bottle_drop/bottle_drop_params.c new file mode 100644 index 000000000..51ebfb9a1 --- /dev/null +++ b/src/modules/bottle_drop/bottle_drop_params.c @@ -0,0 +1,131 @@ +/**************************************************************************** + * + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file bottle_drop_params.c + * Bottle drop parameters + * + * @author Dominik Juchli <juchlid@ethz.ch> + */ + +#include <nuttx/config.h> +#include <systemlib/param/param.h> + +/** + * Ground drag property + * + * This parameter encodes the ground drag coefficient and the corresponding + * decrease in wind speed from the plane altitude to ground altitude. + * + * @unit unknown + * @min 0.001 + * @max 0.1 + * @group Payload drop + */ +PARAM_DEFINE_FLOAT(BD_GPROPERTIES, 0.03f); + +/** + * Plane turn radius + * + * The planes known minimal turn radius - use a higher value + * to make the plane maneuver more distant from the actual drop + * position. This is to ensure the wings are level during the drop. + * + * @unit meter + * @min 30.0 + * @max 500.0 + * @group Payload drop + */ +PARAM_DEFINE_FLOAT(BD_TURNRADIUS, 120.0f); + +/** + * Drop precision + * + * If the system is closer than this distance on passing over the + * drop position, it will release the payload. This is a safeguard + * to prevent a drop out of the required accuracy. + * + * @unit meter + * @min 1.0 + * @max 80.0 + * @group Payload drop + */ +PARAM_DEFINE_FLOAT(BD_PRECISION, 30.0f); + +/** + * Payload drag coefficient of the dropped object + * + * The drag coefficient (cd) is the typical drag + * constant for air. It is in general object specific, + * but the closest primitive shape to the actual object + * should give good results: + * http://en.wikipedia.org/wiki/Drag_coefficient + * + * @unit meter + * @min 0.08 + * @max 1.5 + * @group Payload drop + */ +PARAM_DEFINE_FLOAT(BD_OBJ_CD, 0.1f); + +/** + * Payload mass + * + * A typical small toy ball: + * 0.025 kg + * + * OBC water bottle: + * 0.6 kg + * + * @unit kilogram + * @min 0.001 + * @max 5.0 + * @group Payload drop + */ +PARAM_DEFINE_FLOAT(BD_OBJ_MASS, 0.6f); + +/** + * Payload front surface area + * + * A typical small toy ball: + * (0.045 * 0.045) / 4.0 * pi = 0.001590 m^2 + * + * OBC water bottle: + * (0.063 * 0.063) / 4.0 * pi = 0.003117 m^2 + * + * @unit m^2 + * @min 0.001 + * @max 0.5 + * @group Payload drop + */ +PARAM_DEFINE_FLOAT(BD_OBJ_SURFACE, 0.00311724531f); diff --git a/src/modules/bottle_drop/module.mk b/src/modules/bottle_drop/module.mk new file mode 100644 index 000000000..6b18fff55 --- /dev/null +++ b/src/modules/bottle_drop/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. +# +############################################################################ + +# +# Daemon application +# + +MODULE_COMMAND = bottle_drop + +SRCS = bottle_drop.cpp \ + bottle_drop_params.c diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 97abb76a9..2c50e5c75 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1369,7 +1369,7 @@ FixedwingEstimator::task_main() if (newRangeData) { _ekf->fuseRngData = true; _ekf->useRangeFinder = true; - _ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 500.0f)); + _ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 100.0f)); _ekf->GroundEKF(); } 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 fdb1b2429..6017369aa 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -88,7 +88,6 @@ #include <launchdetection/LaunchDetector.h> #include <ecl/l1/ecl_l1_pos_controller.h> #include <external_lgpl/tecs/tecs.h> -#include <drivers/drv_range_finder.h> #include "landingslope.h" #include "mtecs/mTecs.h" @@ -146,7 +145,6 @@ private: int _params_sub; /**< notification of parameter updates */ int _manual_control_sub; /**< notification of manual control updates */ int _sensor_combined_sub; /**< for body frame accelerations */ - int _range_finder_sub; /**< range finder subscription */ orb_advert_t _attitude_sp_pub; /**< attitude setpoint */ orb_advert_t _tecs_status_pub; /**< TECS status publication */ @@ -162,17 +160,16 @@ private: struct vehicle_global_position_s _global_pos; /**< global vehicle position */ struct position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of mission items */ struct sensor_combined_s _sensor_combined; /**< for body frame accelerations */ - struct range_finder_report _range_finder; /**< range finder report */ perf_counter_t _loop_perf; /**< loop performance counter */ /* land states */ - /* not in non-abort mode for landing yet */ bool land_noreturn_horizontal; bool land_noreturn_vertical; bool land_stayonground; bool land_motor_lim; bool land_onslope; + bool land_useterrain; /* takeoff/launch states */ LaunchDetectionResult launch_detection_state; @@ -243,7 +240,9 @@ private: float land_flare_alt_relative; float land_thrust_lim_alt_relative; float land_heading_hold_horizontal_distance; - float range_finder_rel_alt; + float land_flare_pitch_min_deg; + float land_flare_pitch_max_deg; + int land_use_terrain_estimate; } _parameters; /**< local copies of interesting parameters */ @@ -289,7 +288,9 @@ private: param_t land_flare_alt_relative; param_t land_thrust_lim_alt_relative; param_t land_heading_hold_horizontal_distance; - param_t range_finder_rel_alt; + param_t land_flare_pitch_min_deg; + param_t land_flare_pitch_max_deg; + param_t land_use_terrain_estimate; } _parameter_handles; /**< handles for interesting parameters */ @@ -321,12 +322,6 @@ private: bool vehicle_airspeed_poll(); /** - * Check for range finder updates. - */ - bool range_finder_poll(); - - - /** * Check for position updates. */ void vehicle_attitude_poll(); @@ -347,9 +342,9 @@ private: void navigation_capabilities_publish(); /** - * Get the relative alt either from the difference between estimate and waypoint or from the laser range finder + * Return the terrain estimate during landing: uses the wp altitude value or the terrain estimate if available */ - float get_relative_landingalt(float land_setpoint_alt, float current_alt, const struct range_finder_report &range_finder, float range_finder_use_relative_alt); + float get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos); /** * Control position. @@ -423,7 +418,6 @@ FixedwingPositionControl::FixedwingPositionControl() : _params_sub(-1), _manual_control_sub(-1), _sensor_combined_sub(-1), - _range_finder_sub(-1), /* publications */ _attitude_sp_pub(-1), @@ -441,7 +435,6 @@ FixedwingPositionControl::FixedwingPositionControl() : _global_pos(), _pos_sp_triplet(), _sensor_combined(), - _range_finder(), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")), @@ -451,6 +444,7 @@ FixedwingPositionControl::FixedwingPositionControl() : land_stayonground(false), land_motor_lim(false), land_onslope(false), + land_useterrain(false), launch_detection_state(LAUNCHDETECTION_RES_NONE), last_manual(false), landingslope(), @@ -489,7 +483,9 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT"); _parameter_handles.land_thrust_lim_alt_relative = param_find("FW_LND_TLALT"); _parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST"); - _parameter_handles.range_finder_rel_alt = param_find("FW_LND_RFRALT"); + _parameter_handles.land_flare_pitch_min_deg = param_find("FW_FLARE_PMIN"); + _parameter_handles.land_flare_pitch_max_deg = param_find("FW_FLARE_PMAX"); + _parameter_handles.land_use_terrain_estimate= param_find("FW_LND_USETER"); _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); _parameter_handles.time_const_throt = param_find("FW_T_THRO_CONST"); @@ -590,8 +586,9 @@ FixedwingPositionControl::parameters_update() } param_get(_parameter_handles.land_heading_hold_horizontal_distance, &(_parameters.land_heading_hold_horizontal_distance)); - - param_get(_parameter_handles.range_finder_rel_alt, &(_parameters.range_finder_rel_alt)); + param_get(_parameter_handles.land_flare_pitch_min_deg, &(_parameters.land_flare_pitch_min_deg)); + param_get(_parameter_handles.land_flare_pitch_max_deg, &(_parameters.land_flare_pitch_max_deg)); + param_get(_parameter_handles.land_use_terrain_estimate, &(_parameters.land_use_terrain_estimate)); _l1_control.set_l1_damping(_parameters.l1_damping); _l1_control.set_l1_period(_parameters.l1_period); @@ -695,20 +692,6 @@ FixedwingPositionControl::vehicle_airspeed_poll() return airspeed_updated; } -bool -FixedwingPositionControl::range_finder_poll() -{ - /* check if there is a range finder measurement */ - bool range_finder_updated; - orb_check(_range_finder_sub, &range_finder_updated); - - if (range_finder_updated) { - orb_copy(ORB_ID(sensor_range_finder), _range_finder_sub, &_range_finder); - } - - return range_finder_updated; -} - void FixedwingPositionControl::vehicle_attitude_poll() { @@ -846,21 +829,23 @@ void FixedwingPositionControl::navigation_capabilities_publish() } } -float FixedwingPositionControl::get_relative_landingalt(float land_setpoint_alt, float current_alt, const struct range_finder_report &range_finder, float range_finder_use_relative_alt) +float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos) { - float rel_alt_estimated = current_alt - land_setpoint_alt; - - /* only use range finder if: - * parameter (range_finder_use_relative_alt) > 0 - * the measurement is valid - * the estimated relative altitude (from global altitude estimate and landing waypoint) <= range_finder_use_relative_alt - */ - if (range_finder_use_relative_alt < 0 || !range_finder.valid || range_finder.distance > range_finder_use_relative_alt ) { - return rel_alt_estimated; + if (!isfinite(global_pos.terrain_alt)) { + return land_setpoint_alt; } - return range_finder.distance; - + /* Decide if the terrain estimation can be used, once we switched to using the terrain we stick with it + * for the whole landing */ + if (_parameters.land_use_terrain_estimate && (global_pos.terrain_alt_valid || land_useterrain)) { + if(!land_useterrain) { + mavlink_log_info(_mavlink_fd, "#audio: Landing, using terrain estimate"); + land_useterrain = true; + } + return global_pos.terrain_alt; + } else { + return land_setpoint_alt; + } } bool @@ -965,10 +950,17 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { float bearing_lastwp_currwp = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); + float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); /* Horizontal landing control */ /* switch to heading hold for the last meters, continue heading hold after */ float wp_distance = get_distance_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); + /* calculate a waypoint distance value which is 0 when the aircraft is behind the waypoint */ + float wp_distance_save = wp_distance; + if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) >= math::radians(90.0f)) { + wp_distance_save = 0.0f; + } + //warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO"); if (wp_distance < _parameters.land_heading_hold_horizontal_distance || land_noreturn_horizontal) { @@ -1004,29 +996,30 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* Vertical landing control */ //xxx: using the tecs altitude controller for slope control for now - -// /* do not go down too early */ -// if (wp_distance > 50.0f) { -// altitude_error = (_global_triplet.current.alt + 25.0f) - _global_pos.alt; -// } /* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */ // XXX this could make a great param - 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; + /* Get an estimate of the terrain altitude if available, otherwise terrain_alt will be + * equal to _pos_sp_triplet.current.alt */ + float terrain_alt = get_terrain_altitude_landing(_pos_sp_triplet.current.alt, _global_pos); + /* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */ - float L_altitude_rel = _pos_sp_triplet.previous.valid ? _pos_sp_triplet.previous.alt - _pos_sp_triplet.current.alt : 0.0f; + float L_altitude_rel = _pos_sp_triplet.previous.valid ? + _pos_sp_triplet.previous.alt - terrain_alt : 0.0f; - float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); float landing_slope_alt_rel_desired = landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp); - float relative_alt = get_relative_landingalt(_pos_sp_triplet.current.alt, _global_pos.alt, _range_finder, _parameters.range_finder_rel_alt); - - if ( (relative_alt < landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out - + /* Check if we should start flaring with a vertical and a + * horizontal limit (with some tolerance) + * The horizontal limit is only applied when we are in front of the wp + */ + if (((_global_pos.alt < terrain_alt + landingslope.flare_relative_alt()) && + (wp_distance_save < landingslope.flare_length() + 5.0f)) || + land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out /* land with minimal speed */ // /* force TECS to only control speed with pitch, altitude is only implicitely controlled now */ @@ -1035,7 +1028,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* kill the throttle if param requests it */ throttle_max = _parameters.throttle_max; - if (relative_alt < landingslope.motor_lim_relative_alt() || land_motor_lim) { + if (_global_pos.alt < terrain_alt + landingslope.motor_lim_relative_alt() || land_motor_lim) { throttle_max = math::min(throttle_max, _parameters.throttle_land_max); if (!land_motor_lim) { land_motor_lim = true; @@ -1053,12 +1046,13 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi land_stayonground = true; } - tecs_update_pitch_throttle(_pos_sp_triplet.current.alt + flare_curve_alt_rel, + tecs_update_pitch_throttle(terrain_alt + flare_curve_alt_rel, calculate_target_airspeed(airspeed_land), eas2tas, - flare_pitch_angle_rad, math::radians(15.0f), + math::radians(_parameters.land_flare_pitch_min_deg), + math::radians(_parameters.land_flare_pitch_max_deg), 0.0f, throttle_max, throttle_land, - false, flare_pitch_angle_rad, - _pos_sp_triplet.current.alt + relative_alt, ground_speed, + false, land_motor_lim ? math::radians(_parameters.land_flare_pitch_min_deg) : math::radians(_parameters.pitch_limit_min), + _global_pos.alt, ground_speed, land_motor_lim ? TECS_MODE_LAND_THROTTLELIM : TECS_MODE_LAND); if (!land_noreturn_vertical) { @@ -1079,8 +1073,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi * if current position is below the slope continue at previous wp altitude * until the intersection with slope * */ - float altitude_desired_rel = relative_alt; - if (relative_alt > landing_slope_alt_rel_desired || land_onslope) { + float altitude_desired_rel; + if (_global_pos.alt > terrain_alt + landing_slope_alt_rel_desired || land_onslope) { /* stay on slope */ altitude_desired_rel = landing_slope_alt_rel_desired; if (!land_onslope) { @@ -1089,10 +1083,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } } else { /* continue horizontally */ - altitude_desired_rel = _pos_sp_triplet.previous.valid ? L_altitude_rel : relative_alt; + altitude_desired_rel = _pos_sp_triplet.previous.valid ? L_altitude_rel : + _global_pos.alt - terrain_alt; } - tecs_update_pitch_throttle(_pos_sp_triplet.current.alt + altitude_desired_rel, + tecs_update_pitch_throttle(terrain_alt + altitude_desired_rel, calculate_target_airspeed(airspeed_approach), eas2tas, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), @@ -1101,7 +1096,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _parameters.throttle_cruise, false, math::radians(_parameters.pitch_limit_min), - _pos_sp_triplet.current.alt + relative_alt, + _global_pos.alt, ground_speed); } @@ -1235,8 +1230,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.thrust = 0.0f; } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF && launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { - /* Copy thrust and pitch values from tecs - * making sure again that the correct thrust is used, + /* making sure again that the correct thrust is used, * without depending on library calls for safety reasons */ _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); } else { @@ -1278,7 +1272,6 @@ FixedwingPositionControl::task_main() _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - _range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder)); /* rate limit control mode updates to 5Hz */ orb_set_interval(_control_mode_sub, 200); @@ -1357,7 +1350,6 @@ FixedwingPositionControl::task_main() vehicle_setpoint_poll(); vehicle_sensor_combined_poll(); vehicle_airspeed_poll(); - range_finder_poll(); // vehicle_baro_poll(); math::Vector<3> ground_speed(_global_pos.vel_n, _global_pos.vel_e, _global_pos.vel_d); @@ -1421,6 +1413,7 @@ void FixedwingPositionControl::reset_landing_state() land_stayonground = false; land_motor_lim = false; land_onslope = false; + land_useterrain = false; } void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_sp, float eas2tas, 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 847ecbb5c..c00d82232 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -412,12 +412,28 @@ PARAM_DEFINE_FLOAT(FW_LND_TLALT, -1.0f); PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f); /** - * Relative altitude threshold for range finder measurements for use during landing + * Enable or disable usage of terrain estimate during landing * - * range finder measurements will only be used if the estimated relative altitude (gobal_pos.alt - landing_waypoint.alt) is < FW_LND_RFRALT - * set to < 0 to disable - * the correct value of this parameter depends on your range measuring device as well as on the terrain at the landing location + * 0: disabled, 1: enabled * * @group L1 Control */ -PARAM_DEFINE_FLOAT(FW_LND_RFRALT, -1.0f); +PARAM_DEFINE_INT32(FW_LND_USETER, 0); + +/** + * Flare, minimum pitch + * + * Minimum pitch during flare, a positive sign means nose up + * Applied once FW_LND_TLALT is reached + * + */ +PARAM_DEFINE_FLOAT(FW_FLARE_PMIN, 2.5f); + +/** + * Flare, maximum pitch + * + * Maximum pitch during flare, a positive sign means nose up + * Applied once FW_LND_TLALT is reached + * + */ +PARAM_DEFINE_FLOAT(FW_FLARE_PMAX, 15.0f); diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 4155d6bf4..0d932d20a 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1403,7 +1403,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f); configure_stream("ATTITUDE_TARGET", 3.0f); configure_stream("DISTANCE_SENSOR", 0.5f); - configure_stream("OPTICAL_FLOW", 20.0f); + configure_stream("OPTICAL_FLOW", 5.0f); break; case MAVLINK_MODE_ONBOARD: @@ -1411,6 +1411,9 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("ATTITUDE", 50.0f); configure_stream("GLOBAL_POSITION_INT", 50.0f); configure_stream("CAMERA_CAPTURE", 2.0f); + configure_stream("ATTITUDE_TARGET", 10.0f); + configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f); + configure_stream("VFR_HUD", 10.0f); break; default: diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index c6ec96dcb..a867dd0da 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -383,11 +383,9 @@ Navigator::task_main() /* global position updated */ if (fds[0].revents & POLLIN) { global_position_update(); - static int gposcounter = 0; if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) { have_geofence_position_data = true; } - gposcounter++; } /* Check geofence violation */ |