forked from Archive/PX4-Autopilot
Merge pull request #1372 from PX4/swissfang
UAV Outback challenge improvements from Team Swiss Fang
This commit is contained in:
Lorenz Meier
commit
d856356fce
|
|
@ -9,4 +9,3 @@ sh /etc/init.d/rc.fw_defaults
|
|||
|
||||
set MIXER Viper
|
||||
|
||||
set FAILSAFE "-c567 -p 1000"
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -13,3 +13,5 @@ ekf_att_pos_estimator start
|
|||
#
|
||||
fw_att_control start
|
||||
fw_pos_control_l1 start
|
||||
|
||||
bottle_drop start
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -128,6 +128,11 @@ MODULES += lib/geo_lookup
|
|||
MODULES += lib/conversion
|
||||
MODULES += lib/launchdetection
|
||||
|
||||
#
|
||||
# OBC challenge
|
||||
#
|
||||
MODULES += modules/bottle_drop
|
||||
|
||||
#
|
||||
# Demo apps
|
||||
#
|
||||
|
|
|
|||
|
|
@ -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 */
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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) {
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -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);
|
||||
|
|
@ -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
|
||||
|
|
@ -1230,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 {
|
||||
|
|
|
|||
|
|
@ -1396,7 +1396,7 @@ Mavlink::task_main(int argc, char *argv[])
|
|||
configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f);
|
||||
configure_stream("ATTITUDE_TARGET", 3.0f);
|
||||
configure_stream("DISTANCE_SENSOR", 0.5f);
|
||||
configure_stream("OPTICAL_FLOW", 20.0f);
|
||||
configure_stream("OPTICAL_FLOW", 5.0f);
|
||||
break;
|
||||
|
||||
case MAVLINK_MODE_ONBOARD:
|
||||
|
|
@ -1404,6 +1404,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:
|
||||
|
|
|
|||
|
|
@ -388,11 +388,9 @@ Navigator::task_main()
|
|||
/* global position updated */
|
||||
if (fds[0].revents & POLLIN) {
|
||||
global_position_update();
|
||||
static int gposcounter = 0;
|
||||
if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
|
||||
have_geofence_position_data = true;
|
||||
}
|
||||
gposcounter++;
|
||||
}
|
||||
|
||||
/* Check geofence violation */
|
||||
|
|
|
|||
Loading…
Reference in New Issue