forked from Archive/PX4-Autopilot
Merge branch 'master' of github.com:PX4/Firmware into sf0x_test
This commit is contained in:
commit
8d187cc2fa
|
@ -0,0 +1,41 @@
|
|||
The PX4 firmware is licensed generally under a permissive 3-clause BSD license. Contributions are required
|
||||
to be made under the same license. Any exception to this general rule is listed below.
|
||||
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
|
||||
- PX4 middleware: BSD 3-clause
|
||||
- PX4 flight control stack: BSD 3-clause
|
||||
- NuttX operating system: BSD 3-clause
|
||||
- Exceptions: Currently only this [400 LOC file](https://github.com/PX4/Firmware/blob/master/src/lib/external_lgpl/tecs/tecs.cpp) remains LGPL, but will be replaced with a BSD implementation.
|
|
@ -0,0 +1,10 @@
|
|||
## PX4 Aerial Middleware and Flight Control Stack ##
|
||||
|
||||
* Official Website: http://px4.io
|
||||
* License: BSD 3-clause (see LICENSE.md)
|
||||
* Supported airframes:
|
||||
* Multicopters
|
||||
* Fixed wing
|
||||
* Binaries (always up-to-date from master):
|
||||
* [Downloads](https://pixhawk.org/downloads)
|
||||
* Mailing list: [Google Groups](http://groups.google.com/group/px4users)
|
|
@ -9,5 +9,3 @@ sh /etc/init.d/rc.fw_defaults
|
|||
|
||||
set MIXER Viper
|
||||
|
||||
set FAILSAFE "-c56 -p 1000"
|
||||
set FAILSAFE "-c7 -p 1200"
|
||||
|
|
|
@ -75,3 +75,33 @@ if [ -f /fs/microsd/mount_test_cmds.txt ]
|
|||
then
|
||||
tests mount
|
||||
fi
|
||||
|
||||
#
|
||||
# Run unit tests at board boot, reporting failure as needed.
|
||||
# Add new unit tests using the same pattern as below.
|
||||
#
|
||||
|
||||
set unit_test_failure 0
|
||||
|
||||
if mavlink_tests
|
||||
then
|
||||
else
|
||||
set unit_test_failure 1
|
||||
set unit_test_failure_list "${unit_test_failure_list} mavlink_tests"
|
||||
fi
|
||||
|
||||
if commander_tests
|
||||
then
|
||||
else
|
||||
set unit_test_failure 1
|
||||
set unit_test_failure_list "${unit_test_failure_list} commander_tests"
|
||||
fi
|
||||
|
||||
if [ $unit_test_failure == 0 ]
|
||||
then
|
||||
echo
|
||||
echo "All Unit Tests PASSED"
|
||||
else
|
||||
echo
|
||||
echo "Some Unit Tests FAILED:${unit_test_failure_list}"
|
||||
fi
|
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
@ -24,25 +24,22 @@ MODULES += drivers/l3gd20
|
|||
MODULES += drivers/mpu6000
|
||||
MODULES += drivers/hmc5883
|
||||
MODULES += drivers/ms5611
|
||||
MODULES += drivers/mb12xx
|
||||
#MODULES += drivers/mb12xx
|
||||
MODULES += drivers/gps
|
||||
MODULES += drivers/hil
|
||||
MODULES += drivers/hott/hott_telemetry
|
||||
MODULES += drivers/hott/hott_sensors
|
||||
MODULES += drivers/blinkm
|
||||
#MODULES += drivers/blinkm
|
||||
MODULES += drivers/rgbled
|
||||
MODULES += drivers/mkblctrl
|
||||
MODULES += drivers/airspeed
|
||||
MODULES += drivers/ets_airspeed
|
||||
#MODULES += drivers/ets_airspeed
|
||||
MODULES += drivers/meas_airspeed
|
||||
MODULES += drivers/frsky_telemetry
|
||||
#MODULES += drivers/frsky_telemetry
|
||||
MODULES += modules/sensors
|
||||
|
||||
#
|
||||
# System commands
|
||||
#
|
||||
MODULES += systemcmds/mtd
|
||||
MODULES += systemcmds/bl_update
|
||||
MODULES += systemcmds/mixer
|
||||
MODULES += systemcmds/param
|
||||
MODULES += systemcmds/perf
|
||||
|
@ -67,17 +64,17 @@ MODULES += modules/gpio_led
|
|||
#
|
||||
# Estimation modules (EKF / other filters)
|
||||
#
|
||||
#MODULES += modules/attitude_estimator_ekf
|
||||
MODULES += modules/attitude_estimator_ekf
|
||||
MODULES += modules/ekf_att_pos_estimator
|
||||
#MODULES += modules/position_estimator_inav
|
||||
MODULES += modules/position_estimator_inav
|
||||
|
||||
#
|
||||
# Vehicle Control
|
||||
#
|
||||
MODULES += modules/fw_pos_control_l1
|
||||
MODULES += modules/fw_att_control
|
||||
#MODULES += modules/mc_att_control
|
||||
#MODULES += modules/mc_pos_control
|
||||
MODULES += modules/mc_att_control
|
||||
MODULES += modules/mc_pos_control
|
||||
|
||||
#
|
||||
# Logging
|
||||
|
|
|
@ -54,6 +54,10 @@ MODULES += lib/conversion
|
|||
#
|
||||
LIBRARIES += lib/mathlib/CMSIS
|
||||
|
||||
MODULES += modules/unit_test
|
||||
MODULES += modules/mavlink/mavlink_tests
|
||||
MODULES += modules/commander/commander_tests
|
||||
|
||||
#
|
||||
# Transitional support - add commands from the NuttX export archive.
|
||||
#
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -910,12 +910,14 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
|
|||
// adjust filters
|
||||
float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq();
|
||||
float sample_rate = 1.0e6f/ticks;
|
||||
_set_dlpf_filter(cutoff_freq_hz);
|
||||
_accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
|
||||
_accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
|
||||
_accel_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
|
||||
|
||||
|
||||
float cutoff_freq_hz_gyro = _gyro_filter_x.get_cutoff_freq();
|
||||
_set_dlpf_filter(cutoff_freq_hz_gyro);
|
||||
_gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro);
|
||||
_gyro_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro);
|
||||
_gyro_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro);
|
||||
|
@ -968,11 +970,9 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
|
|||
return _accel_filter_x.get_cutoff_freq();
|
||||
|
||||
case ACCELIOCSLOWPASS:
|
||||
if (arg == 0) {
|
||||
// allow disabling of on-chip filter using
|
||||
// zero as desired filter frequency
|
||||
_set_dlpf_filter(0);
|
||||
}
|
||||
// set hardware filtering
|
||||
_set_dlpf_filter(arg);
|
||||
// set software filtering
|
||||
_accel_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg);
|
||||
_accel_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg);
|
||||
_accel_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg);
|
||||
|
@ -1053,14 +1053,11 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
|
|||
case GYROIOCGLOWPASS:
|
||||
return _gyro_filter_x.get_cutoff_freq();
|
||||
case GYROIOCSLOWPASS:
|
||||
// set hardware filtering
|
||||
_set_dlpf_filter(arg);
|
||||
_gyro_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg);
|
||||
_gyro_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg);
|
||||
_gyro_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg);
|
||||
if (arg == 0) {
|
||||
// allow disabling of on-chip filter using 0
|
||||
// as desired frequency
|
||||
_set_dlpf_filter(0);
|
||||
}
|
||||
return OK;
|
||||
|
||||
case GYROIOCSSCALE:
|
||||
|
|
|
@ -295,6 +295,7 @@ private:
|
|||
float _battery_amp_bias; ///< current sensor bias
|
||||
float _battery_mamphour_total;///< amp hours consumed so far
|
||||
uint64_t _battery_last_timestamp;///< last amp hour calculation timestamp
|
||||
bool _cb_flighttermination; ///< true if the flight termination circuit breaker is enabled
|
||||
|
||||
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
|
||||
bool _dsm_vcc_ctl; ///< true if relay 1 controls DSM satellite RX power
|
||||
|
@ -515,7 +516,8 @@ PX4IO::PX4IO(device::Device *interface) :
|
|||
_battery_amp_per_volt(90.0f / 5.0f), // this matches the 3DR current sensor
|
||||
_battery_amp_bias(0),
|
||||
_battery_mamphour_total(0),
|
||||
_battery_last_timestamp(0)
|
||||
_battery_last_timestamp(0),
|
||||
_cb_flighttermination(true)
|
||||
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
|
||||
, _dsm_vcc_ctl(false)
|
||||
#endif
|
||||
|
@ -1051,6 +1053,9 @@ PX4IO::task_main()
|
|||
}
|
||||
}
|
||||
|
||||
/* Update Circuit breakers */
|
||||
_cb_flighttermination = circuit_breaker_enabled("CBRK_FLIGHTTERM", CBRK_FLIGHTTERM_KEY);
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
@ -1170,7 +1175,7 @@ PX4IO::io_set_arming_state()
|
|||
}
|
||||
|
||||
/* Do not set failsafe if circuit breaker is enabled */
|
||||
if (armed.force_failsafe && !circuit_breaker_enabled("CBRK_FLIGHTTERM", CBRK_FLIGHTTERM_KEY)) {
|
||||
if (armed.force_failsafe && !_cb_flighttermination) {
|
||||
set |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
|
||||
} else {
|
||||
clear |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
|
||||
|
|
|
@ -36,3 +36,5 @@
|
|||
#
|
||||
|
||||
SRCS = rotation.cpp
|
||||
|
||||
MAXOPTIMIZATION = -Os
|
||||
|
|
|
@ -238,6 +238,7 @@ void TECS::_update_height_demand(float demand, float state)
|
|||
_hgt_dem_adj = demand;//0.025f * demand + 0.975f * _hgt_dem_adj_last;
|
||||
_hgt_rate_dem = (_hgt_dem_adj-state)*_heightrate_p + _heightrate_ff * (_hgt_dem_adj - _hgt_dem_adj_last)/_DT;
|
||||
_hgt_dem_adj_last = _hgt_dem_adj;
|
||||
|
||||
// Limit height rate of change
|
||||
if (_hgt_rate_dem > _maxClimbRate) {
|
||||
_hgt_rate_dem = _maxClimbRate;
|
||||
|
|
|
@ -633,7 +633,29 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
|
|||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case VEHICLE_CMD_NAV_GUIDED_ENABLE: {
|
||||
transition_result_t res = TRANSITION_DENIED;
|
||||
static main_state_t main_state_pre_offboard = MAIN_STATE_MANUAL;
|
||||
if (status_local->main_state != MAIN_STATE_OFFBOARD) {
|
||||
main_state_pre_offboard = status_local->main_state;
|
||||
}
|
||||
if (cmd->param1 > 0.5f) {
|
||||
res = main_state_transition(status_local, MAIN_STATE_OFFBOARD);
|
||||
if (res == TRANSITION_DENIED) {
|
||||
print_reject_mode(status_local, "OFFBOARD");
|
||||
status_local->offboard_control_set_by_command = false;
|
||||
} else {
|
||||
/* Set flag that offboard was set via command, main state is not overridden by rc */
|
||||
status_local->offboard_control_set_by_command = true;
|
||||
}
|
||||
} else {
|
||||
/* If the mavlink command is used to enable or disable offboard control:
|
||||
* switch back to previous mode when disabling */
|
||||
res = main_state_transition(status_local, main_state_pre_offboard);
|
||||
status_local->offboard_control_set_by_command = false;
|
||||
}
|
||||
}
|
||||
break;
|
||||
case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
|
||||
case VEHICLE_CMD_PREFLIGHT_CALIBRATION:
|
||||
case VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS:
|
||||
|
@ -775,6 +797,8 @@ int commander_thread_main(int argc, char *argv[])
|
|||
// CIRCUIT BREAKERS
|
||||
status.circuit_breaker_engaged_power_check = false;
|
||||
status.circuit_breaker_engaged_airspd_check = false;
|
||||
status.circuit_breaker_engaged_enginefailure_check = false;
|
||||
status.circuit_breaker_engaged_gpsfailure_check = false;
|
||||
|
||||
/* publish initial state */
|
||||
status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
|
||||
|
@ -980,8 +1004,8 @@ int commander_thread_main(int argc, char *argv[])
|
|||
int32_t ef_throttle_thres = 1.0f;
|
||||
int32_t ef_current2throttle_thres = 0.0f;
|
||||
int32_t ef_time_thres = 1000.0f;
|
||||
uint64_t timestamp_engine_healthy = 0; /**< absolute time when engine
|
||||
was healty*/
|
||||
uint64_t timestamp_engine_healthy = 0; /**< absolute time when engine was healty */
|
||||
|
||||
/* check which state machines for changes, clear "changed" flag */
|
||||
bool arming_state_changed = false;
|
||||
bool main_state_changed = false;
|
||||
|
@ -1028,8 +1052,14 @@ int commander_thread_main(int argc, char *argv[])
|
|||
param_get(_param_system_id, &(status.system_id));
|
||||
param_get(_param_component_id, &(status.component_id));
|
||||
|
||||
status.circuit_breaker_engaged_power_check = circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY);
|
||||
status.circuit_breaker_engaged_airspd_check = circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY);
|
||||
status.circuit_breaker_engaged_power_check =
|
||||
circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY);
|
||||
status.circuit_breaker_engaged_airspd_check =
|
||||
circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY);
|
||||
status.circuit_breaker_engaged_enginefailure_check =
|
||||
circuit_breaker_enabled("CBRK_ENGINEFAIL", CBRK_ENGINEFAIL_KEY);
|
||||
status.circuit_breaker_engaged_gpsfailure_check =
|
||||
circuit_breaker_enabled("CBRK_GPSFAIL", CBRK_GPSFAIL_KEY);
|
||||
|
||||
status_changed = true;
|
||||
|
||||
|
@ -1205,24 +1235,6 @@ int commander_thread_main(int argc, char *argv[])
|
|||
|
||||
check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_good, &(status.condition_global_position_valid), &status_changed);
|
||||
|
||||
/* check if GPS fix is ok */
|
||||
if (gps_position.fix_type >= 3 && //XXX check eph and epv ?
|
||||
hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT) {
|
||||
/* handle the case where gps was regained */
|
||||
if (status.gps_failure) {
|
||||
status.gps_failure = false;
|
||||
status_changed = true;
|
||||
mavlink_log_critical(mavlink_fd, "gps regained");
|
||||
}
|
||||
} else {
|
||||
if (!status.gps_failure) {
|
||||
status.gps_failure = true;
|
||||
status_changed = true;
|
||||
mavlink_log_critical(mavlink_fd, "gps fix lost");
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/* update home position */
|
||||
if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed &&
|
||||
(global_position.eph < eph_threshold) && (global_position.epv < epv_threshold)) {
|
||||
|
@ -1434,6 +1446,24 @@ int commander_thread_main(int argc, char *argv[])
|
|||
globallocalconverter_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, (float)gps_position.alt * 1.0e-3f, hrt_absolute_time());
|
||||
}
|
||||
|
||||
/* check if GPS fix is ok */
|
||||
if (status.circuit_breaker_engaged_gpsfailure_check ||
|
||||
(gps_position.fix_type >= 3 &&
|
||||
hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT)) {
|
||||
/* handle the case where gps was regained */
|
||||
if (status.gps_failure) {
|
||||
status.gps_failure = false;
|
||||
status_changed = true;
|
||||
mavlink_log_critical(mavlink_fd, "gps regained");
|
||||
}
|
||||
} else {
|
||||
if (!status.gps_failure) {
|
||||
status.gps_failure = true;
|
||||
status_changed = true;
|
||||
mavlink_log_critical(mavlink_fd, "gps fix lost");
|
||||
}
|
||||
}
|
||||
|
||||
/* start mission result check */
|
||||
orb_check(mission_result_sub, &updated);
|
||||
|
||||
|
@ -1449,8 +1479,12 @@ int commander_thread_main(int argc, char *argv[])
|
|||
static bool flight_termination_printed = false;
|
||||
if (!flight_termination_printed) {
|
||||
warnx("Flight termination because of navigator request or geofence");
|
||||
mavlink_log_critical(mavlink_fd, "GF violation: flight termination");
|
||||
flight_termination_printed = true;
|
||||
}
|
||||
if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 ) {
|
||||
mavlink_log_critical(mavlink_fd, "GF violation: flight termination");
|
||||
}
|
||||
} // no reset is done here on purpose, on geofence violation we want to stay in flighttermination
|
||||
}
|
||||
|
||||
|
@ -1612,7 +1646,7 @@ int commander_thread_main(int argc, char *argv[])
|
|||
/* Check engine failure
|
||||
* only for fixed wing for now
|
||||
*/
|
||||
if (!circuit_breaker_enabled("CBRK_ENGINEFAIL", CBRK_ENGINEFAIL_KEY) &&
|
||||
if (!status.circuit_breaker_engaged_enginefailure_check &&
|
||||
status.is_rotary_wing == false &&
|
||||
armed.armed &&
|
||||
((actuator_controls.control[3] > ef_throttle_thres &&
|
||||
|
@ -1946,6 +1980,11 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
|
|||
/* set main state according to RC switches */
|
||||
transition_result_t res = TRANSITION_DENIED;
|
||||
|
||||
/* if offboard is set allready by a mavlink command, abort */
|
||||
if (status.offboard_control_set_by_command) {
|
||||
return main_state_transition(status_local, MAIN_STATE_OFFBOARD);
|
||||
}
|
||||
|
||||
/* offboard switch overrides main switch */
|
||||
if (sp_man->offboard_switch == SWITCH_POS_ON) {
|
||||
res = main_state_transition(status_local, MAIN_STATE_OFFBOARD);
|
||||
|
@ -2138,21 +2177,26 @@ set_control_mode()
|
|||
control_mode.flag_control_position_enabled = false;
|
||||
control_mode.flag_control_velocity_enabled = false;
|
||||
break;
|
||||
case OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY:
|
||||
case OFFBOARD_CONTROL_MODE_DIRECT_FORCE:
|
||||
control_mode.flag_control_rates_enabled = true;
|
||||
control_mode.flag_control_attitude_enabled = true;
|
||||
control_mode.flag_control_altitude_enabled = true; /* XXX: hack for now */
|
||||
control_mode.flag_control_climb_rate_enabled = true;
|
||||
control_mode.flag_control_position_enabled = true; /* XXX: hack for now */
|
||||
control_mode.flag_control_velocity_enabled = true;
|
||||
control_mode.flag_control_attitude_enabled = false;
|
||||
control_mode.flag_control_force_enabled = true;
|
||||
control_mode.flag_control_altitude_enabled = false;
|
||||
control_mode.flag_control_climb_rate_enabled = false;
|
||||
control_mode.flag_control_position_enabled = false;
|
||||
control_mode.flag_control_velocity_enabled = false;
|
||||
break;
|
||||
case OFFBOARD_CONTROL_MODE_DIRECT_POSITION:
|
||||
case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED:
|
||||
case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED:
|
||||
case OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED:
|
||||
case OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED:
|
||||
control_mode.flag_control_rates_enabled = true;
|
||||
control_mode.flag_control_attitude_enabled = true;
|
||||
control_mode.flag_control_altitude_enabled = true;
|
||||
control_mode.flag_control_climb_rate_enabled = true;
|
||||
control_mode.flag_control_position_enabled = true;
|
||||
control_mode.flag_control_velocity_enabled = true;
|
||||
//XXX: the flags could depend on sp_offboard.ignore
|
||||
break;
|
||||
default:
|
||||
control_mode.flag_control_rates_enabled = false;
|
||||
|
|
|
@ -159,7 +159,7 @@ PARAM_DEFINE_FLOAT(COM_EF_C2T, 5.0f);
|
|||
* @min 0.0f
|
||||
* @max 7.0f
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(COM_EF_TIME, 5.0f);
|
||||
PARAM_DEFINE_FLOAT(COM_EF_TIME, 10.0f);
|
||||
|
||||
/** RC loss time threshold
|
||||
*
|
||||
|
|
|
@ -48,7 +48,5 @@ extern "C" __EXPORT int commander_tests_main(int argc, char *argv[]);
|
|||
|
||||
int commander_tests_main(int argc, char *argv[])
|
||||
{
|
||||
stateMachineHelperTest();
|
||||
|
||||
return 0;
|
||||
return stateMachineHelperTest() ? 0 : -1;
|
||||
}
|
||||
|
|
|
@ -49,7 +49,7 @@ public:
|
|||
StateMachineHelperTest();
|
||||
virtual ~StateMachineHelperTest();
|
||||
|
||||
virtual void runTests(void);
|
||||
virtual bool run_tests(void);
|
||||
|
||||
private:
|
||||
bool armingStateTransitionTest();
|
||||
|
@ -488,16 +488,13 @@ bool StateMachineHelperTest::isSafeTest(void)
|
|||
return true;
|
||||
}
|
||||
|
||||
void StateMachineHelperTest::runTests(void)
|
||||
bool StateMachineHelperTest::run_tests(void)
|
||||
{
|
||||
ut_run_test(armingStateTransitionTest);
|
||||
ut_run_test(mainStateTransitionTest);
|
||||
ut_run_test(isSafeTest);
|
||||
|
||||
return (_tests_failed == 0);
|
||||
}
|
||||
|
||||
void stateMachineHelperTest(void)
|
||||
{
|
||||
StateMachineHelperTest* test = new StateMachineHelperTest();
|
||||
test->runTests();
|
||||
test->printResults();
|
||||
}
|
||||
ut_declare_test(stateMachineHelperTest, StateMachineHelperTest)
|
|
@ -39,6 +39,6 @@
|
|||
#ifndef STATE_MACHINE_HELPER_TEST_H_
|
||||
#define STATE_MACHINE_HELPER_TEST_
|
||||
|
||||
void stateMachineHelperTest(void);
|
||||
bool stateMachineHelperTest(void);
|
||||
|
||||
#endif /* STATE_MACHINE_HELPER_TEST_H_ */
|
||||
|
|
|
@ -797,7 +797,7 @@ start(void)
|
|||
sem_init(&g_init_sema, 1, 0);
|
||||
|
||||
/* start the worker thread */
|
||||
if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2000, task_main, NULL)) <= 0) {
|
||||
if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 2000, task_main, NULL)) <= 0) {
|
||||
warn("task start failed");
|
||||
return -1;
|
||||
}
|
||||
|
|
|
@ -40,3 +40,5 @@ MODULE_COMMAND = dataman
|
|||
SRCS = dataman.c
|
||||
|
||||
MODULE_STACKSIZE = 1200
|
||||
|
||||
MAXOPTIMIZATION = -Os
|
||||
|
|
|
@ -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();
|
||||
}
|
||||
|
||||
|
|
|
@ -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 */
|
||||
|
||||
|
@ -320,12 +321,6 @@ private:
|
|||
*/
|
||||
bool vehicle_airspeed_poll();
|
||||
|
||||
/**
|
||||
* Check for range finder updates.
|
||||
*/
|
||||
bool range_finder_poll();
|
||||
|
||||
|
||||
/**
|
||||
* Check for position updates.
|
||||
*/
|
||||
|
@ -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;
|
||||
|
||||
/* 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;
|
||||
/* 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 - 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);
|
||||
}
|
||||
|
||||
|
@ -1277,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);
|
||||
|
@ -1356,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);
|
||||
|
@ -1420,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,
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -68,6 +68,13 @@ PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_FIXED_WING);
|
|||
* @group MAVLink
|
||||
*/
|
||||
PARAM_DEFINE_INT32(MAV_USEHILGPS, 0);
|
||||
/**
|
||||
* Forward external setpoint messages
|
||||
* If set to 1 incomming external setpoint messages will be directly forwarded to the controllers if in offboard
|
||||
* control mode
|
||||
* @group MAVLink
|
||||
*/
|
||||
PARAM_DEFINE_INT32(MAV_FWDEXTSP, 1);
|
||||
|
||||
mavlink_system_t mavlink_system = {
|
||||
100,
|
||||
|
|
|
@ -84,7 +84,7 @@ void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int leng
|
|||
extern mavlink_status_t *mavlink_get_channel_status(uint8_t chan);
|
||||
extern mavlink_message_t *mavlink_get_channel_buffer(uint8_t chan);
|
||||
|
||||
#include <v1.0/pixhawk/mavlink.h>
|
||||
#include <v1.0/common/mavlink.h>
|
||||
|
||||
__END_DECLS
|
||||
|
||||
|
|
|
@ -31,38 +31,39 @@
|
|||
*
|
||||
****************************************************************************/
|
||||
|
||||
/// @file mavlink_ftp.cpp
|
||||
/// @author px4dev, Don Gagne <don@thegagnes.com>
|
||||
|
||||
#include <crc32.h>
|
||||
#include <unistd.h>
|
||||
#include <stdio.h>
|
||||
#include <fcntl.h>
|
||||
#include <sys/stat.h>
|
||||
#include <errno.h>
|
||||
|
||||
#include "mavlink_ftp.h"
|
||||
#include "mavlink_tests/mavlink_ftp_test.h"
|
||||
|
||||
// Uncomment the line below to get better debug output. Never commit with this left on.
|
||||
//#define MAVLINK_FTP_DEBUG
|
||||
|
||||
MavlinkFTP *MavlinkFTP::_server;
|
||||
|
||||
MavlinkFTP *
|
||||
MavlinkFTP::getServer()
|
||||
MavlinkFTP::get_server(void)
|
||||
{
|
||||
// XXX this really cries out for some locking...
|
||||
if (_server == nullptr) {
|
||||
_server = new MavlinkFTP;
|
||||
}
|
||||
return _server;
|
||||
static MavlinkFTP server;
|
||||
return &server;
|
||||
}
|
||||
|
||||
MavlinkFTP::MavlinkFTP() :
|
||||
_session_fds{},
|
||||
_workBufs{},
|
||||
_workFree{},
|
||||
_lock{}
|
||||
_request_bufs{},
|
||||
_request_queue{},
|
||||
_request_queue_sem{},
|
||||
_utRcvMsgFunc{},
|
||||
_ftp_test{}
|
||||
{
|
||||
// initialise the request freelist
|
||||
dq_init(&_workFree);
|
||||
sem_init(&_lock, 0, 1);
|
||||
dq_init(&_request_queue);
|
||||
sem_init(&_request_queue_sem, 0, 1);
|
||||
|
||||
// initialize session list
|
||||
for (size_t i=0; i<kMaxSession; i++) {
|
||||
|
@ -71,167 +72,258 @@ MavlinkFTP::MavlinkFTP() :
|
|||
|
||||
// drop work entries onto the free list
|
||||
for (unsigned i = 0; i < kRequestQueueSize; i++) {
|
||||
_qFree(&_workBufs[i]);
|
||||
_return_request(&_request_bufs[i]);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#ifdef MAVLINK_FTP_UNIT_TEST
|
||||
void
|
||||
MavlinkFTP::set_unittest_worker(ReceiveMessageFunc_t rcvMsgFunc, MavlinkFtpTest *ftp_test)
|
||||
{
|
||||
_utRcvMsgFunc = rcvMsgFunc;
|
||||
_ftp_test = ftp_test;
|
||||
}
|
||||
#endif
|
||||
|
||||
void
|
||||
MavlinkFTP::handle_message(Mavlink* mavlink, mavlink_message_t *msg)
|
||||
{
|
||||
// get a free request
|
||||
auto req = _dqFree();
|
||||
struct Request* req = _get_request();
|
||||
|
||||
// if we couldn't get a request slot, just drop it
|
||||
if (req != nullptr) {
|
||||
if (req == nullptr) {
|
||||
warnx("Dropping FTP request: queue full\n");
|
||||
return;
|
||||
}
|
||||
|
||||
// decode the request
|
||||
if (req->decode(mavlink, msg)) {
|
||||
|
||||
// and queue it for the worker
|
||||
work_queue(LPWORK, &req->work, &MavlinkFTP::_workerTrampoline, req, 0);
|
||||
} else {
|
||||
_qFree(req);
|
||||
if (msg->msgid == MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) {
|
||||
mavlink_msg_file_transfer_protocol_decode(msg, &req->message);
|
||||
|
||||
#ifdef MAVLINK_FTP_UNIT_TEST
|
||||
if (!_utRcvMsgFunc) {
|
||||
warnx("Incorrectly written unit test\n");
|
||||
return;
|
||||
}
|
||||
// We use fake ids when unit testing
|
||||
req->serverSystemId = MavlinkFtpTest::serverSystemId;
|
||||
req->serverComponentId = MavlinkFtpTest::serverComponentId;
|
||||
req->serverChannel = MavlinkFtpTest::serverChannel;
|
||||
#else
|
||||
// Not unit testing, use the real thing
|
||||
req->serverSystemId = mavlink->get_system_id();
|
||||
req->serverComponentId = mavlink->get_component_id();
|
||||
req->serverChannel = mavlink->get_channel();
|
||||
#endif
|
||||
|
||||
// This is the system id we want to target when sending
|
||||
req->targetSystemId = msg->sysid;
|
||||
|
||||
if (req->message.target_system == req->serverSystemId) {
|
||||
req->mavlink = mavlink;
|
||||
#ifdef MAVLINK_FTP_UNIT_TEST
|
||||
// We are running in Unit Test mode. Don't queue, just call _worket directly.
|
||||
_process_request(req);
|
||||
#else
|
||||
// We are running in normal mode. Queue the request to the worker
|
||||
work_queue(LPWORK, &req->work, &MavlinkFTP::_worker_trampoline, req, 0);
|
||||
#endif
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
_return_request(req);
|
||||
}
|
||||
|
||||
/// @brief Queued static work queue routine to handle mavlink messages
|
||||
void
|
||||
MavlinkFTP::_workerTrampoline(void *arg)
|
||||
MavlinkFTP::_worker_trampoline(void *arg)
|
||||
{
|
||||
auto req = reinterpret_cast<Request *>(arg);
|
||||
auto server = MavlinkFTP::getServer();
|
||||
Request* req = reinterpret_cast<Request *>(arg);
|
||||
MavlinkFTP* server = MavlinkFTP::get_server();
|
||||
|
||||
// call the server worker with the work item
|
||||
server->_worker(req);
|
||||
server->_process_request(req);
|
||||
}
|
||||
|
||||
/// @brief Processes an FTP message
|
||||
void
|
||||
MavlinkFTP::_worker(Request *req)
|
||||
MavlinkFTP::_process_request(Request *req)
|
||||
{
|
||||
auto hdr = req->header();
|
||||
PayloadHeader *payload = reinterpret_cast<PayloadHeader *>(&req->message.payload[0]);
|
||||
|
||||
ErrorCode errorCode = kErrNone;
|
||||
uint32_t messageCRC;
|
||||
|
||||
// basic sanity checks; must validate length before use
|
||||
if (hdr->size > kMaxDataLength) {
|
||||
errorCode = kErrNoRequest;
|
||||
if (payload->size > kMaxDataLength) {
|
||||
errorCode = kErrInvalidDataSize;
|
||||
goto out;
|
||||
}
|
||||
|
||||
// check request CRC to make sure this is one of ours
|
||||
messageCRC = hdr->crc32;
|
||||
hdr->crc32 = 0;
|
||||
hdr->padding[0] = 0;
|
||||
hdr->padding[1] = 0;
|
||||
hdr->padding[2] = 0;
|
||||
if (crc32(req->rawData(), req->dataSize()) != messageCRC) {
|
||||
errorCode = kErrNoRequest;
|
||||
goto out;
|
||||
warnx("ftp: bad crc");
|
||||
}
|
||||
|
||||
#ifdef MAVLINK_FTP_DEBUG
|
||||
printf("ftp: channel %u opc %u size %u offset %u\n", req->channel(), hdr->opcode, hdr->size, hdr->offset);
|
||||
printf("ftp: channel %u opc %u size %u offset %u\n", req->serverChannel, payload->opcode, payload->size, payload->offset);
|
||||
#endif
|
||||
|
||||
switch (hdr->opcode) {
|
||||
switch (payload->opcode) {
|
||||
case kCmdNone:
|
||||
break;
|
||||
|
||||
case kCmdTerminate:
|
||||
errorCode = _workTerminate(req);
|
||||
case kCmdTerminateSession:
|
||||
errorCode = _workTerminate(payload);
|
||||
break;
|
||||
|
||||
case kCmdReset:
|
||||
errorCode = _workReset();
|
||||
case kCmdResetSessions:
|
||||
errorCode = _workReset(payload);
|
||||
break;
|
||||
|
||||
case kCmdList:
|
||||
errorCode = _workList(req);
|
||||
case kCmdListDirectory:
|
||||
errorCode = _workList(payload);
|
||||
break;
|
||||
|
||||
case kCmdOpen:
|
||||
errorCode = _workOpen(req, false);
|
||||
case kCmdOpenFileRO:
|
||||
errorCode = _workOpen(payload, O_RDONLY);
|
||||
break;
|
||||
|
||||
case kCmdCreate:
|
||||
errorCode = _workOpen(req, true);
|
||||
case kCmdCreateFile:
|
||||
errorCode = _workOpen(payload, O_CREAT | O_EXCL | O_WRONLY);
|
||||
break;
|
||||
|
||||
case kCmdRead:
|
||||
errorCode = _workRead(req);
|
||||
case kCmdOpenFileWO:
|
||||
errorCode = _workOpen(payload, O_CREAT | O_WRONLY);
|
||||
break;
|
||||
|
||||
case kCmdWrite:
|
||||
errorCode = _workWrite(req);
|
||||
case kCmdReadFile:
|
||||
errorCode = _workRead(payload);
|
||||
break;
|
||||
|
||||
case kCmdRemove:
|
||||
errorCode = _workRemove(req);
|
||||
case kCmdWriteFile:
|
||||
errorCode = _workWrite(payload);
|
||||
break;
|
||||
|
||||
case kCmdRemoveFile:
|
||||
errorCode = _workRemoveFile(payload);
|
||||
break;
|
||||
|
||||
case kCmdRename:
|
||||
errorCode = _workRename(payload);
|
||||
break;
|
||||
|
||||
case kCmdTruncateFile:
|
||||
errorCode = _workTruncateFile(payload);
|
||||
break;
|
||||
|
||||
case kCmdCreateDirectory:
|
||||
errorCode = _workCreateDirectory(payload);
|
||||
break;
|
||||
|
||||
case kCmdRemoveDirectory:
|
||||
errorCode = _workRemoveDirectory(payload);
|
||||
break;
|
||||
|
||||
|
||||
case kCmdCalcFileCRC32:
|
||||
errorCode = _workCalcFileCRC32(payload);
|
||||
break;
|
||||
|
||||
default:
|
||||
errorCode = kErrNoRequest;
|
||||
errorCode = kErrUnknownCommand;
|
||||
break;
|
||||
}
|
||||
|
||||
out:
|
||||
// handle success vs. error
|
||||
if (errorCode == kErrNone) {
|
||||
hdr->opcode = kRspAck;
|
||||
payload->req_opcode = payload->opcode;
|
||||
payload->opcode = kRspAck;
|
||||
#ifdef MAVLINK_FTP_DEBUG
|
||||
warnx("FTP: ack\n");
|
||||
#endif
|
||||
} else {
|
||||
int r_errno = errno;
|
||||
warnx("FTP: nak %u", errorCode);
|
||||
hdr->opcode = kRspNak;
|
||||
hdr->size = 1;
|
||||
hdr->data[0] = errorCode;
|
||||
payload->req_opcode = payload->opcode;
|
||||
payload->opcode = kRspNak;
|
||||
payload->size = 1;
|
||||
payload->data[0] = errorCode;
|
||||
if (errorCode == kErrFailErrno) {
|
||||
payload->size = 2;
|
||||
payload->data[1] = r_errno;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// respond to the request
|
||||
_reply(req);
|
||||
|
||||
// free the request buffer back to the freelist
|
||||
_qFree(req);
|
||||
_return_request(req);
|
||||
}
|
||||
|
||||
/// @brief Sends the specified FTP reponse message out through mavlink
|
||||
void
|
||||
MavlinkFTP::_reply(Request *req)
|
||||
{
|
||||
auto hdr = req->header();
|
||||
PayloadHeader *payload = reinterpret_cast<PayloadHeader *>(&req->message.payload[0]);
|
||||
|
||||
hdr->seqNumber = req->header()->seqNumber + 1;
|
||||
payload->seqNumber = payload->seqNumber + 1;
|
||||
|
||||
// message is assumed to be already constructed in the request buffer, so generate the CRC
|
||||
hdr->crc32 = 0;
|
||||
hdr->padding[0] = 0;
|
||||
hdr->padding[1] = 0;
|
||||
hdr->padding[2] = 0;
|
||||
hdr->crc32 = crc32(req->rawData(), req->dataSize());
|
||||
mavlink_message_t msg;
|
||||
msg.checksum = 0;
|
||||
#ifndef MAVLINK_FTP_UNIT_TEST
|
||||
uint16_t len =
|
||||
#endif
|
||||
mavlink_msg_file_transfer_protocol_pack_chan(req->serverSystemId, // Sender system id
|
||||
req->serverComponentId, // Sender component id
|
||||
req->serverChannel, // Channel to send on
|
||||
&msg, // Message to pack payload into
|
||||
0, // Target network
|
||||
req->targetSystemId, // Target system id
|
||||
0, // Target component id
|
||||
(const uint8_t*)payload); // Payload to pack into message
|
||||
|
||||
bool success = true;
|
||||
#ifdef MAVLINK_FTP_UNIT_TEST
|
||||
// Unit test hook is set, call that instead
|
||||
_utRcvMsgFunc(&msg, _ftp_test);
|
||||
#else
|
||||
Mavlink *mavlink = req->mavlink;
|
||||
|
||||
mavlink->lockMessageBufferMutex();
|
||||
success = mavlink->message_buffer_write(&msg, len);
|
||||
mavlink->unlockMessageBufferMutex();
|
||||
|
||||
#endif
|
||||
|
||||
// then pack and send the reply back to the request source
|
||||
req->reply();
|
||||
if (!success) {
|
||||
warnx("FTP TX ERR");
|
||||
}
|
||||
#ifdef MAVLINK_FTP_DEBUG
|
||||
else {
|
||||
warnx("wrote: sys: %d, comp: %d, chan: %d, checksum: %d",
|
||||
req->serverSystemId,
|
||||
req->serverComponentId,
|
||||
req->serverChannel,
|
||||
msg.checksum);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
/// @brief Responds to a List command
|
||||
MavlinkFTP::ErrorCode
|
||||
MavlinkFTP::_workList(Request *req)
|
||||
MavlinkFTP::_workList(PayloadHeader* payload)
|
||||
{
|
||||
auto hdr = req->header();
|
||||
|
||||
char dirPath[kMaxDataLength];
|
||||
strncpy(dirPath, req->dataAsCString(), kMaxDataLength);
|
||||
strncpy(dirPath, _data_as_cstring(payload), kMaxDataLength);
|
||||
|
||||
DIR *dp = opendir(dirPath);
|
||||
|
||||
if (dp == nullptr) {
|
||||
warnx("FTP: can't open path '%s'", dirPath);
|
||||
return kErrNotDir;
|
||||
return kErrFailErrno;
|
||||
}
|
||||
|
||||
#ifdef MAVLINK_FTP_DEBUG
|
||||
warnx("FTP: list %s offset %d", dirPath, hdr->offset);
|
||||
warnx("FTP: list %s offset %d", dirPath, payload->offset);
|
||||
#endif
|
||||
|
||||
ErrorCode errorCode = kErrNone;
|
||||
|
@ -239,19 +331,19 @@ MavlinkFTP::_workList(Request *req)
|
|||
unsigned offset = 0;
|
||||
|
||||
// move to the requested offset
|
||||
seekdir(dp, hdr->offset);
|
||||
seekdir(dp, payload->offset);
|
||||
|
||||
for (;;) {
|
||||
// read the directory entry
|
||||
if (readdir_r(dp, &entry, &result)) {
|
||||
warnx("FTP: list %s readdir_r failure\n", dirPath);
|
||||
errorCode = kErrIO;
|
||||
errorCode = kErrFailErrno;
|
||||
break;
|
||||
}
|
||||
|
||||
// no more entries?
|
||||
if (result == nullptr) {
|
||||
if (hdr->offset != 0 && offset == 0) {
|
||||
if (payload->offset != 0 && offset == 0) {
|
||||
// User is requesting subsequent dir entries but there were none. This means the user asked
|
||||
// to seek past EOF.
|
||||
errorCode = kErrEOF;
|
||||
|
@ -276,14 +368,22 @@ MavlinkFTP::_workList(Request *req)
|
|||
}
|
||||
break;
|
||||
case DTYPE_DIRECTORY:
|
||||
direntType = kDirentDir;
|
||||
if (strcmp(entry.d_name, ".") == 0 || strcmp(entry.d_name, "..") == 0) {
|
||||
// Don't bother sending these back
|
||||
direntType = kDirentSkip;
|
||||
} else {
|
||||
direntType = kDirentDir;
|
||||
}
|
||||
break;
|
||||
default:
|
||||
direntType = kDirentUnknown;
|
||||
break;
|
||||
// We only send back file and diretory entries, skip everything else
|
||||
direntType = kDirentSkip;
|
||||
}
|
||||
|
||||
if (entry.d_type == DTYPE_FILE) {
|
||||
if (direntType == kDirentSkip) {
|
||||
// Skip send only dirent identifier
|
||||
buf[0] = '\0';
|
||||
} else if (direntType == kDirentFile) {
|
||||
// Files send filename and file length
|
||||
snprintf(buf, sizeof(buf), "%s\t%d", entry.d_name, fileSize);
|
||||
} else {
|
||||
|
@ -299,145 +399,230 @@ MavlinkFTP::_workList(Request *req)
|
|||
}
|
||||
|
||||
// Move the data into the buffer
|
||||
hdr->data[offset++] = direntType;
|
||||
strcpy((char *)&hdr->data[offset], buf);
|
||||
payload->data[offset++] = direntType;
|
||||
strcpy((char *)&payload->data[offset], buf);
|
||||
#ifdef MAVLINK_FTP_DEBUG
|
||||
printf("FTP: list %s %s\n", dirPath, (char *)&hdr->data[offset-1]);
|
||||
printf("FTP: list %s %s\n", dirPath, (char *)&payload->data[offset-1]);
|
||||
#endif
|
||||
offset += nameLen + 1;
|
||||
}
|
||||
|
||||
closedir(dp);
|
||||
hdr->size = offset;
|
||||
payload->size = offset;
|
||||
|
||||
return errorCode;
|
||||
}
|
||||
|
||||
/// @brief Responds to an Open command
|
||||
MavlinkFTP::ErrorCode
|
||||
MavlinkFTP::_workOpen(Request *req, bool create)
|
||||
MavlinkFTP::_workOpen(PayloadHeader* payload, int oflag)
|
||||
{
|
||||
auto hdr = req->header();
|
||||
|
||||
int session_index = _findUnusedSession();
|
||||
int session_index = _find_unused_session();
|
||||
if (session_index < 0) {
|
||||
return kErrNoSession;
|
||||
warnx("FTP: Open failed - out of sessions\n");
|
||||
return kErrNoSessionsAvailable;
|
||||
}
|
||||
|
||||
|
||||
char *filename = _data_as_cstring(payload);
|
||||
|
||||
uint32_t fileSize = 0;
|
||||
if (!create) {
|
||||
struct stat st;
|
||||
if (stat(req->dataAsCString(), &st) != 0) {
|
||||
return kErrNotFile;
|
||||
}
|
||||
fileSize = st.st_size;
|
||||
struct stat st;
|
||||
if (stat(filename, &st) != 0) {
|
||||
// fail only if requested open for read
|
||||
if (oflag & O_RDONLY)
|
||||
return kErrFailErrno;
|
||||
else
|
||||
st.st_size = 0;
|
||||
}
|
||||
fileSize = st.st_size;
|
||||
|
||||
int oflag = create ? (O_CREAT | O_EXCL | O_APPEND) : O_RDONLY;
|
||||
|
||||
int fd = ::open(req->dataAsCString(), oflag);
|
||||
int fd = ::open(filename, oflag);
|
||||
if (fd < 0) {
|
||||
return create ? kErrPerm : kErrNotFile;
|
||||
return kErrFailErrno;
|
||||
}
|
||||
_session_fds[session_index] = fd;
|
||||
|
||||
hdr->session = session_index;
|
||||
if (create) {
|
||||
hdr->size = 0;
|
||||
} else {
|
||||
hdr->size = sizeof(uint32_t);
|
||||
*((uint32_t*)hdr->data) = fileSize;
|
||||
}
|
||||
payload->session = session_index;
|
||||
payload->size = sizeof(uint32_t);
|
||||
*((uint32_t*)payload->data) = fileSize;
|
||||
|
||||
return kErrNone;
|
||||
}
|
||||
|
||||
/// @brief Responds to a Read command
|
||||
MavlinkFTP::ErrorCode
|
||||
MavlinkFTP::_workRead(Request *req)
|
||||
MavlinkFTP::_workRead(PayloadHeader* payload)
|
||||
{
|
||||
auto hdr = req->header();
|
||||
int session_index = payload->session;
|
||||
|
||||
int session_index = hdr->session;
|
||||
|
||||
if (!_validSession(session_index)) {
|
||||
return kErrNoSession;
|
||||
if (!_valid_session(session_index)) {
|
||||
return kErrInvalidSession;
|
||||
}
|
||||
|
||||
// Seek to the specified position
|
||||
#ifdef MAVLINK_FTP_DEBUG
|
||||
warnx("seek %d", hdr->offset);
|
||||
warnx("seek %d", payload->offset);
|
||||
#endif
|
||||
if (lseek(_session_fds[session_index], hdr->offset, SEEK_SET) < 0) {
|
||||
if (lseek(_session_fds[session_index], payload->offset, SEEK_SET) < 0) {
|
||||
// Unable to see to the specified location
|
||||
warnx("seek fail");
|
||||
return kErrEOF;
|
||||
}
|
||||
|
||||
int bytes_read = ::read(_session_fds[session_index], &hdr->data[0], kMaxDataLength);
|
||||
int bytes_read = ::read(_session_fds[session_index], &payload->data[0], kMaxDataLength);
|
||||
if (bytes_read < 0) {
|
||||
// Negative return indicates error other than eof
|
||||
warnx("read fail %d", bytes_read);
|
||||
return kErrIO;
|
||||
return kErrFailErrno;
|
||||
}
|
||||
|
||||
hdr->size = bytes_read;
|
||||
payload->size = bytes_read;
|
||||
|
||||
return kErrNone;
|
||||
}
|
||||
|
||||
/// @brief Responds to a Write command
|
||||
MavlinkFTP::ErrorCode
|
||||
MavlinkFTP::_workWrite(Request *req)
|
||||
MavlinkFTP::_workWrite(PayloadHeader* payload)
|
||||
{
|
||||
#if 0
|
||||
// NYI: Coming soon
|
||||
auto hdr = req->header();
|
||||
int session_index = payload->session;
|
||||
|
||||
// look up session
|
||||
auto session = getSession(hdr->session);
|
||||
if (session == nullptr) {
|
||||
return kErrNoSession;
|
||||
if (!_valid_session(session_index)) {
|
||||
return kErrInvalidSession;
|
||||
}
|
||||
|
||||
// append to file
|
||||
int result = session->append(hdr->offset, &hdr->data[0], hdr->size);
|
||||
|
||||
if (result < 0) {
|
||||
// XXX might also be no space, I/O, etc.
|
||||
return kErrNotAppend;
|
||||
}
|
||||
|
||||
hdr->size = result;
|
||||
return kErrNone;
|
||||
#else
|
||||
return kErrPerm;
|
||||
// Seek to the specified position
|
||||
#ifdef MAVLINK_FTP_DEBUG
|
||||
warnx("seek %d", payload->offset);
|
||||
#endif
|
||||
}
|
||||
|
||||
MavlinkFTP::ErrorCode
|
||||
MavlinkFTP::_workRemove(Request *req)
|
||||
{
|
||||
//auto hdr = req->header();
|
||||
|
||||
// for now, send error reply
|
||||
return kErrPerm;
|
||||
}
|
||||
|
||||
MavlinkFTP::ErrorCode
|
||||
MavlinkFTP::_workTerminate(Request *req)
|
||||
{
|
||||
auto hdr = req->header();
|
||||
|
||||
if (!_validSession(hdr->session)) {
|
||||
return kErrNoSession;
|
||||
if (lseek(_session_fds[session_index], payload->offset, SEEK_SET) < 0) {
|
||||
// Unable to see to the specified location
|
||||
warnx("seek fail");
|
||||
return kErrFailErrno;
|
||||
}
|
||||
|
||||
::close(_session_fds[hdr->session]);
|
||||
|
||||
int bytes_written = ::write(_session_fds[session_index], &payload->data[0], payload->size);
|
||||
if (bytes_written < 0) {
|
||||
// Negative return indicates error other than eof
|
||||
warnx("write fail %d", bytes_written);
|
||||
return kErrFailErrno;
|
||||
}
|
||||
|
||||
payload->size = sizeof(uint32_t);
|
||||
*((uint32_t*)payload->data) = bytes_written;
|
||||
|
||||
return kErrNone;
|
||||
}
|
||||
|
||||
/// @brief Responds to a RemoveFile command
|
||||
MavlinkFTP::ErrorCode
|
||||
MavlinkFTP::_workReset(void)
|
||||
MavlinkFTP::_workRemoveFile(PayloadHeader* payload)
|
||||
{
|
||||
char file[kMaxDataLength];
|
||||
strncpy(file, _data_as_cstring(payload), kMaxDataLength);
|
||||
|
||||
if (unlink(file) == 0) {
|
||||
payload->size = 0;
|
||||
return kErrNone;
|
||||
} else {
|
||||
return kErrFailErrno;
|
||||
}
|
||||
}
|
||||
|
||||
/// @brief Responds to a TruncateFile command
|
||||
MavlinkFTP::ErrorCode
|
||||
MavlinkFTP::_workTruncateFile(PayloadHeader* payload)
|
||||
{
|
||||
char file[kMaxDataLength];
|
||||
const char temp_file[] = "/fs/microsd/.trunc.tmp";
|
||||
strncpy(file, _data_as_cstring(payload), kMaxDataLength);
|
||||
payload->size = 0;
|
||||
|
||||
// emulate truncate(file, payload->offset) by
|
||||
// copying to temp and overwrite with O_TRUNC flag.
|
||||
|
||||
struct stat st;
|
||||
if (stat(file, &st) != 0) {
|
||||
return kErrFailErrno;
|
||||
}
|
||||
|
||||
if (!S_ISREG(st.st_mode)) {
|
||||
errno = EISDIR;
|
||||
return kErrFailErrno;
|
||||
}
|
||||
|
||||
// check perms allow us to write (not romfs)
|
||||
if (!(st.st_mode & (S_IWUSR | S_IWGRP | S_IWOTH))) {
|
||||
errno = EROFS;
|
||||
return kErrFailErrno;
|
||||
}
|
||||
|
||||
if (payload->offset == (unsigned)st.st_size) {
|
||||
// nothing to do
|
||||
return kErrNone;
|
||||
}
|
||||
else if (payload->offset == 0) {
|
||||
// 1: truncate all data
|
||||
int fd = ::open(file, O_TRUNC | O_WRONLY);
|
||||
if (fd < 0) {
|
||||
return kErrFailErrno;
|
||||
}
|
||||
|
||||
::close(fd);
|
||||
return kErrNone;
|
||||
}
|
||||
else if (payload->offset > (unsigned)st.st_size) {
|
||||
// 2: extend file
|
||||
int fd = ::open(file, O_WRONLY);
|
||||
if (fd < 0) {
|
||||
return kErrFailErrno;
|
||||
}
|
||||
|
||||
if (lseek(fd, payload->offset - 1, SEEK_SET) < 0) {
|
||||
::close(fd);
|
||||
return kErrFailErrno;
|
||||
}
|
||||
|
||||
bool ok = 1 == ::write(fd, "", 1);
|
||||
::close(fd);
|
||||
|
||||
return (ok)? kErrNone : kErrFailErrno;
|
||||
}
|
||||
else {
|
||||
// 3: truncate
|
||||
if (_copy_file(file, temp_file, payload->offset) != 0) {
|
||||
return kErrFailErrno;
|
||||
}
|
||||
if (_copy_file(temp_file, file, payload->offset) != 0) {
|
||||
return kErrFailErrno;
|
||||
}
|
||||
if (::unlink(temp_file) != 0) {
|
||||
return kErrFailErrno;
|
||||
}
|
||||
|
||||
return kErrNone;
|
||||
}
|
||||
}
|
||||
|
||||
/// @brief Responds to a Terminate command
|
||||
MavlinkFTP::ErrorCode
|
||||
MavlinkFTP::_workTerminate(PayloadHeader* payload)
|
||||
{
|
||||
if (!_valid_session(payload->session)) {
|
||||
return kErrInvalidSession;
|
||||
}
|
||||
|
||||
::close(_session_fds[payload->session]);
|
||||
_session_fds[payload->session] = -1;
|
||||
|
||||
payload->size = 0;
|
||||
|
||||
return kErrNone;
|
||||
}
|
||||
|
||||
/// @brief Responds to a Reset command
|
||||
MavlinkFTP::ErrorCode
|
||||
MavlinkFTP::_workReset(PayloadHeader* payload)
|
||||
{
|
||||
for (size_t i=0; i<kMaxSession; i++) {
|
||||
if (_session_fds[i] != -1) {
|
||||
|
@ -446,11 +631,104 @@ MavlinkFTP::_workReset(void)
|
|||
}
|
||||
}
|
||||
|
||||
payload->size = 0;
|
||||
|
||||
return kErrNone;
|
||||
}
|
||||
|
||||
/// @brief Responds to a Rename command
|
||||
MavlinkFTP::ErrorCode
|
||||
MavlinkFTP::_workRename(PayloadHeader* payload)
|
||||
{
|
||||
char oldpath[kMaxDataLength];
|
||||
char newpath[kMaxDataLength];
|
||||
|
||||
char *ptr = _data_as_cstring(payload);
|
||||
size_t oldpath_sz = strlen(ptr);
|
||||
|
||||
if (oldpath_sz == payload->size) {
|
||||
// no newpath
|
||||
errno = EINVAL;
|
||||
return kErrFailErrno;
|
||||
}
|
||||
|
||||
strncpy(oldpath, ptr, kMaxDataLength);
|
||||
strncpy(newpath, ptr + oldpath_sz + 1, kMaxDataLength);
|
||||
|
||||
if (rename(oldpath, newpath) == 0) {
|
||||
payload->size = 0;
|
||||
return kErrNone;
|
||||
} else {
|
||||
return kErrFailErrno;
|
||||
}
|
||||
}
|
||||
|
||||
/// @brief Responds to a RemoveDirectory command
|
||||
MavlinkFTP::ErrorCode
|
||||
MavlinkFTP::_workRemoveDirectory(PayloadHeader* payload)
|
||||
{
|
||||
char dir[kMaxDataLength];
|
||||
strncpy(dir, _data_as_cstring(payload), kMaxDataLength);
|
||||
|
||||
if (rmdir(dir) == 0) {
|
||||
payload->size = 0;
|
||||
return kErrNone;
|
||||
} else {
|
||||
return kErrFailErrno;
|
||||
}
|
||||
}
|
||||
|
||||
/// @brief Responds to a CreateDirectory command
|
||||
MavlinkFTP::ErrorCode
|
||||
MavlinkFTP::_workCreateDirectory(PayloadHeader* payload)
|
||||
{
|
||||
char dir[kMaxDataLength];
|
||||
strncpy(dir, _data_as_cstring(payload), kMaxDataLength);
|
||||
|
||||
if (mkdir(dir, S_IRWXU | S_IRWXG | S_IRWXO) == 0) {
|
||||
payload->size = 0;
|
||||
return kErrNone;
|
||||
} else {
|
||||
return kErrFailErrno;
|
||||
}
|
||||
}
|
||||
|
||||
/// @brief Responds to a CalcFileCRC32 command
|
||||
MavlinkFTP::ErrorCode
|
||||
MavlinkFTP::_workCalcFileCRC32(PayloadHeader* payload)
|
||||
{
|
||||
char file_buf[256];
|
||||
uint32_t checksum = 0;
|
||||
ssize_t bytes_read;
|
||||
strncpy(file_buf, _data_as_cstring(payload), kMaxDataLength);
|
||||
|
||||
int fd = ::open(file_buf, O_RDONLY);
|
||||
if (fd < 0) {
|
||||
return kErrFailErrno;
|
||||
}
|
||||
|
||||
do {
|
||||
bytes_read = ::read(fd, file_buf, sizeof(file_buf));
|
||||
if (bytes_read < 0) {
|
||||
int r_errno = errno;
|
||||
::close(fd);
|
||||
errno = r_errno;
|
||||
return kErrFailErrno;
|
||||
}
|
||||
|
||||
checksum = crc32part((uint8_t*)file_buf, bytes_read, checksum);
|
||||
} while (bytes_read == sizeof(file_buf));
|
||||
|
||||
::close(fd);
|
||||
|
||||
payload->size = sizeof(uint32_t);
|
||||
*((uint32_t*)payload->data) = checksum;
|
||||
return kErrNone;
|
||||
}
|
||||
|
||||
/// @brief Returns true if the specified session is a valid open session
|
||||
bool
|
||||
MavlinkFTP::_validSession(unsigned index)
|
||||
MavlinkFTP::_valid_session(unsigned index)
|
||||
{
|
||||
if ((index >= kMaxSession) || (_session_fds[index] < 0)) {
|
||||
return false;
|
||||
|
@ -458,8 +736,9 @@ MavlinkFTP::_validSession(unsigned index)
|
|||
return true;
|
||||
}
|
||||
|
||||
/// @brief Returns an unused session index
|
||||
int
|
||||
MavlinkFTP::_findUnusedSession(void)
|
||||
MavlinkFTP::_find_unused_session(void)
|
||||
{
|
||||
for (size_t i=0; i<kMaxSession; i++) {
|
||||
if (_session_fds[i] == -1) {
|
||||
|
@ -470,16 +749,105 @@ MavlinkFTP::_findUnusedSession(void)
|
|||
return -1;
|
||||
}
|
||||
|
||||
/// @brief Guarantees that the payload data is null terminated.
|
||||
/// @return Returns a pointer to the payload data as a char *
|
||||
char *
|
||||
MavlinkFTP::Request::dataAsCString()
|
||||
MavlinkFTP::_data_as_cstring(PayloadHeader* payload)
|
||||
{
|
||||
// guarantee nul termination
|
||||
if (header()->size < kMaxDataLength) {
|
||||
requestData()[header()->size] = '\0';
|
||||
if (payload->size < kMaxDataLength) {
|
||||
payload->data[payload->size] = '\0';
|
||||
} else {
|
||||
requestData()[kMaxDataLength - 1] = '\0';
|
||||
payload->data[kMaxDataLength - 1] = '\0';
|
||||
}
|
||||
|
||||
// and return data
|
||||
return (char *)&(header()->data[0]);
|
||||
return (char *)&(payload->data[0]);
|
||||
}
|
||||
|
||||
/// @brief Returns a unused Request entry. NULL if none available.
|
||||
MavlinkFTP::Request *
|
||||
MavlinkFTP::_get_request(void)
|
||||
{
|
||||
_lock_request_queue();
|
||||
Request* req = reinterpret_cast<Request *>(dq_remfirst(&_request_queue));
|
||||
_unlock_request_queue();
|
||||
return req;
|
||||
}
|
||||
|
||||
/// @brief Locks a semaphore to provide exclusive access to the request queue
|
||||
void
|
||||
MavlinkFTP::_lock_request_queue(void)
|
||||
{
|
||||
do {}
|
||||
while (sem_wait(&_request_queue_sem) != 0);
|
||||
}
|
||||
|
||||
/// @brief Unlocks the semaphore providing exclusive access to the request queue
|
||||
void
|
||||
MavlinkFTP::_unlock_request_queue(void)
|
||||
{
|
||||
sem_post(&_request_queue_sem);
|
||||
}
|
||||
|
||||
/// @brief Returns a no longer needed request to the queue
|
||||
void
|
||||
MavlinkFTP::_return_request(Request *req)
|
||||
{
|
||||
_lock_request_queue();
|
||||
dq_addlast(&req->work.dq, &_request_queue);
|
||||
_unlock_request_queue();
|
||||
}
|
||||
|
||||
/// @brief Copy file (with limited space)
|
||||
int
|
||||
MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, ssize_t length)
|
||||
{
|
||||
char buff[512];
|
||||
int src_fd = -1, dst_fd = -1;
|
||||
int op_errno = 0;
|
||||
|
||||
src_fd = ::open(src_path, O_RDONLY);
|
||||
if (src_fd < 0) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
dst_fd = ::open(dst_path, O_CREAT | O_TRUNC | O_WRONLY);
|
||||
if (dst_fd < 0) {
|
||||
op_errno = errno;
|
||||
::close(src_fd);
|
||||
errno = op_errno;
|
||||
return -1;
|
||||
}
|
||||
|
||||
while (length > 0) {
|
||||
ssize_t bytes_read, bytes_written;
|
||||
size_t blen = (length > sizeof(buff))? sizeof(buff) : length;
|
||||
|
||||
bytes_read = ::read(src_fd, buff, blen);
|
||||
if (bytes_read == 0) {
|
||||
// EOF
|
||||
break;
|
||||
}
|
||||
else if (bytes_read < 0) {
|
||||
warnx("cp: read");
|
||||
op_errno = errno;
|
||||
break;
|
||||
}
|
||||
|
||||
bytes_written = ::write(dst_fd, buff, bytes_read);
|
||||
if (bytes_written != bytes_read) {
|
||||
warnx("cp: short write");
|
||||
op_errno = errno;
|
||||
break;
|
||||
}
|
||||
|
||||
length -= bytes_written;
|
||||
}
|
||||
|
||||
::close(src_fd);
|
||||
::close(dst_fd);
|
||||
|
||||
errno = op_errno;
|
||||
return (length > 0)? -1 : 0;
|
||||
}
|
||||
|
|
|
@ -33,17 +33,8 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
/**
|
||||
* @file mavlink_ftp.h
|
||||
*
|
||||
* MAVLink remote file server.
|
||||
*
|
||||
* A limited number of requests (currently 2) may be outstanding at a time.
|
||||
* Additional messages will be discarded.
|
||||
*
|
||||
* Messages consist of a fixed header, followed by a data area.
|
||||
*
|
||||
*/
|
||||
/// @file mavlink_ftp.h
|
||||
/// @author px4dev, Don Gagne <don@thegagnes.com>
|
||||
|
||||
#include <dirent.h>
|
||||
#include <queue.h>
|
||||
|
@ -54,183 +45,136 @@
|
|||
#include "mavlink_messages.h"
|
||||
#include "mavlink_main.h"
|
||||
|
||||
class MavlinkFtpTest;
|
||||
|
||||
/// @brief MAVLink remote file server. Support FTP like commands using MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL message.
|
||||
/// A limited number of requests (kRequestQueueSize) may be outstanding at a time. Additional messages will be discarded.
|
||||
class MavlinkFTP
|
||||
{
|
||||
public:
|
||||
/// @brief Returns the one Mavlink FTP server in the system.
|
||||
static MavlinkFTP* get_server(void);
|
||||
|
||||
/// @brief Contructor is only public so unit test code can new objects.
|
||||
MavlinkFTP();
|
||||
|
||||
/// @brief Adds the specified message to the work queue.
|
||||
void handle_message(Mavlink* mavlink, mavlink_message_t *msg);
|
||||
|
||||
typedef void (*ReceiveMessageFunc_t)(const mavlink_message_t *msg, MavlinkFtpTest* ftpTest);
|
||||
|
||||
/// @brief Sets up the server to run in unit test mode.
|
||||
/// @param rcvmsgFunc Function which will be called to handle outgoing mavlink messages.
|
||||
/// @param ftp_test MavlinkFtpTest object which the function is associated with
|
||||
void set_unittest_worker(ReceiveMessageFunc_t rcvMsgFunc, MavlinkFtpTest *ftp_test);
|
||||
|
||||
static MavlinkFTP *getServer();
|
||||
|
||||
// static interface
|
||||
void handle_message(Mavlink* mavlink,
|
||||
mavlink_message_t *msg);
|
||||
|
||||
private:
|
||||
|
||||
static const unsigned kRequestQueueSize = 2;
|
||||
|
||||
static MavlinkFTP *_server;
|
||||
|
||||
/// @brief Trying to pack structures across differing compilers is questionable for Clients, so we pad the
|
||||
/// structure ourselves to 32 bit alignment which should get us what we want.
|
||||
struct RequestHeader
|
||||
/// @brief This is the payload which is in mavlink_file_transfer_protocol_t.payload. We pad the structure ourselves to
|
||||
/// 32 bit alignment to avoid usage of any pack pragmas.
|
||||
struct PayloadHeader
|
||||
{
|
||||
uint16_t seqNumber; ///< sequence number for message
|
||||
uint8_t session; ///< Session id for read and write commands
|
||||
uint8_t opcode; ///< Command opcode
|
||||
uint8_t size; ///< Size of data
|
||||
uint8_t padding[3];
|
||||
uint32_t crc32; ///< CRC for entire Request structure, with crc32 and padding set to 0
|
||||
uint32_t offset; ///< Offsets for List and Read commands
|
||||
uint8_t data[];
|
||||
uint16_t seqNumber; ///< sequence number for message
|
||||
uint8_t session; ///< Session id for read and write commands
|
||||
uint8_t opcode; ///< Command opcode
|
||||
uint8_t size; ///< Size of data
|
||||
uint8_t req_opcode; ///< Request opcode returned in kRspAck, kRspNak message
|
||||
uint8_t padding[2]; ///< 32 bit aligment padding
|
||||
uint32_t offset; ///< Offsets for List and Read commands
|
||||
uint8_t data[]; ///< command data, varies by Opcode
|
||||
};
|
||||
|
||||
|
||||
/// @brief Command opcodes
|
||||
enum Opcode : uint8_t
|
||||
{
|
||||
kCmdNone, // ignored, always acked
|
||||
kCmdTerminate, // releases sessionID, closes file
|
||||
kCmdReset, // terminates all sessions
|
||||
kCmdList, // list files in <path> from <offset>
|
||||
kCmdOpen, // opens <path> for reading, returns <session>
|
||||
kCmdRead, // reads <size> bytes from <offset> in <session>
|
||||
kCmdCreate, // creates <path> for writing, returns <session>
|
||||
kCmdWrite, // appends <size> bytes at <offset> in <session>
|
||||
kCmdRemove, // remove file (only if created by server?)
|
||||
|
||||
kRspAck,
|
||||
kRspNak
|
||||
kCmdNone, ///< ignored, always acked
|
||||
kCmdTerminateSession, ///< Terminates open Read session
|
||||
kCmdResetSessions, ///< Terminates all open Read sessions
|
||||
kCmdListDirectory, ///< List files in <path> from <offset>
|
||||
kCmdOpenFileRO, ///< Opens file at <path> for reading, returns <session>
|
||||
kCmdReadFile, ///< Reads <size> bytes from <offset> in <session>
|
||||
kCmdCreateFile, ///< Creates file at <path> for writing, returns <session>
|
||||
kCmdWriteFile, ///< Writes <size> bytes to <offset> in <session>
|
||||
kCmdRemoveFile, ///< Remove file at <path>
|
||||
kCmdCreateDirectory, ///< Creates directory at <path>
|
||||
kCmdRemoveDirectory, ///< Removes Directory at <path>, must be empty
|
||||
kCmdOpenFileWO, ///< Opens file at <path> for writing, returns <session>
|
||||
kCmdTruncateFile, ///< Truncate file at <path> to <offset> length
|
||||
kCmdRename, ///< Rename <path1> to <path2>
|
||||
kCmdCalcFileCRC32, ///< Calculate CRC32 for file at <path>
|
||||
|
||||
kRspAck = 128, ///< Ack response
|
||||
kRspNak ///< Nak response
|
||||
};
|
||||
|
||||
|
||||
/// @brief Error codes returned in Nak response PayloadHeader.data[0].
|
||||
enum ErrorCode : uint8_t
|
||||
{
|
||||
{
|
||||
kErrNone,
|
||||
kErrNoRequest,
|
||||
kErrNoSession,
|
||||
kErrSequence,
|
||||
kErrNotDir,
|
||||
kErrNotFile,
|
||||
kErrEOF,
|
||||
kErrNotAppend,
|
||||
kErrTooBig,
|
||||
kErrIO,
|
||||
kErrPerm
|
||||
};
|
||||
|
||||
int _findUnusedSession(void);
|
||||
bool _validSession(unsigned index);
|
||||
|
||||
static const unsigned kMaxSession = 2;
|
||||
int _session_fds[kMaxSession];
|
||||
|
||||
class Request
|
||||
kErrFail, ///< Unknown failure
|
||||
kErrFailErrno, ///< Command failed, errno sent back in PayloadHeader.data[1]
|
||||
kErrInvalidDataSize, ///< PayloadHeader.size is invalid
|
||||
kErrInvalidSession, ///< Session is not currently open
|
||||
kErrNoSessionsAvailable, ///< All available Sessions in use
|
||||
kErrEOF, ///< Offset past end of file for List and Read commands
|
||||
kErrUnknownCommand ///< Unknown command opcode
|
||||
};
|
||||
|
||||
private:
|
||||
/// @brief Unit of work which is queued to work_queue
|
||||
struct Request
|
||||
{
|
||||
public:
|
||||
union {
|
||||
dq_entry_t entry;
|
||||
work_s work;
|
||||
};
|
||||
|
||||
bool decode(Mavlink *mavlink, mavlink_message_t *fromMessage) {
|
||||
if (fromMessage->msgid == MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) {
|
||||
_systemId = fromMessage->sysid;
|
||||
_mavlink = mavlink;
|
||||
mavlink_msg_file_transfer_protocol_decode(fromMessage, &_message);
|
||||
return _message.target_system == _mavlink->get_system_id();
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
void reply() {
|
||||
|
||||
// XXX the proper way would be an IOCTL / uORB call, rather than exploiting the
|
||||
// flat memory architecture, as we're operating between threads here.
|
||||
mavlink_message_t msg;
|
||||
msg.checksum = 0;
|
||||
unsigned len = mavlink_msg_file_transfer_protocol_pack_chan(_mavlink->get_system_id(), // Sender system id
|
||||
_mavlink->get_component_id(), // Sender component id
|
||||
_mavlink->get_channel(), // Channel to send on
|
||||
&msg, // Message to pack payload into
|
||||
0, // Target network
|
||||
_systemId, // Target system id
|
||||
0, // Target component id
|
||||
rawData()); // Payload to pack into message
|
||||
|
||||
_mavlink->lockMessageBufferMutex();
|
||||
bool success = _mavlink->message_buffer_write(&msg, len);
|
||||
_mavlink->unlockMessageBufferMutex();
|
||||
|
||||
if (!success) {
|
||||
warnx("FTP TX ERR");
|
||||
}
|
||||
#ifdef MAVLINK_FTP_DEBUG
|
||||
else {
|
||||
warnx("wrote: sys: %d, comp: %d, chan: %d, len: %d, checksum: %d",
|
||||
_mavlink->get_system_id(),
|
||||
_mavlink->get_component_id(),
|
||||
_mavlink->get_channel(),
|
||||
len,
|
||||
msg.checksum);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
uint8_t *rawData() { return &_message.payload[0]; }
|
||||
RequestHeader *header() { return reinterpret_cast<RequestHeader *>(&_message.payload[0]); }
|
||||
uint8_t *requestData() { return &(header()->data[0]); }
|
||||
unsigned dataSize() { return header()->size + sizeof(RequestHeader); }
|
||||
mavlink_channel_t channel() { return _mavlink->get_channel(); }
|
||||
|
||||
char *dataAsCString();
|
||||
|
||||
private:
|
||||
Mavlink *_mavlink;
|
||||
mavlink_file_transfer_protocol_t _message;
|
||||
uint8_t _systemId;
|
||||
work_s work; ///< work queue entry
|
||||
Mavlink *mavlink; ///< Mavlink to reply to
|
||||
uint8_t serverSystemId; ///< System ID to send from
|
||||
uint8_t serverComponentId; ///< Component ID to send from
|
||||
uint8_t serverChannel; ///< Channel to send to
|
||||
uint8_t targetSystemId; ///< System ID to target reply to
|
||||
|
||||
mavlink_file_transfer_protocol_t message; ///< Protocol message
|
||||
};
|
||||
|
||||
Request *_get_request(void);
|
||||
void _return_request(Request *req);
|
||||
void _lock_request_queue(void);
|
||||
void _unlock_request_queue(void);
|
||||
|
||||
char *_data_as_cstring(PayloadHeader* payload);
|
||||
|
||||
static void _worker_trampoline(void *arg);
|
||||
void _process_request(Request *req);
|
||||
void _reply(Request *req);
|
||||
int _copy_file(const char *src_path, const char *dst_path, ssize_t length);
|
||||
|
||||
static const char kDirentFile = 'F';
|
||||
static const char kDirentDir = 'D';
|
||||
static const char kDirentUnknown = 'U';
|
||||
static const uint8_t kMaxDataLength = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(RequestHeader);
|
||||
ErrorCode _workList(PayloadHeader *payload);
|
||||
ErrorCode _workOpen(PayloadHeader *payload, int oflag);
|
||||
ErrorCode _workRead(PayloadHeader *payload);
|
||||
ErrorCode _workWrite(PayloadHeader *payload);
|
||||
ErrorCode _workTerminate(PayloadHeader *payload);
|
||||
ErrorCode _workReset(PayloadHeader* payload);
|
||||
ErrorCode _workRemoveDirectory(PayloadHeader *payload);
|
||||
ErrorCode _workCreateDirectory(PayloadHeader *payload);
|
||||
ErrorCode _workRemoveFile(PayloadHeader *payload);
|
||||
ErrorCode _workTruncateFile(PayloadHeader *payload);
|
||||
ErrorCode _workRename(PayloadHeader *payload);
|
||||
ErrorCode _workCalcFileCRC32(PayloadHeader *payload);
|
||||
|
||||
/// Request worker; runs on the low-priority work queue to service
|
||||
/// remote requests.
|
||||
///
|
||||
static void _workerTrampoline(void *arg);
|
||||
void _worker(Request *req);
|
||||
|
||||
/// Reply to a request (XXX should be a Request method)
|
||||
///
|
||||
void _reply(Request *req);
|
||||
|
||||
ErrorCode _workList(Request *req);
|
||||
ErrorCode _workOpen(Request *req, bool create);
|
||||
ErrorCode _workRead(Request *req);
|
||||
ErrorCode _workWrite(Request *req);
|
||||
ErrorCode _workRemove(Request *req);
|
||||
ErrorCode _workTerminate(Request *req);
|
||||
ErrorCode _workReset();
|
||||
|
||||
// work freelist
|
||||
Request _workBufs[kRequestQueueSize];
|
||||
dq_queue_t _workFree;
|
||||
sem_t _lock;
|
||||
|
||||
void _qLock() { do {} while (sem_wait(&_lock) != 0); }
|
||||
void _qUnlock() { sem_post(&_lock); }
|
||||
|
||||
void _qFree(Request *req) {
|
||||
_qLock();
|
||||
dq_addlast(&req->entry, &_workFree);
|
||||
_qUnlock();
|
||||
}
|
||||
|
||||
Request *_dqFree() {
|
||||
_qLock();
|
||||
auto req = reinterpret_cast<Request *>(dq_remfirst(&_workFree));
|
||||
_qUnlock();
|
||||
return req;
|
||||
}
|
||||
|
||||
static const unsigned kRequestQueueSize = 2; ///< Max number of queued requests
|
||||
Request _request_bufs[kRequestQueueSize]; ///< Request buffers which hold work
|
||||
dq_queue_t _request_queue; ///< Queue of available Request buffers
|
||||
sem_t _request_queue_sem; ///< Semaphore for locking access to _request_queue
|
||||
|
||||
int _find_unused_session(void);
|
||||
bool _valid_session(unsigned index);
|
||||
|
||||
static const char kDirentFile = 'F'; ///< Identifies File returned from List command
|
||||
static const char kDirentDir = 'D'; ///< Identifies Directory returned from List command
|
||||
static const char kDirentSkip = 'S'; ///< Identifies Skipped entry from List command
|
||||
|
||||
/// @brief Maximum data size in RequestHeader::data
|
||||
static const uint8_t kMaxDataLength = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(PayloadHeader);
|
||||
|
||||
static const unsigned kMaxSession = 2; ///< Max number of active sessions
|
||||
int _session_fds[kMaxSession]; ///< Session file descriptors, 0 for empty slot
|
||||
|
||||
ReceiveMessageFunc_t _utRcvMsgFunc; ///< Unit test override for mavlink message sending
|
||||
MavlinkFtpTest *_ftp_test; ///< Additional parameter to _utRcvMsgFunc;
|
||||
};
|
||||
|
|
|
@ -123,6 +123,7 @@ Mavlink::Mavlink() :
|
|||
_task_running(false),
|
||||
_hil_enabled(false),
|
||||
_use_hil_gps(false),
|
||||
_forward_externalsp(false),
|
||||
_is_usb_uart(false),
|
||||
_wait_to_transmit(false),
|
||||
_received_messages(false),
|
||||
|
@ -483,6 +484,7 @@ void Mavlink::mavlink_update_system(void)
|
|||
_param_component_id = param_find("MAV_COMP_ID");
|
||||
_param_system_type = param_find("MAV_TYPE");
|
||||
_param_use_hil_gps = param_find("MAV_USEHILGPS");
|
||||
_param_forward_externalsp = param_find("MAV_FWDEXTSP");
|
||||
}
|
||||
|
||||
/* update system and component id */
|
||||
|
@ -529,6 +531,11 @@ void Mavlink::mavlink_update_system(void)
|
|||
param_get(_param_use_hil_gps, &use_hil_gps);
|
||||
|
||||
_use_hil_gps = (bool)use_hil_gps;
|
||||
|
||||
int32_t forward_externalsp;
|
||||
param_get(_param_forward_externalsp, &forward_externalsp);
|
||||
|
||||
_forward_externalsp = (bool)forward_externalsp;
|
||||
}
|
||||
|
||||
int Mavlink::get_system_id()
|
||||
|
@ -1387,26 +1394,26 @@ Mavlink::task_main(int argc, char *argv[])
|
|||
configure_stream("SYS_STATUS", 1.0f);
|
||||
configure_stream("GPS_GLOBAL_ORIGIN", 0.5f);
|
||||
configure_stream("HIGHRES_IMU", 1.0f);
|
||||
configure_stream("ATTITUDE", 15.0f);
|
||||
configure_stream("ATTITUDE", 10.0f);
|
||||
configure_stream("VFR_HUD", 8.0f);
|
||||
configure_stream("GPS_RAW_INT", 1.0f);
|
||||
configure_stream("GLOBAL_POSITION_INT", 3.0f);
|
||||
configure_stream("LOCAL_POSITION_NED", 3.0f);
|
||||
configure_stream("RC_CHANNELS_RAW", 1.0f);
|
||||
configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f);
|
||||
configure_stream("ATTITUDE_TARGET", 15.0f);
|
||||
configure_stream("ATTITUDE_TARGET", 3.0f);
|
||||
configure_stream("DISTANCE_SENSOR", 0.5f);
|
||||
configure_stream("OPTICAL_FLOW", 20.0f);
|
||||
configure_stream("OPTICAL_FLOW", 5.0f);
|
||||
break;
|
||||
|
||||
case MAVLINK_MODE_ONBOARD:
|
||||
configure_stream("SYS_STATUS", 1.0f);
|
||||
// XXX OBC change back: We need to be bandwidth-efficient here too
|
||||
configure_stream("ATTITUDE", 50.0f);
|
||||
configure_stream("GLOBAL_POSITION_INT", 50.0f);
|
||||
configure_stream("CAMERA_CAPTURE", 2.0f);
|
||||
configure_stream("ATTITUDE_TARGET", 50.0f);
|
||||
configure_stream("POSITION_TARGET_GLOBAL_INT", 20.0f);
|
||||
configure_stream("ATTITUDE_TARGET", 10.0f);
|
||||
configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f);
|
||||
configure_stream("VFR_HUD", 10.0f);
|
||||
break;
|
||||
|
||||
default:
|
||||
|
|
|
@ -137,6 +137,8 @@ public:
|
|||
|
||||
bool get_use_hil_gps() { return _use_hil_gps; }
|
||||
|
||||
bool get_forward_externalsp() { return _forward_externalsp; }
|
||||
|
||||
bool get_flow_control_enabled() { return _flow_control_enabled; }
|
||||
|
||||
bool get_forwarding_on() { return _forwarding_on; }
|
||||
|
@ -232,7 +234,7 @@ public:
|
|||
bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); }
|
||||
|
||||
bool message_buffer_write(const void *ptr, int size);
|
||||
|
||||
|
||||
void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); }
|
||||
void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); }
|
||||
|
||||
|
@ -275,6 +277,7 @@ private:
|
|||
/* states */
|
||||
bool _hil_enabled; /**< Hardware In the Loop mode */
|
||||
bool _use_hil_gps; /**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */
|
||||
bool _forward_externalsp; /**< Forward external setpoint messages to controllers directly if in offboard mode */
|
||||
bool _is_usb_uart; /**< Port is USB */
|
||||
bool _wait_to_transmit; /**< Wait to transmit until received messages. */
|
||||
bool _received_messages; /**< Whether we've received valid mavlink messages. */
|
||||
|
@ -349,6 +352,7 @@ private:
|
|||
param_t _param_component_id;
|
||||
param_t _param_system_type;
|
||||
param_t _param_use_hil_gps;
|
||||
param_t _param_forward_externalsp;
|
||||
|
||||
perf_counter_t _loop_perf; /**< loop performance counter */
|
||||
perf_counter_t _txerr_perf; /**< TX error counter */
|
||||
|
|
|
@ -37,6 +37,7 @@
|
|||
*
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
* @author Anton Babushkin <anton.babushkin@me.com>
|
||||
* @author Thomas Gubler <thomasgubler@gmail.com>
|
||||
*/
|
||||
|
||||
/* XXX trim includes */
|
||||
|
@ -105,10 +106,11 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
|
|||
_flow_pub(-1),
|
||||
_range_pub(-1),
|
||||
_offboard_control_sp_pub(-1),
|
||||
_local_pos_sp_pub(-1),
|
||||
_global_vel_sp_pub(-1),
|
||||
_att_sp_pub(-1),
|
||||
_rates_sp_pub(-1),
|
||||
_force_sp_pub(-1),
|
||||
_pos_sp_triplet_pub(-1),
|
||||
_vicon_position_pub(-1),
|
||||
_vision_position_pub(-1),
|
||||
_telemetry_status_pub(-1),
|
||||
|
@ -123,7 +125,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
|
|||
{
|
||||
|
||||
// make sure the FTP server is started
|
||||
(void)MavlinkFTP::getServer();
|
||||
(void)MavlinkFTP::get_server();
|
||||
}
|
||||
|
||||
MavlinkReceiver::~MavlinkReceiver()
|
||||
|
@ -154,6 +156,14 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
|
|||
handle_message_vicon_position_estimate(msg);
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED:
|
||||
handle_message_set_position_target_local_ned(msg);
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET:
|
||||
handle_message_set_attitude_target(msg);
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE:
|
||||
handle_message_vision_position_estimate(msg);
|
||||
break;
|
||||
|
@ -175,7 +185,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
|
|||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL:
|
||||
MavlinkFTP::getServer()->handle_message(_mavlink, msg);
|
||||
MavlinkFTP::get_server()->handle_message(_mavlink, msg);
|
||||
break;
|
||||
|
||||
default:
|
||||
|
@ -474,6 +484,189 @@ MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg)
|
|||
}
|
||||
}
|
||||
|
||||
void
|
||||
MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t *msg)
|
||||
{
|
||||
mavlink_set_position_target_local_ned_t set_position_target_local_ned;
|
||||
mavlink_msg_set_position_target_local_ned_decode(msg, &set_position_target_local_ned);
|
||||
|
||||
struct offboard_control_setpoint_s offboard_control_sp;
|
||||
memset(&offboard_control_sp, 0, sizeof(offboard_control_sp));//XXX breaks compatibility with multiple setpoints
|
||||
|
||||
/* Only accept messages which are intended for this system */
|
||||
if ((mavlink_system.sysid == set_position_target_local_ned.target_system ||
|
||||
set_position_target_local_ned.target_system == 0) &&
|
||||
(mavlink_system.compid == set_position_target_local_ned.target_component ||
|
||||
set_position_target_local_ned.target_component == 0)) {
|
||||
|
||||
/* convert mavlink type (local, NED) to uORB offboard control struct */
|
||||
switch (set_position_target_local_ned.coordinate_frame) {
|
||||
case MAV_FRAME_LOCAL_NED:
|
||||
offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED;
|
||||
break;
|
||||
case MAV_FRAME_LOCAL_OFFSET_NED:
|
||||
offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED;
|
||||
break;
|
||||
case MAV_FRAME_BODY_NED:
|
||||
offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED;
|
||||
break;
|
||||
case MAV_FRAME_BODY_OFFSET_NED:
|
||||
offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED;
|
||||
break;
|
||||
default:
|
||||
/* invalid setpoint, avoid publishing */
|
||||
return;
|
||||
}
|
||||
offboard_control_sp.position[0] = set_position_target_local_ned.x;
|
||||
offboard_control_sp.position[1] = set_position_target_local_ned.y;
|
||||
offboard_control_sp.position[2] = set_position_target_local_ned.z;
|
||||
offboard_control_sp.velocity[0] = set_position_target_local_ned.vx;
|
||||
offboard_control_sp.velocity[1] = set_position_target_local_ned.vy;
|
||||
offboard_control_sp.velocity[2] = set_position_target_local_ned.vz;
|
||||
offboard_control_sp.acceleration[0] = set_position_target_local_ned.afx;
|
||||
offboard_control_sp.acceleration[1] = set_position_target_local_ned.afy;
|
||||
offboard_control_sp.acceleration[2] = set_position_target_local_ned.afz;
|
||||
offboard_control_sp.yaw = set_position_target_local_ned.yaw;
|
||||
offboard_control_sp.yaw_rate = set_position_target_local_ned.yaw_rate;
|
||||
offboard_control_sp.isForceSetpoint = (bool)(set_position_target_local_ned.type_mask & (1 << 9));
|
||||
|
||||
/* If we are in force control mode, for now set offboard mode to force control */
|
||||
if (offboard_control_sp.isForceSetpoint) {
|
||||
offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_FORCE;
|
||||
}
|
||||
|
||||
/* set ignore flags */
|
||||
for (int i = 0; i < 9; i++) {
|
||||
offboard_control_sp.ignore &= ~(1 << i);
|
||||
offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << i));
|
||||
}
|
||||
offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_YAW);
|
||||
offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << 10)) <<
|
||||
OFB_IGN_BIT_YAW;
|
||||
offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_YAWRATE);
|
||||
offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << 11)) <<
|
||||
OFB_IGN_BIT_YAWRATE;
|
||||
|
||||
offboard_control_sp.timestamp = hrt_absolute_time();
|
||||
|
||||
if (_offboard_control_sp_pub < 0) {
|
||||
_offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp);
|
||||
}
|
||||
|
||||
/* If we are in offboard control mode and offboard control loop through is enabled
|
||||
* also publish the setpoint topic which is read by the controller */
|
||||
if (_mavlink->get_forward_externalsp()) {
|
||||
bool updated;
|
||||
orb_check(_control_mode_sub, &updated);
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
|
||||
}
|
||||
if (_control_mode.flag_control_offboard_enabled) {
|
||||
if (offboard_control_sp.isForceSetpoint &&
|
||||
offboard_control_sp_ignore_position_all(offboard_control_sp) &&
|
||||
offboard_control_sp_ignore_velocity_all(offboard_control_sp)) {
|
||||
/* The offboard setpoint is a force setpoint only, directly writing to the force
|
||||
* setpoint topic and not publishing the setpoint triplet topic */
|
||||
struct vehicle_force_setpoint_s force_sp;
|
||||
force_sp.x = offboard_control_sp.acceleration[0];
|
||||
force_sp.y = offboard_control_sp.acceleration[1];
|
||||
force_sp.z = offboard_control_sp.acceleration[2];
|
||||
//XXX: yaw
|
||||
if (_force_sp_pub < 0) {
|
||||
_force_sp_pub = orb_advertise(ORB_ID(vehicle_force_setpoint), &force_sp);
|
||||
} else {
|
||||
orb_publish(ORB_ID(vehicle_force_setpoint), _force_sp_pub, &force_sp);
|
||||
}
|
||||
} else {
|
||||
/* It's not a pure force setpoint: publish to setpoint triplet topic */
|
||||
struct position_setpoint_triplet_s pos_sp_triplet;
|
||||
pos_sp_triplet.previous.valid = false;
|
||||
pos_sp_triplet.next.valid = false;
|
||||
pos_sp_triplet.current.valid = true;
|
||||
pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others
|
||||
|
||||
/* set the local pos values if the setpoint type is 'local pos' and none
|
||||
* of the local pos fields is set to 'ignore' */
|
||||
if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
|
||||
!offboard_control_sp_ignore_position_some(offboard_control_sp)) {
|
||||
pos_sp_triplet.current.position_valid = true;
|
||||
pos_sp_triplet.current.x = offboard_control_sp.position[0];
|
||||
pos_sp_triplet.current.y = offboard_control_sp.position[1];
|
||||
pos_sp_triplet.current.z = offboard_control_sp.position[2];
|
||||
} else {
|
||||
pos_sp_triplet.current.position_valid = false;
|
||||
}
|
||||
|
||||
/* set the local vel values if the setpoint type is 'local pos' and none
|
||||
* of the local vel fields is set to 'ignore' */
|
||||
if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
|
||||
!offboard_control_sp_ignore_velocity_some(offboard_control_sp)) {
|
||||
pos_sp_triplet.current.velocity_valid = true;
|
||||
pos_sp_triplet.current.vx = offboard_control_sp.velocity[0];
|
||||
pos_sp_triplet.current.vy = offboard_control_sp.velocity[1];
|
||||
pos_sp_triplet.current.vz = offboard_control_sp.velocity[2];
|
||||
} else {
|
||||
pos_sp_triplet.current.velocity_valid = false;
|
||||
}
|
||||
|
||||
/* set the local acceleration values if the setpoint type is 'local pos' and none
|
||||
* of the accelerations fields is set to 'ignore' */
|
||||
if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
|
||||
!offboard_control_sp_ignore_acceleration_some(offboard_control_sp)) {
|
||||
pos_sp_triplet.current.acceleration_valid = true;
|
||||
pos_sp_triplet.current.a_x = offboard_control_sp.acceleration[0];
|
||||
pos_sp_triplet.current.a_y = offboard_control_sp.acceleration[1];
|
||||
pos_sp_triplet.current.a_z = offboard_control_sp.acceleration[2];
|
||||
pos_sp_triplet.current.acceleration_is_force =
|
||||
offboard_control_sp.isForceSetpoint;
|
||||
|
||||
} else {
|
||||
pos_sp_triplet.current.acceleration_valid = false;
|
||||
}
|
||||
|
||||
/* set the yaw sp value if the setpoint type is 'local pos' and the yaw
|
||||
* field is not set to 'ignore' */
|
||||
if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
|
||||
!offboard_control_sp_ignore_yaw(offboard_control_sp)) {
|
||||
pos_sp_triplet.current.yaw_valid = true;
|
||||
pos_sp_triplet.current.yaw = offboard_control_sp.yaw;
|
||||
|
||||
} else {
|
||||
pos_sp_triplet.current.yaw_valid = false;
|
||||
}
|
||||
|
||||
/* set the yawrate sp value if the setpoint type is 'local pos' and the yawrate
|
||||
* field is not set to 'ignore' */
|
||||
if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
|
||||
!offboard_control_sp_ignore_yawrate(offboard_control_sp)) {
|
||||
pos_sp_triplet.current.yawspeed_valid = true;
|
||||
pos_sp_triplet.current.yawspeed = offboard_control_sp.yaw_rate;
|
||||
|
||||
} else {
|
||||
pos_sp_triplet.current.yawspeed_valid = false;
|
||||
}
|
||||
//XXX handle global pos setpoints (different MAV frames)
|
||||
|
||||
|
||||
if (_pos_sp_triplet_pub < 0) {
|
||||
_pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet),
|
||||
&pos_sp_triplet);
|
||||
} else {
|
||||
orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub,
|
||||
&pos_sp_triplet);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
|
||||
{
|
||||
|
@ -513,6 +706,103 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
|
|||
}
|
||||
}
|
||||
|
||||
void
|
||||
MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
|
||||
{
|
||||
mavlink_set_attitude_target_t set_attitude_target;
|
||||
mavlink_msg_set_attitude_target_decode(msg, &set_attitude_target);
|
||||
|
||||
struct offboard_control_setpoint_s offboard_control_sp;
|
||||
memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); //XXX breaks compatibility with multiple setpoints
|
||||
|
||||
/* Only accept messages which are intended for this system */
|
||||
if ((mavlink_system.sysid == set_attitude_target.target_system ||
|
||||
set_attitude_target.target_system == 0) &&
|
||||
(mavlink_system.compid == set_attitude_target.target_component ||
|
||||
set_attitude_target.target_component == 0)) {
|
||||
for (int i = 0; i < 4; i++) {
|
||||
offboard_control_sp.attitude[i] = set_attitude_target.q[i];
|
||||
}
|
||||
offboard_control_sp.attitude_rate[0] = set_attitude_target.body_roll_rate;
|
||||
offboard_control_sp.attitude_rate[1] = set_attitude_target.body_pitch_rate;
|
||||
offboard_control_sp.attitude_rate[2] = set_attitude_target.body_yaw_rate;
|
||||
|
||||
/* set correct ignore flags for body rate fields: copy from mavlink message */
|
||||
for (int i = 0; i < 3; i++) {
|
||||
offboard_control_sp.ignore &= ~(1 << (i + OFB_IGN_BIT_BODYRATE_X));
|
||||
offboard_control_sp.ignore |= (set_attitude_target.type_mask & (1 << i)) << OFB_IGN_BIT_BODYRATE_X;
|
||||
}
|
||||
/* set correct ignore flags for thrust field: copy from mavlink message */
|
||||
offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_THRUST);
|
||||
offboard_control_sp.ignore |= ((set_attitude_target.type_mask & (1 << 6)) << OFB_IGN_BIT_THRUST);
|
||||
/* set correct ignore flags for attitude field: copy from mavlink message */
|
||||
offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_ATT);
|
||||
offboard_control_sp.ignore |= ((set_attitude_target.type_mask & (1 << 7)) << OFB_IGN_BIT_ATT);
|
||||
|
||||
|
||||
offboard_control_sp.timestamp = hrt_absolute_time();
|
||||
offboard_control_sp.mode =OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; //XXX handle rate control mode
|
||||
|
||||
if (_offboard_control_sp_pub < 0) {
|
||||
_offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp);
|
||||
}
|
||||
|
||||
/* If we are in offboard control mode and offboard control loop through is enabled
|
||||
* also publish the setpoint topic which is read by the controller */
|
||||
if (_mavlink->get_forward_externalsp()) {
|
||||
bool updated;
|
||||
orb_check(_control_mode_sub, &updated);
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
|
||||
}
|
||||
|
||||
if (_control_mode.flag_control_offboard_enabled) {
|
||||
|
||||
/* Publish attitude setpoint if attitude and thrust ignore bits are not set */
|
||||
if (!(offboard_control_sp_ignore_attitude(offboard_control_sp) ||
|
||||
offboard_control_sp_ignore_thrust(offboard_control_sp))) {
|
||||
struct vehicle_attitude_setpoint_s att_sp;
|
||||
att_sp.timestamp = hrt_absolute_time();
|
||||
mavlink_quaternion_to_euler(set_attitude_target.q,
|
||||
&att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body);
|
||||
mavlink_quaternion_to_dcm(set_attitude_target.q, att_sp.R_body);
|
||||
att_sp.R_valid = true;
|
||||
att_sp.thrust = set_attitude_target.thrust;
|
||||
memcpy(att_sp.q_d, set_attitude_target.q, sizeof(att_sp.q_d));
|
||||
att_sp.q_d_valid = true;
|
||||
if (_att_sp_pub < 0) {
|
||||
_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
|
||||
} else {
|
||||
orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &att_sp);
|
||||
}
|
||||
}
|
||||
|
||||
/* Publish attitude rate setpoint if bodyrate and thrust ignore bits are not set */
|
||||
///XXX add support for ignoring individual axes
|
||||
if (!(offboard_control_sp_ignore_bodyrates_some(offboard_control_sp) ||
|
||||
offboard_control_sp_ignore_thrust(offboard_control_sp))) {
|
||||
struct vehicle_rates_setpoint_s rates_sp;
|
||||
rates_sp.timestamp = hrt_absolute_time();
|
||||
rates_sp.roll = set_attitude_target.body_roll_rate;
|
||||
rates_sp.pitch = set_attitude_target.body_pitch_rate;
|
||||
rates_sp.yaw = set_attitude_target.body_yaw_rate;
|
||||
rates_sp.thrust = set_attitude_target.thrust;
|
||||
|
||||
if (_att_sp_pub < 0) {
|
||||
_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
|
||||
} else {
|
||||
orb_publish(ORB_ID(vehicle_rates_setpoint), _rates_sp_pub, &rates_sp);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
|
||||
{
|
||||
|
|
|
@ -71,6 +71,7 @@
|
|||
#include <uORB/topics/debug_key_value.h>
|
||||
#include <uORB/topics/airspeed.h>
|
||||
#include <uORB/topics/battery_status.h>
|
||||
#include <uORB/topics/vehicle_force_setpoint.h>
|
||||
|
||||
#include "mavlink_ftp.h"
|
||||
|
||||
|
@ -117,6 +118,8 @@ private:
|
|||
void handle_message_vicon_position_estimate(mavlink_message_t *msg);
|
||||
void handle_message_vision_position_estimate(mavlink_message_t *msg);
|
||||
void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg);
|
||||
void handle_message_set_position_target_local_ned(mavlink_message_t *msg);
|
||||
void handle_message_set_attitude_target(mavlink_message_t *msg);
|
||||
void handle_message_radio_status(mavlink_message_t *msg);
|
||||
void handle_message_manual_control(mavlink_message_t *msg);
|
||||
void handle_message_heartbeat(mavlink_message_t *msg);
|
||||
|
@ -145,10 +148,11 @@ private:
|
|||
orb_advert_t _flow_pub;
|
||||
orb_advert_t _range_pub;
|
||||
orb_advert_t _offboard_control_sp_pub;
|
||||
orb_advert_t _local_pos_sp_pub;
|
||||
orb_advert_t _global_vel_sp_pub;
|
||||
orb_advert_t _att_sp_pub;
|
||||
orb_advert_t _rates_sp_pub;
|
||||
orb_advert_t _force_sp_pub;
|
||||
orb_advert_t _pos_sp_triplet_pub;
|
||||
orb_advert_t _vicon_position_pub;
|
||||
orb_advert_t _vision_position_pub;
|
||||
orb_advert_t _telemetry_status_pub;
|
||||
|
|
|
@ -0,0 +1,761 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2014 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/// @file mavlink_ftp_test.cpp
|
||||
/// @author Don Gagne <don@thegagnes.com>
|
||||
|
||||
#include <sys/stat.h>
|
||||
#include <crc32.h>
|
||||
#include <stdio.h>
|
||||
#include <fcntl.h>
|
||||
|
||||
#include "mavlink_ftp_test.h"
|
||||
#include "../mavlink_ftp.h"
|
||||
|
||||
/// @brief Test case file name for Read command. File are generated using mavlink_ftp_test_data.py
|
||||
const MavlinkFtpTest::ReadTestCase MavlinkFtpTest::_rgReadTestCases[] = {
|
||||
{ "/etc/unit_test_data/mavlink_tests/test_238.data", MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader) - 1}, // Read takes less than single packet
|
||||
{ "/etc/unit_test_data/mavlink_tests/test_239.data", MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader) }, // Read completely fills single packet
|
||||
{ "/etc/unit_test_data/mavlink_tests/test_240.data", MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader) + 1 }, // Read take two packets
|
||||
};
|
||||
|
||||
const char MavlinkFtpTest::_unittest_microsd_dir[] = "/fs/microsd/ftp_unit_test_dir";
|
||||
const char MavlinkFtpTest::_unittest_microsd_file[] = "/fs/microsd/ftp_unit_test_dir/file";
|
||||
|
||||
MavlinkFtpTest::MavlinkFtpTest() :
|
||||
_ftp_server{},
|
||||
_reply_msg{},
|
||||
_lastOutgoingSeqNumber{}
|
||||
{
|
||||
}
|
||||
|
||||
MavlinkFtpTest::~MavlinkFtpTest()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
/// @brief Called before every test to initialize the FTP Server.
|
||||
void MavlinkFtpTest::_init(void)
|
||||
{
|
||||
_ftp_server = new MavlinkFTP;;
|
||||
_ftp_server->set_unittest_worker(MavlinkFtpTest::receive_message, this);
|
||||
|
||||
_cleanup_microsd();
|
||||
}
|
||||
|
||||
/// @brief Called after every test to take down the FTP Server.
|
||||
void MavlinkFtpTest::_cleanup(void)
|
||||
{
|
||||
delete _ftp_server;
|
||||
|
||||
_cleanup_microsd();
|
||||
}
|
||||
|
||||
/// @brief Tests for correct behavior of an Ack response.
|
||||
bool MavlinkFtpTest::_ack_test(void)
|
||||
{
|
||||
MavlinkFTP::PayloadHeader payload;
|
||||
mavlink_file_transfer_protocol_t ftp_msg;
|
||||
MavlinkFTP::PayloadHeader *reply;
|
||||
|
||||
payload.opcode = MavlinkFTP::kCmdNone;
|
||||
|
||||
bool success = _send_receive_msg(&payload, // FTP payload header
|
||||
0, // size in bytes of data
|
||||
nullptr, // Data to start into FTP message payload
|
||||
&ftp_msg, // Response from server
|
||||
&reply); // Payload inside FTP message response
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
||||
ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
|
||||
ut_compare("Incorrect payload size", reply->size, 0);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/// @brief Tests for correct response to an invalid opcpde.
|
||||
bool MavlinkFtpTest::_bad_opcode_test(void)
|
||||
{
|
||||
MavlinkFTP::PayloadHeader payload;
|
||||
mavlink_file_transfer_protocol_t ftp_msg;
|
||||
MavlinkFTP::PayloadHeader *reply;
|
||||
|
||||
payload.opcode = 0xFF; // bogus opcode
|
||||
|
||||
bool success = _send_receive_msg(&payload, // FTP payload header
|
||||
0, // size in bytes of data
|
||||
nullptr, // Data to start into FTP message payload
|
||||
&ftp_msg, // Response from server
|
||||
&reply); // Payload inside FTP message response
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
||||
ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
|
||||
ut_compare("Incorrect payload size", reply->size, 1);
|
||||
ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrUnknownCommand);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/// @brief Tests for correct reponse to a payload which an invalid data size field.
|
||||
bool MavlinkFtpTest::_bad_datasize_test(void)
|
||||
{
|
||||
mavlink_message_t msg;
|
||||
MavlinkFTP::PayloadHeader payload;
|
||||
mavlink_file_transfer_protocol_t ftp_msg;
|
||||
MavlinkFTP::PayloadHeader *reply;
|
||||
|
||||
payload.opcode = MavlinkFTP::kCmdListDirectory;
|
||||
|
||||
_setup_ftp_msg(&payload, 0, nullptr, &msg);
|
||||
|
||||
// Set the data size to be one larger than is legal
|
||||
((MavlinkFTP::PayloadHeader*)((mavlink_file_transfer_protocol_t*)msg.payload64)->payload)->size = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN + 1;
|
||||
|
||||
_ftp_server->handle_message(nullptr /* mavlink */, &msg);
|
||||
|
||||
if (!_decode_message(&_reply_msg, &ftp_msg, &reply)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
|
||||
ut_compare("Incorrect payload size", reply->size, 1);
|
||||
ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrInvalidDataSize);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool MavlinkFtpTest::_list_test(void)
|
||||
{
|
||||
MavlinkFTP::PayloadHeader payload;
|
||||
mavlink_file_transfer_protocol_t ftp_msg;
|
||||
MavlinkFTP::PayloadHeader *reply;
|
||||
|
||||
char response1[] = "Dempty_dir|Ftest_238.data\t238|Ftest_239.data\t239|Ftest_240.data\t240";
|
||||
char response2[] = "Ddev|Detc|Dfs|Dobj";
|
||||
|
||||
struct _testCase {
|
||||
const char *dir; ///< Directory to run List command on
|
||||
char *response; ///< Expected response entries from List command
|
||||
int response_count; ///< Number of directories that should be returned
|
||||
bool success; ///< true: List command should succeed, false: List command should fail
|
||||
};
|
||||
struct _testCase rgTestCases[] = {
|
||||
{ "/bogus", nullptr, 0, false },
|
||||
{ "/etc/unit_test_data/mavlink_tests", response1, 4, true },
|
||||
{ "/", response2, 4, true },
|
||||
};
|
||||
|
||||
for (size_t i=0; i<sizeof(rgTestCases)/sizeof(rgTestCases[0]); i++) {
|
||||
const struct _testCase *test = &rgTestCases[i];
|
||||
|
||||
payload.opcode = MavlinkFTP::kCmdListDirectory;
|
||||
payload.offset = 0;
|
||||
|
||||
bool success = _send_receive_msg(&payload, // FTP payload header
|
||||
strlen(test->dir)+1, // size in bytes of data
|
||||
(uint8_t*)test->dir, // Data to start into FTP message payload
|
||||
&ftp_msg, // Response from server
|
||||
&reply); // Payload inside FTP message response
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (test->success) {
|
||||
ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
|
||||
ut_compare("Incorrect payload size", reply->size, strlen(test->response) + 1);
|
||||
|
||||
// The return order of directories from the List command is not repeatable. So we can't do a direct comparison
|
||||
// to a hardcoded return result string.
|
||||
|
||||
// Convert null terminators to seperator char so we can use strok to parse returned data
|
||||
for (uint8_t j=0; j<reply->size-1; j++) {
|
||||
if (reply->data[j] == 0) {
|
||||
reply->data[j] = '|';
|
||||
}
|
||||
}
|
||||
|
||||
// Loop over returned directory entries trying to find then in the response list
|
||||
char *dir;
|
||||
int response_count = 0;
|
||||
dir = strtok((char *)&reply->data[0], "|");
|
||||
while (dir != nullptr) {
|
||||
ut_assert("Returned directory not found in expected response", strstr(test->response, dir));
|
||||
response_count++;
|
||||
dir = strtok(nullptr, "|");
|
||||
}
|
||||
|
||||
ut_compare("Incorrect number of directory entires returned", test->response_count, response_count);
|
||||
} else {
|
||||
ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
|
||||
ut_compare("Incorrect payload size", reply->size, 2);
|
||||
ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno);
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/// @brief Tests for correct reponse to a List command on a valid directory, but with an offset that
|
||||
/// is beyond the last directory entry.
|
||||
bool MavlinkFtpTest::_list_eof_test(void)
|
||||
{
|
||||
MavlinkFTP::PayloadHeader payload;
|
||||
mavlink_file_transfer_protocol_t ftp_msg;
|
||||
MavlinkFTP::PayloadHeader *reply;
|
||||
const char *dir = "/";
|
||||
|
||||
payload.opcode = MavlinkFTP::kCmdListDirectory;
|
||||
payload.offset = 4; // offset past top level dirs
|
||||
|
||||
bool success = _send_receive_msg(&payload, // FTP payload header
|
||||
strlen(dir)+1, // size in bytes of data
|
||||
(uint8_t*)dir, // Data to start into FTP message payload
|
||||
&ftp_msg, // Response from server
|
||||
&reply); // Payload inside FTP message response
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
||||
ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
|
||||
ut_compare("Incorrect payload size", reply->size, 1);
|
||||
ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrEOF);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/// @brief Tests for correct reponse to an Open command on a file which does not exist.
|
||||
bool MavlinkFtpTest::_open_badfile_test(void)
|
||||
{
|
||||
MavlinkFTP::PayloadHeader payload;
|
||||
mavlink_file_transfer_protocol_t ftp_msg;
|
||||
MavlinkFTP::PayloadHeader *reply;
|
||||
const char *dir = "/foo"; // non-existent file
|
||||
|
||||
payload.opcode = MavlinkFTP::kCmdOpenFileRO;
|
||||
payload.offset = 0;
|
||||
|
||||
bool success = _send_receive_msg(&payload, // FTP payload header
|
||||
strlen(dir)+1, // size in bytes of data
|
||||
(uint8_t*)dir, // Data to start into FTP message payload
|
||||
&ftp_msg, // Response from server
|
||||
&reply); // Payload inside FTP message response
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
||||
ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
|
||||
ut_compare("Incorrect payload size", reply->size, 2);
|
||||
ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/// @brief Tests for correct reponse to an Open command on a file, followed by Terminate
|
||||
bool MavlinkFtpTest::_open_terminate_test(void)
|
||||
{
|
||||
MavlinkFTP::PayloadHeader payload;
|
||||
mavlink_file_transfer_protocol_t ftp_msg;
|
||||
MavlinkFTP::PayloadHeader *reply;
|
||||
|
||||
for (size_t i=0; i<sizeof(_rgReadTestCases)/sizeof(_rgReadTestCases[0]); i++) {
|
||||
struct stat st;
|
||||
const ReadTestCase *test = &_rgReadTestCases[i];
|
||||
|
||||
payload.opcode = MavlinkFTP::kCmdOpenFileRO;
|
||||
payload.offset = 0;
|
||||
|
||||
bool success = _send_receive_msg(&payload, // FTP payload header
|
||||
strlen(test->file)+1, // size in bytes of data
|
||||
(uint8_t*)test->file, // Data to start into FTP message payload
|
||||
&ftp_msg, // Response from server
|
||||
&reply); // Payload inside FTP message response
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
||||
ut_compare("stat failed", stat(test->file, &st), 0);
|
||||
|
||||
|
||||
ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
|
||||
ut_compare("Incorrect payload size", reply->size, sizeof(uint32_t));
|
||||
ut_compare("File size incorrect", *((uint32_t*)&reply->data[0]), (uint32_t)st.st_size);
|
||||
|
||||
payload.opcode = MavlinkFTP::kCmdTerminateSession;
|
||||
payload.session = reply->session;
|
||||
payload.size = 0;
|
||||
|
||||
success = _send_receive_msg(&payload, // FTP payload header
|
||||
0, // size in bytes of data
|
||||
nullptr, // Data to start into FTP message payload
|
||||
&ftp_msg, // Response from server
|
||||
&reply); // Payload inside FTP message response
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
||||
ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
|
||||
ut_compare("Incorrect payload size", reply->size, 0);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/// @brief Tests for correct reponse to a Terminate command on an invalid session.
|
||||
bool MavlinkFtpTest::_terminate_badsession_test(void)
|
||||
{
|
||||
MavlinkFTP::PayloadHeader payload;
|
||||
mavlink_file_transfer_protocol_t ftp_msg;
|
||||
MavlinkFTP::PayloadHeader *reply;
|
||||
const char *file = _rgReadTestCases[0].file;
|
||||
|
||||
payload.opcode = MavlinkFTP::kCmdOpenFileRO;
|
||||
payload.offset = 0;
|
||||
|
||||
bool success = _send_receive_msg(&payload, // FTP payload header
|
||||
strlen(file)+1, // size in bytes of data
|
||||
(uint8_t*)file, // Data to start into FTP message payload
|
||||
&ftp_msg, // Response from server
|
||||
&reply); // Payload inside FTP message response
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
||||
ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
|
||||
|
||||
payload.opcode = MavlinkFTP::kCmdTerminateSession;
|
||||
payload.session = reply->session + 1;
|
||||
payload.size = 0;
|
||||
|
||||
success = _send_receive_msg(&payload, // FTP payload header
|
||||
0, // size in bytes of data
|
||||
nullptr, // Data to start into FTP message payload
|
||||
&ftp_msg, // Response from server
|
||||
&reply); // Payload inside FTP message response
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
||||
ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
|
||||
ut_compare("Incorrect payload size", reply->size, 1);
|
||||
ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrInvalidSession);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/// @brief Tests for correct reponse to a Read command on an open session.
|
||||
bool MavlinkFtpTest::_read_test(void)
|
||||
{
|
||||
MavlinkFTP::PayloadHeader payload;
|
||||
mavlink_file_transfer_protocol_t ftp_msg;
|
||||
MavlinkFTP::PayloadHeader *reply;
|
||||
|
||||
for (size_t i=0; i<sizeof(_rgReadTestCases)/sizeof(_rgReadTestCases[0]); i++) {
|
||||
struct stat st;
|
||||
const ReadTestCase *test = &_rgReadTestCases[i];
|
||||
|
||||
// Read in the file so we can compare it to what we get back
|
||||
ut_compare("stat failed", stat(test->file, &st), 0);
|
||||
uint8_t *bytes = new uint8_t[st.st_size];
|
||||
ut_assert("new failed", bytes != nullptr);
|
||||
int fd = ::open(test->file, O_RDONLY);
|
||||
ut_assert("open failed", fd != -1);
|
||||
int bytes_read = ::read(fd, bytes, st.st_size);
|
||||
ut_compare("read failed", bytes_read, st.st_size);
|
||||
::close(fd);
|
||||
|
||||
// Test case data files are created for specific boundary conditions
|
||||
ut_compare("Test case data files are out of date", test->length, st.st_size);
|
||||
|
||||
payload.opcode = MavlinkFTP::kCmdOpenFileRO;
|
||||
payload.offset = 0;
|
||||
|
||||
bool success = _send_receive_msg(&payload, // FTP payload header
|
||||
strlen(test->file)+1, // size in bytes of data
|
||||
(uint8_t*)test->file, // Data to start into FTP message payload
|
||||
&ftp_msg, // Response from server
|
||||
&reply); // Payload inside FTP message response
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
||||
ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
|
||||
|
||||
payload.opcode = MavlinkFTP::kCmdReadFile;
|
||||
payload.session = reply->session;
|
||||
payload.offset = 0;
|
||||
|
||||
success = _send_receive_msg(&payload, // FTP payload header
|
||||
0, // size in bytes of data
|
||||
nullptr, // Data to start into FTP message payload
|
||||
&ftp_msg, // Response from server
|
||||
&reply); // Payload inside FTP message response
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
||||
ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
|
||||
ut_compare("Offset incorrect", reply->offset, 0);
|
||||
|
||||
if (test->length <= MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader)) {
|
||||
ut_compare("Payload size incorrect", reply->size, (uint32_t)st.st_size);
|
||||
ut_compare("File contents differ", memcmp(reply->data, bytes, st.st_size), 0);
|
||||
}
|
||||
|
||||
payload.opcode = MavlinkFTP::kCmdTerminateSession;
|
||||
payload.session = reply->session;
|
||||
payload.size = 0;
|
||||
|
||||
success = _send_receive_msg(&payload, // FTP payload header
|
||||
0, // size in bytes of data
|
||||
nullptr, // Data to start into FTP message payload
|
||||
&ftp_msg, // Response from server
|
||||
&reply); // Payload inside FTP message response
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
||||
ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
|
||||
ut_compare("Incorrect payload size", reply->size, 0);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/// @brief Tests for correct reponse to a Read command on an invalid session.
|
||||
bool MavlinkFtpTest::_read_badsession_test(void)
|
||||
{
|
||||
MavlinkFTP::PayloadHeader payload;
|
||||
mavlink_file_transfer_protocol_t ftp_msg;
|
||||
MavlinkFTP::PayloadHeader *reply;
|
||||
const char *file = _rgReadTestCases[0].file;
|
||||
|
||||
payload.opcode = MavlinkFTP::kCmdOpenFileRO;
|
||||
payload.offset = 0;
|
||||
|
||||
bool success = _send_receive_msg(&payload, // FTP payload header
|
||||
strlen(file)+1, // size in bytes of data
|
||||
(uint8_t*)file, // Data to start into FTP message payload
|
||||
&ftp_msg, // Response from server
|
||||
&reply); // Payload inside FTP message response
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
||||
ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
|
||||
|
||||
payload.opcode = MavlinkFTP::kCmdReadFile;
|
||||
payload.session = reply->session + 1; // Invalid session
|
||||
payload.offset = 0;
|
||||
|
||||
success = _send_receive_msg(&payload, // FTP payload header
|
||||
0, // size in bytes of data
|
||||
nullptr, // Data to start into FTP message payload
|
||||
&ftp_msg, // Response from server
|
||||
&reply); // Payload inside FTP message response
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
||||
ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
|
||||
ut_compare("Incorrect payload size", reply->size, 1);
|
||||
ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrInvalidSession);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool MavlinkFtpTest::_removedirectory_test(void)
|
||||
{
|
||||
MavlinkFTP::PayloadHeader payload;
|
||||
mavlink_file_transfer_protocol_t ftp_msg;
|
||||
MavlinkFTP::PayloadHeader *reply;
|
||||
int fd;
|
||||
|
||||
struct _testCase {
|
||||
const char *dir;
|
||||
bool success;
|
||||
bool deleteFile;
|
||||
};
|
||||
static const struct _testCase rgTestCases[] = {
|
||||
{ "/bogus", false, false },
|
||||
{ "/etc/unit_test_data/mavlink_tests/empty_dir", false, false },
|
||||
{ _unittest_microsd_dir, false, false },
|
||||
{ _unittest_microsd_file, false, false },
|
||||
{ _unittest_microsd_dir, true, true },
|
||||
};
|
||||
|
||||
ut_compare("mkdir failed", ::mkdir(_unittest_microsd_dir, S_IRWXU | S_IRWXG | S_IRWXO), 0);
|
||||
ut_assert("open failed", (fd = ::open(_unittest_microsd_file, O_CREAT | O_EXCL)) != -1);
|
||||
::close(fd);
|
||||
|
||||
for (size_t i=0; i<sizeof(rgTestCases)/sizeof(rgTestCases[0]); i++) {
|
||||
const struct _testCase *test = &rgTestCases[i];
|
||||
|
||||
if (test->deleteFile) {
|
||||
ut_compare("unlink failed", ::unlink(_unittest_microsd_file), 0);
|
||||
}
|
||||
|
||||
payload.opcode = MavlinkFTP::kCmdRemoveDirectory;
|
||||
payload.offset = 0;
|
||||
|
||||
bool success = _send_receive_msg(&payload, // FTP payload header
|
||||
strlen(test->dir)+1, // size in bytes of data
|
||||
(uint8_t*)test->dir, // Data to start into FTP message payload
|
||||
&ftp_msg, // Response from server
|
||||
&reply); // Payload inside FTP message response
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (test->success) {
|
||||
ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
|
||||
ut_compare("Incorrect payload size", reply->size, 0);
|
||||
} else {
|
||||
ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
|
||||
ut_compare("Incorrect payload size", reply->size, 2);
|
||||
ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno);
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool MavlinkFtpTest::_createdirectory_test(void)
|
||||
{
|
||||
MavlinkFTP::PayloadHeader payload;
|
||||
mavlink_file_transfer_protocol_t ftp_msg;
|
||||
MavlinkFTP::PayloadHeader *reply;
|
||||
|
||||
struct _testCase {
|
||||
const char *dir;
|
||||
bool success;
|
||||
};
|
||||
static const struct _testCase rgTestCases[] = {
|
||||
{ "/etc/bogus", false },
|
||||
{ _unittest_microsd_dir, true },
|
||||
{ _unittest_microsd_dir, false },
|
||||
{ "/fs/microsd/bogus/bogus", false },
|
||||
};
|
||||
|
||||
for (size_t i=0; i<sizeof(rgTestCases)/sizeof(rgTestCases[0]); i++) {
|
||||
const struct _testCase *test = &rgTestCases[i];
|
||||
|
||||
payload.opcode = MavlinkFTP::kCmdCreateDirectory;
|
||||
payload.offset = 0;
|
||||
|
||||
bool success = _send_receive_msg(&payload, // FTP payload header
|
||||
strlen(test->dir)+1, // size in bytes of data
|
||||
(uint8_t*)test->dir, // Data to start into FTP message payload
|
||||
&ftp_msg, // Response from server
|
||||
&reply); // Payload inside FTP message response
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (test->success) {
|
||||
ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
|
||||
ut_compare("Incorrect payload size", reply->size, 0);
|
||||
} else {
|
||||
ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
|
||||
ut_compare("Incorrect payload size", reply->size, 2);
|
||||
ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno);
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool MavlinkFtpTest::_removefile_test(void)
|
||||
{
|
||||
MavlinkFTP::PayloadHeader payload;
|
||||
mavlink_file_transfer_protocol_t ftp_msg;
|
||||
MavlinkFTP::PayloadHeader *reply;
|
||||
int fd;
|
||||
|
||||
struct _testCase {
|
||||
const char *file;
|
||||
bool success;
|
||||
};
|
||||
static const struct _testCase rgTestCases[] = {
|
||||
{ "/bogus", false },
|
||||
{ _rgReadTestCases[0].file, false },
|
||||
{ _unittest_microsd_dir, false },
|
||||
{ _unittest_microsd_file, true },
|
||||
{ _unittest_microsd_file, false },
|
||||
};
|
||||
|
||||
ut_compare("mkdir failed", ::mkdir(_unittest_microsd_dir, S_IRWXU | S_IRWXG | S_IRWXO), 0);
|
||||
ut_assert("open failed", (fd = ::open(_unittest_microsd_file, O_CREAT | O_EXCL)) != -1);
|
||||
::close(fd);
|
||||
|
||||
for (size_t i=0; i<sizeof(rgTestCases)/sizeof(rgTestCases[0]); i++) {
|
||||
const struct _testCase *test = &rgTestCases[i];
|
||||
|
||||
payload.opcode = MavlinkFTP::kCmdRemoveFile;
|
||||
payload.offset = 0;
|
||||
|
||||
bool success = _send_receive_msg(&payload, // FTP payload header
|
||||
strlen(test->file)+1, // size in bytes of data
|
||||
(uint8_t*)test->file, // Data to start into FTP message payload
|
||||
&ftp_msg, // Response from server
|
||||
&reply); // Payload inside FTP message response
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (test->success) {
|
||||
ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
|
||||
ut_compare("Incorrect payload size", reply->size, 0);
|
||||
} else {
|
||||
ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
|
||||
ut_compare("Incorrect payload size", reply->size, 2);
|
||||
ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno);
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/// @brief Static method used as callback from MavlinkFTP. This method will be called by MavlinkFTP when
|
||||
/// it needs to send a message out on Mavlink.
|
||||
void MavlinkFtpTest::receive_message(const mavlink_message_t *msg, MavlinkFtpTest *ftp_test)
|
||||
{
|
||||
ftp_test->_receive_message(msg);
|
||||
}
|
||||
|
||||
/// @brief Non-Static version of receive_message
|
||||
void MavlinkFtpTest::_receive_message(const mavlink_message_t *msg)
|
||||
{
|
||||
// Move the message into our own member variable
|
||||
memcpy(&_reply_msg, msg, sizeof(mavlink_message_t));
|
||||
}
|
||||
|
||||
/// @brief Decode and validate the incoming message
|
||||
bool MavlinkFtpTest::_decode_message(const mavlink_message_t *msg, ///< Mavlink message to decode
|
||||
mavlink_file_transfer_protocol_t *ftp_msg, ///< Decoded FTP message
|
||||
MavlinkFTP::PayloadHeader **payload) ///< Payload inside FTP message response
|
||||
{
|
||||
mavlink_msg_file_transfer_protocol_decode(msg, ftp_msg);
|
||||
|
||||
// Make sure the targets are correct
|
||||
ut_compare("Target network non-zero", ftp_msg->target_network, 0);
|
||||
ut_compare("Target system id mismatch", ftp_msg->target_system, clientSystemId);
|
||||
ut_compare("Target component id mismatch", ftp_msg->target_component, clientComponentId);
|
||||
|
||||
*payload = reinterpret_cast<MavlinkFTP::PayloadHeader *>(ftp_msg->payload);
|
||||
|
||||
// Make sure we have a good sequence number
|
||||
ut_compare("Sequence number mismatch", (*payload)->seqNumber, _lastOutgoingSeqNumber + 1);
|
||||
|
||||
// Bump sequence number for next outgoing message
|
||||
_lastOutgoingSeqNumber++;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/// @brief Initializes an FTP message into a mavlink message
|
||||
void MavlinkFtpTest::_setup_ftp_msg(MavlinkFTP::PayloadHeader *payload_header, ///< FTP payload header
|
||||
uint8_t size, ///< size in bytes of data
|
||||
const uint8_t *data, ///< Data to start into FTP message payload
|
||||
mavlink_message_t *msg) ///< Returned mavlink message
|
||||
{
|
||||
uint8_t payload_bytes[MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN];
|
||||
MavlinkFTP::PayloadHeader *payload = reinterpret_cast<MavlinkFTP::PayloadHeader *>(payload_bytes);
|
||||
|
||||
memcpy(payload, payload_header, sizeof(MavlinkFTP::PayloadHeader));
|
||||
|
||||
payload->seqNumber = _lastOutgoingSeqNumber;
|
||||
payload->size = size;
|
||||
if (size != 0) {
|
||||
memcpy(payload->data, data, size);
|
||||
}
|
||||
|
||||
payload->padding[0] = 0;
|
||||
payload->padding[1] = 0;
|
||||
|
||||
msg->checksum = 0;
|
||||
mavlink_msg_file_transfer_protocol_pack(clientSystemId, // Sender system id
|
||||
clientComponentId, // Sender component id
|
||||
msg, // Message to pack payload into
|
||||
0, // Target network
|
||||
serverSystemId, // Target system id
|
||||
serverComponentId, // Target component id
|
||||
payload_bytes); // Payload to pack into message
|
||||
}
|
||||
|
||||
/// @brief Sends the specified FTP message to the server and returns response
|
||||
bool MavlinkFtpTest::_send_receive_msg(MavlinkFTP::PayloadHeader *payload_header, ///< FTP payload header
|
||||
uint8_t size, ///< size in bytes of data
|
||||
const uint8_t *data, ///< Data to start into FTP message payload
|
||||
mavlink_file_transfer_protocol_t *ftp_msg_reply, ///< Response from server
|
||||
MavlinkFTP::PayloadHeader **payload_reply) ///< Payload inside FTP message response
|
||||
{
|
||||
mavlink_message_t msg;
|
||||
|
||||
_setup_ftp_msg(payload_header, size, data, &msg);
|
||||
_ftp_server->handle_message(nullptr /* mavlink */, &msg);
|
||||
return _decode_message(&_reply_msg, ftp_msg_reply, payload_reply);
|
||||
}
|
||||
|
||||
/// @brief Cleans up an files created on microsd during testing
|
||||
void MavlinkFtpTest::_cleanup_microsd(void)
|
||||
{
|
||||
::unlink(_unittest_microsd_file);
|
||||
::rmdir(_unittest_microsd_dir);
|
||||
}
|
||||
|
||||
/// @brief Runs all the unit tests
|
||||
bool MavlinkFtpTest::run_tests(void)
|
||||
{
|
||||
ut_run_test(_ack_test);
|
||||
ut_run_test(_bad_opcode_test);
|
||||
ut_run_test(_bad_datasize_test);
|
||||
ut_run_test(_list_test);
|
||||
ut_run_test(_list_eof_test);
|
||||
ut_run_test(_open_badfile_test);
|
||||
ut_run_test(_open_terminate_test);
|
||||
ut_run_test(_terminate_badsession_test);
|
||||
ut_run_test(_read_test);
|
||||
ut_run_test(_read_badsession_test);
|
||||
ut_run_test(_removedirectory_test);
|
||||
ut_run_test(_createdirectory_test);
|
||||
ut_run_test(_removefile_test);
|
||||
|
||||
return (_tests_failed == 0);
|
||||
|
||||
}
|
||||
|
||||
ut_declare_test(mavlink_ftp_test, MavlinkFtpTest)
|
|
@ -0,0 +1,108 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2014 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/// @file mavlink_ftp_test.h
|
||||
/// @author Don Gagne <don@thegagnes.com>
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <unit_test/unit_test.h>
|
||||
#include "../mavlink_bridge_header.h"
|
||||
#include "../mavlink_ftp.h"
|
||||
|
||||
class MavlinkFtpTest : public UnitTest
|
||||
{
|
||||
public:
|
||||
MavlinkFtpTest();
|
||||
virtual ~MavlinkFtpTest();
|
||||
|
||||
virtual bool run_tests(void);
|
||||
|
||||
static void receive_message(const mavlink_message_t *msg, MavlinkFtpTest* ftpTest);
|
||||
|
||||
static const uint8_t serverSystemId = 50; ///< System ID for server
|
||||
static const uint8_t serverComponentId = 1; ///< Component ID for server
|
||||
static const uint8_t serverChannel = 0; ///< Channel to send to
|
||||
|
||||
static const uint8_t clientSystemId = 1; ///< System ID for client
|
||||
static const uint8_t clientComponentId = 0; ///< Component ID for client
|
||||
|
||||
// We don't want any of these
|
||||
MavlinkFtpTest(const MavlinkFtpTest&);
|
||||
MavlinkFtpTest& operator=(const MavlinkFtpTest&);
|
||||
|
||||
private:
|
||||
virtual void _init(void);
|
||||
virtual void _cleanup(void);
|
||||
|
||||
bool _ack_test(void);
|
||||
bool _bad_opcode_test(void);
|
||||
bool _bad_datasize_test(void);
|
||||
bool _list_test(void);
|
||||
bool _list_eof_test(void);
|
||||
bool _open_badfile_test(void);
|
||||
bool _open_terminate_test(void);
|
||||
bool _terminate_badsession_test(void);
|
||||
bool _read_test(void);
|
||||
bool _read_badsession_test(void);
|
||||
bool _removedirectory_test(void);
|
||||
bool _createdirectory_test(void);
|
||||
bool _removefile_test(void);
|
||||
|
||||
void _receive_message(const mavlink_message_t *msg);
|
||||
void _setup_ftp_msg(MavlinkFTP::PayloadHeader *payload_header, uint8_t size, const uint8_t *data, mavlink_message_t *msg);
|
||||
bool _decode_message(const mavlink_message_t *msg, mavlink_file_transfer_protocol_t *ftp_msg, MavlinkFTP::PayloadHeader **payload);
|
||||
bool _send_receive_msg(MavlinkFTP::PayloadHeader *payload_header,
|
||||
uint8_t size,
|
||||
const uint8_t *data,
|
||||
mavlink_file_transfer_protocol_t *ftp_msg_reply,
|
||||
MavlinkFTP::PayloadHeader **payload_reply);
|
||||
void _cleanup_microsd(void);
|
||||
|
||||
MavlinkFTP *_ftp_server;
|
||||
|
||||
mavlink_message_t _reply_msg;
|
||||
|
||||
uint16_t _lastOutgoingSeqNumber;
|
||||
|
||||
struct ReadTestCase {
|
||||
const char *file;
|
||||
const uint16_t length;
|
||||
};
|
||||
static const ReadTestCase _rgReadTestCases[];
|
||||
|
||||
static const char _unittest_microsd_dir[];
|
||||
static const char _unittest_microsd_file[];
|
||||
};
|
||||
|
||||
bool mavlink_ftp_test(void);
|
|
@ -0,0 +1,7 @@
|
|||
import sys
|
||||
print 'Arguments: file - ' + sys.argv[1] + ', length - ' + sys.argv[2]
|
||||
file = open(sys.argv[1], 'w')
|
||||
for i in range(int(sys.argv[2])):
|
||||
b = bytearray([i & 0xFF])
|
||||
file.write(b)
|
||||
file.close()
|
|
@ -0,0 +1,47 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2014 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file mavlink_ftp_tests.cpp
|
||||
*/
|
||||
|
||||
#include <systemlib/err.h>
|
||||
|
||||
#include "mavlink_ftp_test.h"
|
||||
|
||||
extern "C" __EXPORT int mavlink_tests_main(int argc, char *argv[]);
|
||||
|
||||
int mavlink_tests_main(int argc, char *argv[])
|
||||
{
|
||||
return mavlink_ftp_test() ? 0 : -1;
|
||||
}
|
|
@ -0,0 +1,50 @@
|
|||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2014 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
#
|
||||
# System state machine tests.
|
||||
#
|
||||
|
||||
MODULE_COMMAND = mavlink_tests
|
||||
SRCS = mavlink_tests.cpp \
|
||||
mavlink_ftp_test.cpp \
|
||||
../mavlink_ftp.cpp \
|
||||
../mavlink.c
|
||||
|
||||
INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink
|
||||
|
||||
MODULE_STACKSIZE = 5000
|
||||
|
||||
MAXOPTIMIZATION = -Os
|
||||
|
||||
EXTRACXXFLAGS = -Weffc++ -DMAVLINK_FTP_UNIT_TEST
|
|
@ -76,6 +76,7 @@
|
|||
|
||||
#define TILT_COS_MAX 0.7f
|
||||
#define SIGMA 0.000001f
|
||||
#define MIN_DIST 0.01f
|
||||
|
||||
/**
|
||||
* Multicopter position control app start / stop handling function
|
||||
|
@ -179,6 +180,7 @@ private:
|
|||
|
||||
bool _reset_pos_sp;
|
||||
bool _reset_alt_sp;
|
||||
bool _mode_auto;
|
||||
|
||||
math::Vector<3> _pos;
|
||||
math::Vector<3> _pos_sp;
|
||||
|
@ -219,6 +221,11 @@ private:
|
|||
*/
|
||||
void reset_alt_sp();
|
||||
|
||||
/**
|
||||
* Check if position setpoint is too far from current position and adjust it if needed.
|
||||
*/
|
||||
void limit_pos_sp_offset();
|
||||
|
||||
/**
|
||||
* Set position setpoint using manual control
|
||||
*/
|
||||
|
@ -229,6 +236,14 @@ private:
|
|||
*/
|
||||
void control_offboard(float dt);
|
||||
|
||||
bool cross_sphere_line(const math::Vector<3>& sphere_c, float sphere_r,
|
||||
const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3>& res);
|
||||
|
||||
/**
|
||||
* Set position setpoint for AUTO
|
||||
*/
|
||||
void control_auto(float dt);
|
||||
|
||||
/**
|
||||
* Select between barometric and global (AMSL) altitudes
|
||||
*/
|
||||
|
@ -283,7 +298,8 @@ MulticopterPositionControl::MulticopterPositionControl() :
|
|||
_ref_timestamp(0),
|
||||
|
||||
_reset_pos_sp(true),
|
||||
_reset_alt_sp(true)
|
||||
_reset_alt_sp(true),
|
||||
_mode_auto(false)
|
||||
{
|
||||
memset(&_att, 0, sizeof(_att));
|
||||
memset(&_att_sp, 0, sizeof(_att_sp));
|
||||
|
@ -533,6 +549,29 @@ MulticopterPositionControl::reset_alt_sp()
|
|||
}
|
||||
}
|
||||
|
||||
void
|
||||
MulticopterPositionControl::limit_pos_sp_offset()
|
||||
{
|
||||
math::Vector<3> pos_sp_offs;
|
||||
pos_sp_offs.zero();
|
||||
|
||||
if (_control_mode.flag_control_position_enabled) {
|
||||
pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0);
|
||||
pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1);
|
||||
}
|
||||
|
||||
if (_control_mode.flag_control_altitude_enabled) {
|
||||
pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2);
|
||||
}
|
||||
|
||||
float pos_sp_offs_norm = pos_sp_offs.length();
|
||||
|
||||
if (pos_sp_offs_norm > 1.0f) {
|
||||
pos_sp_offs /= pos_sp_offs_norm;
|
||||
_pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
MulticopterPositionControl::control_manual(float dt)
|
||||
{
|
||||
|
@ -614,7 +653,6 @@ MulticopterPositionControl::control_offboard(float dt)
|
|||
_pos_sp(0) = _pos_sp_triplet.current.x;
|
||||
_pos_sp(1) = _pos_sp_triplet.current.y;
|
||||
_pos_sp(2) = _pos_sp_triplet.current.z;
|
||||
_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
|
||||
|
||||
} else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) {
|
||||
/* control velocity */
|
||||
|
@ -624,6 +662,11 @@ MulticopterPositionControl::control_offboard(float dt)
|
|||
/* set position setpoint move rate */
|
||||
_sp_move_rate(0) = _pos_sp_triplet.current.vx;
|
||||
_sp_move_rate(1) = _pos_sp_triplet.current.vy;
|
||||
}
|
||||
|
||||
if (_pos_sp_triplet.current.yaw_valid) {
|
||||
_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
|
||||
} else if (_pos_sp_triplet.current.yawspeed_valid) {
|
||||
_att_sp.yaw_body = _att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * dt;
|
||||
}
|
||||
|
||||
|
@ -647,6 +690,170 @@ MulticopterPositionControl::control_offboard(float dt)
|
|||
}
|
||||
}
|
||||
|
||||
bool
|
||||
MulticopterPositionControl::cross_sphere_line(const math::Vector<3>& sphere_c, float sphere_r,
|
||||
const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3>& res)
|
||||
{
|
||||
/* project center of sphere on line */
|
||||
/* normalized AB */
|
||||
math::Vector<3> ab_norm = line_b - line_a;
|
||||
ab_norm.normalize();
|
||||
math::Vector<3> d = line_a + ab_norm * ((sphere_c - line_a) * ab_norm);
|
||||
float cd_len = (sphere_c - d).length();
|
||||
|
||||
/* we have triangle CDX with known CD and CX = R, find DX */
|
||||
if (sphere_r > cd_len) {
|
||||
/* have two roots, select one in A->B direction from D */
|
||||
float dx_len = sqrtf(sphere_r * sphere_r - cd_len * cd_len);
|
||||
res = d + ab_norm * dx_len;
|
||||
return true;
|
||||
|
||||
} else {
|
||||
/* have no roots, return D */
|
||||
res = d;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
MulticopterPositionControl::control_auto(float dt)
|
||||
{
|
||||
if (!_mode_auto) {
|
||||
_mode_auto = true;
|
||||
/* reset position setpoint on AUTO mode activation */
|
||||
reset_pos_sp();
|
||||
reset_alt_sp();
|
||||
}
|
||||
|
||||
bool updated;
|
||||
orb_check(_pos_sp_triplet_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
|
||||
}
|
||||
|
||||
if (_pos_sp_triplet.current.valid) {
|
||||
/* in case of interrupted mission don't go to waypoint but stay at current position */
|
||||
_reset_pos_sp = true;
|
||||
_reset_alt_sp = true;
|
||||
|
||||
/* project setpoint to local frame */
|
||||
math::Vector<3> curr_sp;
|
||||
map_projection_project(&_ref_pos,
|
||||
_pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon,
|
||||
&curr_sp.data[0], &curr_sp.data[1]);
|
||||
curr_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt);
|
||||
|
||||
/* scaled space: 1 == position error resulting max allowed speed, L1 = 1 in this space */
|
||||
math::Vector<3> scale = _params.pos_p.edivide(_params.vel_max); // TODO add mult param here
|
||||
|
||||
/* convert current setpoint to scaled space */
|
||||
math::Vector<3> curr_sp_s = curr_sp.emult(scale);
|
||||
|
||||
/* by default use current setpoint as is */
|
||||
math::Vector<3> pos_sp_s = curr_sp_s;
|
||||
|
||||
if (_pos_sp_triplet.current.type == SETPOINT_TYPE_POSITION && _pos_sp_triplet.previous.valid) {
|
||||
/* follow "previous - current" line */
|
||||
math::Vector<3> prev_sp;
|
||||
map_projection_project(&_ref_pos,
|
||||
_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon,
|
||||
&prev_sp.data[0], &prev_sp.data[1]);
|
||||
prev_sp(2) = -(_pos_sp_triplet.previous.alt - _ref_alt);
|
||||
|
||||
if ((curr_sp - prev_sp).length() > MIN_DIST) {
|
||||
|
||||
/* find X - cross point of L1 sphere and trajectory */
|
||||
math::Vector<3> pos_s = _pos.emult(scale);
|
||||
math::Vector<3> prev_sp_s = prev_sp.emult(scale);
|
||||
math::Vector<3> prev_curr_s = curr_sp_s - prev_sp_s;
|
||||
math::Vector<3> curr_pos_s = pos_s - curr_sp_s;
|
||||
float curr_pos_s_len = curr_pos_s.length();
|
||||
if (curr_pos_s_len < 1.0f) {
|
||||
/* copter is closer to waypoint than L1 radius */
|
||||
/* check next waypoint and use it to avoid slowing down when passing via waypoint */
|
||||
if (_pos_sp_triplet.next.valid) {
|
||||
math::Vector<3> next_sp;
|
||||
map_projection_project(&_ref_pos,
|
||||
_pos_sp_triplet.next.lat, _pos_sp_triplet.next.lon,
|
||||
&next_sp.data[0], &next_sp.data[1]);
|
||||
next_sp(2) = -(_pos_sp_triplet.next.alt - _ref_alt);
|
||||
|
||||
if ((next_sp - curr_sp).length() > MIN_DIST) {
|
||||
math::Vector<3> next_sp_s = next_sp.emult(scale);
|
||||
|
||||
/* calculate angle prev - curr - next */
|
||||
math::Vector<3> curr_next_s = next_sp_s - curr_sp_s;
|
||||
math::Vector<3> prev_curr_s_norm = prev_curr_s.normalized();
|
||||
|
||||
/* cos(a) * curr_next, a = angle between current and next trajectory segments */
|
||||
float cos_a_curr_next = prev_curr_s_norm * curr_next_s;
|
||||
|
||||
/* cos(b), b = angle pos - curr_sp - prev_sp */
|
||||
float cos_b = -curr_pos_s * prev_curr_s_norm / curr_pos_s_len;
|
||||
|
||||
if (cos_a_curr_next > 0.0f && cos_b > 0.0f) {
|
||||
float curr_next_s_len = curr_next_s.length();
|
||||
/* if curr - next distance is larger than L1 radius, limit it */
|
||||
if (curr_next_s_len > 1.0f) {
|
||||
cos_a_curr_next /= curr_next_s_len;
|
||||
}
|
||||
|
||||
/* feed forward position setpoint offset */
|
||||
math::Vector<3> pos_ff = prev_curr_s_norm *
|
||||
cos_a_curr_next * cos_b * cos_b * (1.0f - curr_pos_s_len) *
|
||||
(1.0f - expf(-curr_pos_s_len * curr_pos_s_len * 20.0f));
|
||||
pos_sp_s += pos_ff;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
bool near = cross_sphere_line(pos_s, 1.0f, prev_sp_s, curr_sp_s, pos_sp_s);
|
||||
if (near) {
|
||||
/* L1 sphere crosses trajectory */
|
||||
|
||||
} else {
|
||||
/* copter is too far from trajectory */
|
||||
/* if copter is behind prev waypoint, go directly to prev waypoint */
|
||||
if ((pos_sp_s - prev_sp_s) * prev_curr_s < 0.0f) {
|
||||
pos_sp_s = prev_sp_s;
|
||||
}
|
||||
|
||||
/* if copter is in front of curr waypoint, go directly to curr waypoint */
|
||||
if ((pos_sp_s - curr_sp_s) * prev_curr_s > 0.0f) {
|
||||
pos_sp_s = curr_sp_s;
|
||||
}
|
||||
|
||||
pos_sp_s = pos_s + (pos_sp_s - pos_s).normalized();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* move setpoint not faster than max allowed speed */
|
||||
math::Vector<3> pos_sp_old_s = _pos_sp.emult(scale);
|
||||
|
||||
/* difference between current and desired position setpoints, 1 = max speed */
|
||||
math::Vector<3> d_pos_m = (pos_sp_s - pos_sp_old_s).edivide(_params.pos_p);
|
||||
float d_pos_m_len = d_pos_m.length();
|
||||
if (d_pos_m_len > dt) {
|
||||
pos_sp_s = pos_sp_old_s + (d_pos_m / d_pos_m_len * dt).emult(_params.pos_p);
|
||||
}
|
||||
|
||||
/* scale result back to normal space */
|
||||
_pos_sp = pos_sp_s.edivide(scale);
|
||||
|
||||
/* update yaw setpoint if needed */
|
||||
if (isfinite(_pos_sp_triplet.current.yaw)) {
|
||||
_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
|
||||
}
|
||||
|
||||
} else {
|
||||
/* no waypoint, do nothing, setpoint was already reset */
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
MulticopterPositionControl::task_main()
|
||||
{
|
||||
|
@ -750,41 +957,16 @@ MulticopterPositionControl::task_main()
|
|||
if (_control_mode.flag_control_manual_enabled) {
|
||||
/* manual control */
|
||||
control_manual(dt);
|
||||
_mode_auto = false;
|
||||
|
||||
} else if (_control_mode.flag_control_offboard_enabled) {
|
||||
/* offboard control */
|
||||
control_offboard(dt);
|
||||
_mode_auto = false;
|
||||
|
||||
} else {
|
||||
/* AUTO */
|
||||
bool updated;
|
||||
orb_check(_pos_sp_triplet_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
|
||||
}
|
||||
|
||||
if (_pos_sp_triplet.current.valid) {
|
||||
/* in case of interrupted mission don't go to waypoint but stay at current position */
|
||||
_reset_pos_sp = true;
|
||||
_reset_alt_sp = true;
|
||||
|
||||
/* project setpoint to local frame */
|
||||
map_projection_project(&_ref_pos,
|
||||
_pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon,
|
||||
&_pos_sp.data[0], &_pos_sp.data[1]);
|
||||
_pos_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt);
|
||||
|
||||
/* update yaw setpoint if needed */
|
||||
if (isfinite(_pos_sp_triplet.current.yaw)) {
|
||||
_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
|
||||
}
|
||||
|
||||
} else {
|
||||
/* no waypoint, loiter, reset position setpoint if needed */
|
||||
reset_pos_sp();
|
||||
reset_alt_sp();
|
||||
}
|
||||
control_auto(dt);
|
||||
}
|
||||
|
||||
/* fill local position setpoint */
|
||||
|
@ -846,16 +1028,6 @@ MulticopterPositionControl::task_main()
|
|||
_vel_sp(2) = _params.land_speed;
|
||||
}
|
||||
|
||||
|
||||
if (!_control_mode.flag_control_manual_enabled) {
|
||||
/* limit 3D speed only in non-manual modes */
|
||||
float vel_sp_norm = _vel_sp.edivide(_params.vel_max).length();
|
||||
|
||||
if (vel_sp_norm > 1.0f) {
|
||||
_vel_sp /= vel_sp_norm;
|
||||
}
|
||||
}
|
||||
|
||||
_global_vel_sp.vx = _vel_sp(0);
|
||||
_global_vel_sp.vy = _vel_sp(1);
|
||||
_global_vel_sp.vz = _vel_sp(2);
|
||||
|
@ -1132,9 +1304,9 @@ MulticopterPositionControl::task_main()
|
|||
/* position controller disabled, reset setpoints */
|
||||
_reset_alt_sp = true;
|
||||
_reset_pos_sp = true;
|
||||
_mode_auto = false;
|
||||
reset_int_z = true;
|
||||
reset_int_xy = true;
|
||||
|
||||
}
|
||||
|
||||
/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */
|
||||
|
|
|
@ -37,6 +37,7 @@
|
|||
*
|
||||
* @author Julian Oes <julian@oes.ch>
|
||||
* @author Thomas Gubler <thomasgubler@gmail.com>
|
||||
* @author Anton Babushkin <anton.babushkin@me.com>
|
||||
*/
|
||||
|
||||
#include <sys/types.h>
|
||||
|
@ -113,6 +114,7 @@ Mission::on_inactive()
|
|||
update_offboard_mission();
|
||||
}
|
||||
|
||||
/* require takeoff after non-loiter or landing */
|
||||
if (!_navigator->get_can_loiter_at_sp() || _navigator->get_vstatus()->condition_landed) {
|
||||
_need_takeoff = true;
|
||||
}
|
||||
|
@ -147,8 +149,19 @@ Mission::on_active()
|
|||
|
||||
/* lets check if we reached the current mission item */
|
||||
if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) {
|
||||
advance_mission();
|
||||
set_mission_items();
|
||||
if (_mission_item.autocontinue) {
|
||||
/* switch to next waypoint if 'autocontinue' flag set */
|
||||
advance_mission();
|
||||
set_mission_items();
|
||||
|
||||
} else {
|
||||
/* else just report that item reached */
|
||||
if (_mission_type == MISSION_TYPE_OFFBOARD) {
|
||||
if (!(_navigator->get_mission_result()->seq_reached == _current_offboard_mission_index && _navigator->get_mission_result()->reached)) {
|
||||
set_mission_item_reached();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} else if (_mission_type != MISSION_TYPE_NONE &&_param_altmode.get() == MISSION_ALTMODE_FOH) {
|
||||
altitude_sp_foh_update();
|
||||
|
@ -221,7 +234,7 @@ Mission::update_offboard_mission()
|
|||
_current_offboard_mission_index = 0;
|
||||
}
|
||||
|
||||
report_current_offboard_mission_item();
|
||||
set_current_offboard_mission_item();
|
||||
}
|
||||
|
||||
|
||||
|
@ -359,6 +372,10 @@ Mission::set_mission_items()
|
|||
/* no mission available or mission finished, switch to loiter */
|
||||
if (_mission_type != MISSION_TYPE_NONE) {
|
||||
mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering");
|
||||
|
||||
/* use last setpoint for loiter */
|
||||
_navigator->set_can_loiter_at_sp(true);
|
||||
|
||||
} else if (!user_feedback_done) {
|
||||
/* only tell users that we got no mission if there has not been any
|
||||
* better, more specific feedback yet
|
||||
|
@ -375,10 +392,11 @@ Mission::set_mission_items()
|
|||
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
|
||||
pos_sp_triplet->next.valid = false;
|
||||
|
||||
/* reuse setpoint for LOITER only if it's not IDLE */
|
||||
_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER);
|
||||
|
||||
reset_mission_item_reached();
|
||||
report_mission_finished();
|
||||
set_mission_finished();
|
||||
|
||||
_navigator->set_position_setpoint_triplet_updated();
|
||||
return;
|
||||
|
@ -415,7 +433,7 @@ Mission::set_mission_items()
|
|||
takeoff_alt += _navigator->get_home_position()->alt;
|
||||
}
|
||||
|
||||
/* perform takeoff at least to NAV_TAKEOFF_ALT above home/ground, even if first waypoint is lower */
|
||||
/* takeoff to at least NAV_TAKEOFF_ALT above home/ground, even if first waypoint is lower */
|
||||
if (_navigator->get_vstatus()->condition_landed) {
|
||||
takeoff_alt = fmaxf(takeoff_alt, _navigator->get_global_position()->alt + _param_takeoff_alt.get());
|
||||
|
||||
|
@ -423,32 +441,46 @@ Mission::set_mission_items()
|
|||
takeoff_alt = fmaxf(takeoff_alt, _navigator->get_home_position()->alt + _param_takeoff_alt.get());
|
||||
}
|
||||
|
||||
mavlink_log_critical(_navigator->get_mavlink_fd(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt));
|
||||
/* check if we already above takeoff altitude */
|
||||
if (_navigator->get_global_position()->alt < takeoff_alt - _navigator->get_acceptance_radius()) {
|
||||
mavlink_log_critical(_navigator->get_mavlink_fd(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt));
|
||||
|
||||
_mission_item.lat = _navigator->get_global_position()->lat;
|
||||
_mission_item.lon = _navigator->get_global_position()->lon;
|
||||
_mission_item.altitude = takeoff_alt;
|
||||
_mission_item.altitude_is_relative = false;
|
||||
_mission_item.nav_cmd = NAV_CMD_TAKEOFF;
|
||||
_mission_item.lat = _navigator->get_global_position()->lat;
|
||||
_mission_item.lon = _navigator->get_global_position()->lon;
|
||||
_mission_item.altitude = takeoff_alt;
|
||||
_mission_item.altitude_is_relative = false;
|
||||
_mission_item.autocontinue = true;
|
||||
_mission_item.time_inside = 0;
|
||||
|
||||
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
|
||||
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
|
||||
|
||||
} else {
|
||||
/* set current position setpoint from mission item */
|
||||
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
|
||||
_navigator->set_position_setpoint_triplet_updated();
|
||||
return;
|
||||
|
||||
/* require takeoff after landing or idle */
|
||||
if (pos_sp_triplet->current.type == SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == SETPOINT_TYPE_IDLE) {
|
||||
_need_takeoff = true;
|
||||
} else {
|
||||
/* skip takeoff */
|
||||
_takeoff = false;
|
||||
}
|
||||
}
|
||||
|
||||
_navigator->set_can_loiter_at_sp(false);
|
||||
reset_mission_item_reached();
|
||||
/* set current position setpoint from mission item */
|
||||
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
|
||||
|
||||
if (_mission_type == MISSION_TYPE_OFFBOARD) {
|
||||
report_current_offboard_mission_item();
|
||||
}
|
||||
// TODO: report onboard mission item somehow
|
||||
/* require takeoff after landing or idle */
|
||||
if (pos_sp_triplet->current.type == SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == SETPOINT_TYPE_IDLE) {
|
||||
_need_takeoff = true;
|
||||
}
|
||||
|
||||
_navigator->set_can_loiter_at_sp(false);
|
||||
reset_mission_item_reached();
|
||||
|
||||
if (_mission_type == MISSION_TYPE_OFFBOARD) {
|
||||
set_current_offboard_mission_item();
|
||||
}
|
||||
// TODO: report onboard mission item somehow
|
||||
|
||||
if (_mission_item.autocontinue && _mission_item.time_inside <= 0.001f) {
|
||||
/* try to read next mission item */
|
||||
struct mission_item_s mission_item_next;
|
||||
|
||||
|
@ -460,6 +492,10 @@ Mission::set_mission_items()
|
|||
/* next mission item is not available */
|
||||
pos_sp_triplet->next.valid = false;
|
||||
}
|
||||
|
||||
} else {
|
||||
/* vehicle will be paused on current waypoint, don't set next item */
|
||||
pos_sp_triplet->next.valid = false;
|
||||
}
|
||||
|
||||
/* Save the distance between the current sp and the previous one */
|
||||
|
@ -666,19 +702,19 @@ Mission::save_offboard_mission_state()
|
|||
}
|
||||
|
||||
void
|
||||
Mission::report_mission_item_reached()
|
||||
Mission::set_mission_item_reached()
|
||||
{
|
||||
if (_mission_type == MISSION_TYPE_OFFBOARD) {
|
||||
_navigator->get_mission_result()->reached = true;
|
||||
_navigator->get_mission_result()->seq_reached = _current_offboard_mission_index;
|
||||
}
|
||||
_navigator->get_mission_result()->reached = true;
|
||||
_navigator->get_mission_result()->seq_reached = _current_offboard_mission_index;
|
||||
_navigator->publish_mission_result();
|
||||
}
|
||||
|
||||
void
|
||||
Mission::report_current_offboard_mission_item()
|
||||
Mission::set_current_offboard_mission_item()
|
||||
{
|
||||
warnx("current offboard mission index: %d", _current_offboard_mission_index);
|
||||
_navigator->get_mission_result()->reached = false;
|
||||
_navigator->get_mission_result()->finished = false;
|
||||
_navigator->get_mission_result()->seq_current = _current_offboard_mission_index;
|
||||
_navigator->publish_mission_result();
|
||||
|
||||
|
@ -686,9 +722,8 @@ Mission::report_current_offboard_mission_item()
|
|||
}
|
||||
|
||||
void
|
||||
Mission::report_mission_finished()
|
||||
Mission::set_mission_finished()
|
||||
{
|
||||
_navigator->get_mission_result()->finished = true;
|
||||
_navigator->publish_mission_result();
|
||||
}
|
||||
|
||||
|
|
|
@ -37,6 +37,7 @@
|
|||
*
|
||||
* @author Julian Oes <julian@oes.ch>
|
||||
* @author Thomas Gubler <thomasgubler@gmail.com>
|
||||
* @author Anton Babushkin <anton.babushkin@me.com>
|
||||
*/
|
||||
|
||||
#ifndef NAVIGATOR_MISSION_H
|
||||
|
@ -130,19 +131,19 @@ private:
|
|||
void save_offboard_mission_state();
|
||||
|
||||
/**
|
||||
* Report that a mission item has been reached
|
||||
* Set a mission item as reached
|
||||
*/
|
||||
void report_mission_item_reached();
|
||||
void set_mission_item_reached();
|
||||
|
||||
/**
|
||||
* Rport the current mission item
|
||||
* Set the current offboard mission item
|
||||
*/
|
||||
void report_current_offboard_mission_item();
|
||||
void set_current_offboard_mission_item();
|
||||
|
||||
/**
|
||||
* Report that the mission is finished if one exists or that none exists
|
||||
* Set that the mission is finished if one exists or that none exists
|
||||
*/
|
||||
void report_mission_finished();
|
||||
void set_mission_finished();
|
||||
|
||||
control::BlockParamInt _param_onboard_enabled;
|
||||
control::BlockParamFloat _param_takeoff_alt;
|
||||
|
@ -154,8 +155,8 @@ private:
|
|||
|
||||
int _current_onboard_mission_index;
|
||||
int _current_offboard_mission_index;
|
||||
bool _need_takeoff;
|
||||
bool _takeoff;
|
||||
bool _need_takeoff; /**< if true, then takeoff must be performed before going to the first waypoint (if needed) */
|
||||
bool _takeoff; /**< takeoff state flag */
|
||||
|
||||
enum {
|
||||
MISSION_TYPE_NONE,
|
||||
|
|
|
@ -84,7 +84,12 @@ bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_
|
|||
bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
|
||||
{
|
||||
|
||||
return (checkGeofence(dm_current, nMissionItems, geofence) && checkHomePositionAltitude(dm_current, nMissionItems, home_alt));
|
||||
/* Perform checks and issue feedback to the user for all checks */
|
||||
bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence);
|
||||
bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt);
|
||||
|
||||
/* Mission is only marked as feasible if all checks return true */
|
||||
return (resGeofence && resHomeAltitude);
|
||||
}
|
||||
|
||||
bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
|
||||
|
@ -93,7 +98,13 @@ bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_curre
|
|||
updateNavigationCapabilities();
|
||||
// warnx("_nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_horizontal_slope_displacement %.4f", _nav_caps.landing_slope_angle_rad, _nav_caps.landing_horizontal_slope_displacement);
|
||||
|
||||
return (checkFixedWingLanding(dm_current, nMissionItems) && checkGeofence(dm_current, nMissionItems, geofence) && checkHomePositionAltitude(dm_current, nMissionItems, home_alt));
|
||||
/* Perform checks and issue feedback to the user for all checks */
|
||||
bool resLanding = checkFixedWingLanding(dm_current, nMissionItems);
|
||||
bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence);
|
||||
bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt);
|
||||
|
||||
/* Mission is only marked as feasible if all checks return true */
|
||||
return (resLanding && resGeofence && resHomeAltitude);
|
||||
}
|
||||
|
||||
bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence)
|
||||
|
@ -216,9 +227,8 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size
|
|||
}
|
||||
}
|
||||
|
||||
|
||||
// float slope_alt = wp_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_distance)/_flare_constant) - _H1_virt;
|
||||
return false;
|
||||
/* No landing waypoints or no waypoints */
|
||||
return true;
|
||||
}
|
||||
|
||||
void MissionFeasibilityChecker::updateNavigationCapabilities()
|
||||
|
|
|
@ -46,7 +46,6 @@ SRCS = navigator_main.cpp \
|
|||
loiter.cpp \
|
||||
rtl.cpp \
|
||||
rtl_params.c \
|
||||
offboard.cpp \
|
||||
mission_feasibility_checker.cpp \
|
||||
geofence.cpp \
|
||||
geofence_params.c \
|
||||
|
@ -63,3 +62,5 @@ INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink
|
|||
MODULE_STACKSIZE = 1200
|
||||
|
||||
EXTRACXXFLAGS = -Weffc++
|
||||
|
||||
MAXOPTIMIZATION = -Os
|
||||
|
|
|
@ -60,7 +60,6 @@
|
|||
#include "mission.h"
|
||||
#include "loiter.h"
|
||||
#include "rtl.h"
|
||||
#include "offboard.h"
|
||||
#include "datalinkloss.h"
|
||||
#include "enginefailure.h"
|
||||
#include "gpsfailure.h"
|
||||
|
@ -70,7 +69,7 @@
|
|||
/**
|
||||
* Number of navigation modes that need on_active/on_inactive calls
|
||||
*/
|
||||
#define NAVIGATOR_MODE_ARRAY_SIZE 8
|
||||
#define NAVIGATOR_MODE_ARRAY_SIZE 7
|
||||
|
||||
class Navigator : public control::SuperBlock
|
||||
{
|
||||
|
@ -139,7 +138,6 @@ public:
|
|||
|
||||
int get_onboard_mission_sub() { return _onboard_mission_sub; }
|
||||
int get_offboard_mission_sub() { return _offboard_mission_sub; }
|
||||
int get_offboard_control_sp_sub() { return _offboard_control_sp_sub; }
|
||||
Geofence& get_geofence() { return _geofence; }
|
||||
bool get_can_loiter_at_sp() { return _can_loiter_at_sp; }
|
||||
float get_loiter_radius() { return _param_loiter_radius.get(); }
|
||||
|
@ -159,7 +157,6 @@ private:
|
|||
int _home_pos_sub; /**< home position subscription */
|
||||
int _vstatus_sub; /**< vehicle status subscription */
|
||||
int _capabilities_sub; /**< notification of vehicle capabilities updates */
|
||||
int _offboard_control_sp_sub; /*** offboard control subscription */
|
||||
int _control_mode_sub; /**< vehicle control mode subscription */
|
||||
int _onboard_mission_sub; /**< onboard mission subscription */
|
||||
int _offboard_mission_sub; /**< offboard mission subscription */
|
||||
|
@ -199,7 +196,6 @@ private:
|
|||
RTL _rtl; /**< class that handles RTL */
|
||||
RCLoss _rcLoss; /**< class that handles RTL according to
|
||||
OBC rules (rc loss mode) */
|
||||
Offboard _offboard; /**< class that handles offboard */
|
||||
DataLinkLoss _dataLinkLoss; /**< class that handles the OBC datalink loss mode */
|
||||
EngineFailure _engineFailure; /**< class that handles the engine failure mode
|
||||
(FW only!) */
|
||||
|
|
|
@ -68,7 +68,6 @@
|
|||
#include <uORB/topics/mission.h>
|
||||
#include <uORB/topics/fence.h>
|
||||
#include <uORB/topics/navigation_capabilities.h>
|
||||
#include <uORB/topics/offboard_control_setpoint.h>
|
||||
#include <drivers/drv_baro.h>
|
||||
|
||||
#include <systemlib/err.h>
|
||||
|
@ -87,6 +86,7 @@
|
|||
*/
|
||||
extern "C" __EXPORT int navigator_main(int argc, char *argv[]);
|
||||
|
||||
#define GEOFENCE_CHECK_INTERVAL 200000
|
||||
|
||||
namespace navigator
|
||||
{
|
||||
|
@ -104,7 +104,6 @@ Navigator::Navigator() :
|
|||
_home_pos_sub(-1),
|
||||
_vstatus_sub(-1),
|
||||
_capabilities_sub(-1),
|
||||
_offboard_control_sp_sub(-1),
|
||||
_control_mode_sub(-1),
|
||||
_onboard_mission_sub(-1),
|
||||
_offboard_mission_sub(-1),
|
||||
|
@ -133,7 +132,6 @@ Navigator::Navigator() :
|
|||
_loiter(this, "LOI"),
|
||||
_rtl(this, "RTL"),
|
||||
_rcLoss(this, "RCL"),
|
||||
_offboard(this, "OFF"),
|
||||
_dataLinkLoss(this, "DLL"),
|
||||
_engineFailure(this, "EF"),
|
||||
_gpsFailure(this, "GPSF"),
|
||||
|
@ -148,11 +146,10 @@ Navigator::Navigator() :
|
|||
_navigation_mode_array[0] = &_mission;
|
||||
_navigation_mode_array[1] = &_loiter;
|
||||
_navigation_mode_array[2] = &_rtl;
|
||||
_navigation_mode_array[3] = &_offboard;
|
||||
_navigation_mode_array[4] = &_dataLinkLoss;
|
||||
_navigation_mode_array[5] = &_engineFailure;
|
||||
_navigation_mode_array[6] = &_gpsFailure;
|
||||
_navigation_mode_array[7] = &_rcLoss;
|
||||
_navigation_mode_array[3] = &_dataLinkLoss;
|
||||
_navigation_mode_array[4] = &_engineFailure;
|
||||
_navigation_mode_array[5] = &_gpsFailure;
|
||||
_navigation_mode_array[6] = &_rcLoss;
|
||||
|
||||
updateParams();
|
||||
}
|
||||
|
@ -281,7 +278,6 @@ Navigator::task_main()
|
|||
_onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission));
|
||||
_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
|
||||
_param_update_sub = orb_subscribe(ORB_ID(parameter_update));
|
||||
_offboard_control_sp_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
|
||||
|
||||
/* copy all topics first time */
|
||||
vehicle_status_update();
|
||||
|
@ -343,7 +339,7 @@ Navigator::task_main()
|
|||
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
|
||||
}
|
||||
|
||||
bool have_geofence_position_data = false;
|
||||
static bool have_geofence_position_data = false;
|
||||
|
||||
/* gps updated */
|
||||
if (fds[7].revents & POLLIN) {
|
||||
|
@ -393,8 +389,11 @@ Navigator::task_main()
|
|||
}
|
||||
|
||||
/* Check geofence violation */
|
||||
if (have_geofence_position_data) {
|
||||
static hrt_abstime last_geofence_check = 0;
|
||||
if (have_geofence_position_data && hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL) {
|
||||
bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter);
|
||||
last_geofence_check = hrt_absolute_time();
|
||||
have_geofence_position_data = false;
|
||||
if (!inside) {
|
||||
/* inform other apps via the mission result */
|
||||
_mission_result.geofence_violated = true;
|
||||
|
@ -422,13 +421,11 @@ Navigator::task_main()
|
|||
case NAVIGATION_STATE_POSCTL:
|
||||
case NAVIGATION_STATE_LAND:
|
||||
case NAVIGATION_STATE_TERMINATION:
|
||||
case NAVIGATION_STATE_OFFBOARD:
|
||||
_navigation_mode = nullptr;
|
||||
_can_loiter_at_sp = false;
|
||||
break;
|
||||
case NAVIGATION_STATE_AUTO_MISSION:
|
||||
/* Some failsafe modes prohibit the fallback to mission
|
||||
* usually this is done after some time to make sure
|
||||
* that the full failsafe operation is performed */
|
||||
_navigation_mode = &_mission;
|
||||
break;
|
||||
case NAVIGATION_STATE_AUTO_LOITER:
|
||||
|
@ -444,7 +441,7 @@ Navigator::task_main()
|
|||
case NAVIGATION_STATE_AUTO_RTL:
|
||||
_navigation_mode = &_rtl;
|
||||
break;
|
||||
case NAVIGATION_STATE_AUTO_RTGS: //XXX OBC: differentiate between rc loss and dl loss here
|
||||
case NAVIGATION_STATE_AUTO_RTGS:
|
||||
/* Use complex data link loss mode only when enabled via param
|
||||
* otherwise use rtl */
|
||||
if (_param_datalinkloss_obc.get() != 0) {
|
||||
|
@ -459,9 +456,6 @@ Navigator::task_main()
|
|||
case NAVIGATION_STATE_AUTO_LANDGPSFAIL:
|
||||
_navigation_mode = &_gpsFailure;
|
||||
break;
|
||||
case NAVIGATION_STATE_OFFBOARD:
|
||||
_navigation_mode = &_offboard;
|
||||
break;
|
||||
default:
|
||||
_navigation_mode = nullptr;
|
||||
_can_loiter_at_sp = false;
|
||||
|
|
|
@ -1,129 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
/**
|
||||
* @file offboard.cpp
|
||||
*
|
||||
* Helper class for offboard commands
|
||||
*
|
||||
* @author Julian Oes <julian@oes.ch>
|
||||
*/
|
||||
|
||||
#include <string.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdbool.h>
|
||||
#include <math.h>
|
||||
#include <fcntl.h>
|
||||
|
||||
#include <mavlink/mavlink_log.h>
|
||||
#include <systemlib/err.h>
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/position_setpoint_triplet.h>
|
||||
|
||||
#include "navigator.h"
|
||||
#include "offboard.h"
|
||||
|
||||
Offboard::Offboard(Navigator *navigator, const char *name) :
|
||||
NavigatorMode(navigator, name),
|
||||
_offboard_control_sp({0})
|
||||
{
|
||||
/* load initial params */
|
||||
updateParams();
|
||||
/* initial reset */
|
||||
on_inactive();
|
||||
}
|
||||
|
||||
Offboard::~Offboard()
|
||||
{
|
||||
}
|
||||
|
||||
void
|
||||
Offboard::on_inactive()
|
||||
{
|
||||
}
|
||||
|
||||
void
|
||||
Offboard::on_activation()
|
||||
{
|
||||
}
|
||||
|
||||
void
|
||||
Offboard::on_active()
|
||||
{
|
||||
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
|
||||
|
||||
bool updated;
|
||||
orb_check(_navigator->get_offboard_control_sp_sub(), &updated);
|
||||
if (updated) {
|
||||
update_offboard_control_setpoint();
|
||||
}
|
||||
|
||||
/* copy offboard setpoints to the corresponding topics */
|
||||
if (_navigator->get_control_mode()->flag_control_position_enabled
|
||||
&& _offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION) {
|
||||
/* position control */
|
||||
pos_sp_triplet->current.x = _offboard_control_sp.p1;
|
||||
pos_sp_triplet->current.y = _offboard_control_sp.p2;
|
||||
pos_sp_triplet->current.yaw = _offboard_control_sp.p3;
|
||||
pos_sp_triplet->current.z = -_offboard_control_sp.p4;
|
||||
|
||||
pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD;
|
||||
pos_sp_triplet->current.valid = true;
|
||||
pos_sp_triplet->current.position_valid = true;
|
||||
|
||||
_navigator->set_position_setpoint_triplet_updated();
|
||||
|
||||
} else if (_navigator->get_control_mode()->flag_control_velocity_enabled
|
||||
&& _offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY) {
|
||||
/* velocity control */
|
||||
pos_sp_triplet->current.vx = _offboard_control_sp.p2;
|
||||
pos_sp_triplet->current.vy = _offboard_control_sp.p1;
|
||||
pos_sp_triplet->current.yawspeed = _offboard_control_sp.p3;
|
||||
pos_sp_triplet->current.vz = _offboard_control_sp.p4;
|
||||
|
||||
pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD;
|
||||
pos_sp_triplet->current.valid = true;
|
||||
pos_sp_triplet->current.velocity_valid = true;
|
||||
|
||||
_navigator->set_position_setpoint_triplet_updated();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
Offboard::update_offboard_control_setpoint()
|
||||
{
|
||||
orb_copy(ORB_ID(offboard_control_setpoint), _navigator->get_offboard_control_sp_sub(), &_offboard_control_sp);
|
||||
|
||||
}
|
|
@ -106,7 +106,7 @@ PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0);
|
|||
* @max 121212
|
||||
* @group Circuit Breaker
|
||||
*/
|
||||
PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 0);
|
||||
PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 121212);
|
||||
|
||||
/**
|
||||
* Circuit breaker for engine failure detection
|
||||
|
@ -120,7 +120,21 @@ PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 0);
|
|||
* @max 284953
|
||||
* @group Circuit Breaker
|
||||
*/
|
||||
PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 0);
|
||||
PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 284953);
|
||||
|
||||
/**
|
||||
* Circuit breaker for gps failure detection
|
||||
*
|
||||
* Setting this parameter to 240024 will disable the gps failure detection.
|
||||
* If the aircraft is in gps failure mode the gps failure flag will be
|
||||
* set to healthy
|
||||
* WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK
|
||||
*
|
||||
* @min 0
|
||||
* @max 240024
|
||||
* @group Circuit Breaker
|
||||
*/
|
||||
PARAM_DEFINE_INT32(CBRK_GPSFAIL, 240024);
|
||||
|
||||
bool circuit_breaker_enabled(const char* breaker, int32_t magic)
|
||||
{
|
||||
|
|
|
@ -55,6 +55,7 @@
|
|||
#define CBRK_AIRSPD_CHK_KEY 162128
|
||||
#define CBRK_FLIGHTTERM_KEY 121212
|
||||
#define CBRK_ENGINEFAIL_KEY 284953
|
||||
#define CBRK_GPSFAIL_KEY 240024
|
||||
|
||||
#include <stdbool.h>
|
||||
|
||||
|
|
|
@ -130,6 +130,9 @@
|
|||
|
||||
#include <nuttx/config.h>
|
||||
#include "drivers/drv_mixer.h"
|
||||
|
||||
#include <uORB/topics/multirotor_motor_limits.h>
|
||||
|
||||
#include "mixer_load.h"
|
||||
|
||||
/**
|
||||
|
@ -531,6 +534,9 @@ private:
|
|||
float _yaw_scale;
|
||||
float _idle_speed;
|
||||
|
||||
orb_advert_t _limits_pub;
|
||||
multirotor_motor_limits_s _limits;
|
||||
|
||||
unsigned _rotor_count;
|
||||
const Rotor *_rotors;
|
||||
|
||||
|
|
|
@ -36,7 +36,8 @@
|
|||
*
|
||||
* Multi-rotor mixers.
|
||||
*/
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/multirotor_motor_limits.h>
|
||||
#include <nuttx/config.h>
|
||||
|
||||
#include <sys/types.h>
|
||||
|
@ -302,6 +303,11 @@ MultirotorMixer::mix(float *outputs, unsigned space)
|
|||
float min_out = 0.0f;
|
||||
float max_out = 0.0f;
|
||||
|
||||
_limits.roll_pitch = false;
|
||||
_limits.yaw = false;
|
||||
_limits.throttle_upper = false;
|
||||
_limits.throttle_lower = false;
|
||||
|
||||
/* perform initial mix pass yielding unbounded outputs, ignore yaw */
|
||||
for (unsigned i = 0; i < _rotor_count; i++) {
|
||||
float out = roll * _rotors[i].roll_scale +
|
||||
|
@ -311,6 +317,7 @@ MultirotorMixer::mix(float *outputs, unsigned space)
|
|||
/* limit yaw if it causes outputs clipping */
|
||||
if (out >= 0.0f && out < -yaw * _rotors[i].yaw_scale) {
|
||||
yaw = -out / _rotors[i].yaw_scale;
|
||||
_limits.yaw = true;
|
||||
}
|
||||
|
||||
/* calculate min and max output values */
|
||||
|
@ -332,6 +339,7 @@ MultirotorMixer::mix(float *outputs, unsigned space)
|
|||
for (unsigned i = 0; i < _rotor_count; i++) {
|
||||
outputs[i] = scale_in * (roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) + thrust;
|
||||
}
|
||||
_limits.roll_pitch = true;
|
||||
|
||||
} else {
|
||||
/* roll/pitch mixed without limiting, add yaw control */
|
||||
|
@ -344,6 +352,7 @@ MultirotorMixer::mix(float *outputs, unsigned space)
|
|||
float scale_out;
|
||||
if (max_out > 1.0f) {
|
||||
scale_out = 1.0f / max_out;
|
||||
_limits.throttle_upper = true;
|
||||
|
||||
} else {
|
||||
scale_out = 1.0f;
|
||||
|
@ -351,9 +360,20 @@ MultirotorMixer::mix(float *outputs, unsigned space)
|
|||
|
||||
/* scale outputs to range _idle_speed..1, and do final limiting */
|
||||
for (unsigned i = 0; i < _rotor_count; i++) {
|
||||
if (outputs[i] < _idle_speed) {
|
||||
_limits.throttle_lower = true;
|
||||
}
|
||||
outputs[i] = constrain(_idle_speed + (outputs[i] * (1.0f - _idle_speed) * scale_out), _idle_speed, 1.0f);
|
||||
}
|
||||
|
||||
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) || defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
|
||||
/* publish/advertise motor limits if running on FMU */
|
||||
if (_limits_pub > 0) {
|
||||
orb_publish(ORB_ID(multirotor_motor_limits), _limits_pub, &_limits);
|
||||
} else {
|
||||
_limits_pub = orb_advertise(ORB_ID(multirotor_motor_limits), &_limits);
|
||||
}
|
||||
#endif
|
||||
return _rotor_count;
|
||||
}
|
||||
|
||||
|
|
|
@ -322,7 +322,8 @@ param_get_value_ptr(param_t param)
|
|||
v = ¶m_info_base[param].val;
|
||||
}
|
||||
|
||||
if (param_type(param) == PARAM_TYPE_STRUCT) {
|
||||
if (param_type(param) >= PARAM_TYPE_STRUCT
|
||||
&& param_type(param) <= PARAM_TYPE_STRUCT_MAX) {
|
||||
result = v->p;
|
||||
|
||||
} else {
|
||||
|
|
|
@ -307,7 +307,7 @@ __EXPORT int param_load_default(void);
|
|||
struct param_info_s __param__##_name = { \
|
||||
#_name, \
|
||||
PARAM_TYPE_STRUCT + sizeof(_default), \
|
||||
.val.p = &_default; \
|
||||
.val.p = &_default \
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
|
@ -192,6 +192,9 @@ ORB_DEFINE(actuator_outputs_1, struct actuator_outputs_s);
|
|||
ORB_DEFINE(actuator_outputs_2, struct actuator_outputs_s);
|
||||
ORB_DEFINE(actuator_outputs_3, struct actuator_outputs_s);
|
||||
|
||||
#include "topics/multirotor_motor_limits.h"
|
||||
ORB_DEFINE(multirotor_motor_limits, struct multirotor_motor_limits_s);
|
||||
|
||||
#include "topics/telemetry_status.h"
|
||||
ORB_DEFINE(telemetry_status_0, struct telemetry_status_s);
|
||||
ORB_DEFINE(telemetry_status_1, struct telemetry_status_s);
|
||||
|
|
|
@ -1,6 +1,6 @@
|
|||
/***************************************************************************
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
|
||||
* Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
|
@ -30,43 +30,40 @@
|
|||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file offboard.h
|
||||
* @file multirotor_motor_limits.h
|
||||
*
|
||||
* Helper class for offboard commands
|
||||
*
|
||||
* @author Julian Oes <julian@oes.ch>
|
||||
* Definition of multirotor_motor_limits topic
|
||||
*/
|
||||
|
||||
#ifndef NAVIGATOR_OFFBOARD_H
|
||||
#define NAVIGATOR_OFFBOARD_H
|
||||
#ifndef MULTIROTOR_MOTOR_LIMITS_H_
|
||||
#define MULTIROTOR_MOTOR_LIMITS_H_
|
||||
|
||||
#include <controllib/blocks.hpp>
|
||||
#include <controllib/block/BlockParam.hpp>
|
||||
#include "../uORB.h"
|
||||
#include <stdint.h>
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/offboard_control_setpoint.h>
|
||||
/**
|
||||
* @addtogroup topics
|
||||
* @{
|
||||
*/
|
||||
|
||||
#include "navigator_mode.h"
|
||||
|
||||
class Navigator;
|
||||
|
||||
class Offboard : public NavigatorMode
|
||||
{
|
||||
public:
|
||||
Offboard(Navigator *navigator, const char *name);
|
||||
|
||||
~Offboard();
|
||||
|
||||
virtual void on_inactive();
|
||||
|
||||
virtual void on_activation();
|
||||
|
||||
virtual void on_active();
|
||||
private:
|
||||
void update_offboard_control_setpoint();
|
||||
|
||||
struct offboard_control_setpoint_s _offboard_control_sp;
|
||||
/**
|
||||
* Motor limits
|
||||
*/
|
||||
struct multirotor_motor_limits_s {
|
||||
uint8_t roll_pitch : 1; // roll/pitch limit reached
|
||||
uint8_t yaw : 1; // yaw limit reached
|
||||
uint8_t throttle_lower : 1; // lower throttle limit reached
|
||||
uint8_t throttle_upper : 1; // upper throttle limit reached
|
||||
uint8_t reserved : 4;
|
||||
};
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
/* register this as object request broker structure */
|
||||
ORB_DECLARE(multirotor_motor_limits);
|
||||
|
||||
#endif
|
|
@ -53,11 +53,42 @@ enum OFFBOARD_CONTROL_MODE {
|
|||
OFFBOARD_CONTROL_MODE_DIRECT = 0,
|
||||
OFFBOARD_CONTROL_MODE_DIRECT_RATES = 1,
|
||||
OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE = 2,
|
||||
OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY = 3,
|
||||
OFFBOARD_CONTROL_MODE_DIRECT_POSITION = 4,
|
||||
OFFBOARD_CONTROL_MODE_ATT_YAW_RATE = 5,
|
||||
OFFBOARD_CONTROL_MODE_ATT_YAW_POS = 6,
|
||||
OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 7, /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */
|
||||
OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED = 3,
|
||||
OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED = 4,
|
||||
OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED = 5,
|
||||
OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED = 6,
|
||||
OFFBOARD_CONTROL_MODE_DIRECT_GLOBAL = 7,
|
||||
OFFBOARD_CONTROL_MODE_DIRECT_FORCE = 8,
|
||||
OFFBOARD_CONTROL_MODE_ATT_YAW_RATE = 9,
|
||||
OFFBOARD_CONTROL_MODE_ATT_YAW_POS = 10,
|
||||
OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 11 /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */
|
||||
};
|
||||
|
||||
enum OFFBOARD_CONTROL_FRAME {
|
||||
OFFBOARD_CONTROL_FRAME_LOCAL_NED = 0,
|
||||
OFFBOARD_CONTROL_FRAME_LOCAL_OFFSET_NED = 1,
|
||||
OFFBOARD_CONTROL_FRAME_BODY_NED = 2,
|
||||
OFFBOARD_CONTROL_FRAME_BODY_OFFSET_NED = 3,
|
||||
OFFBOARD_CONTROL_FRAME_GLOBAL = 4
|
||||
};
|
||||
|
||||
/* mappings for the ignore bitmask */
|
||||
enum {OFB_IGN_BIT_POS_X,
|
||||
OFB_IGN_BIT_POS_Y,
|
||||
OFB_IGN_BIT_POS_Z,
|
||||
OFB_IGN_BIT_VEL_X,
|
||||
OFB_IGN_BIT_VEL_Y,
|
||||
OFB_IGN_BIT_VEL_Z,
|
||||
OFB_IGN_BIT_ACC_X,
|
||||
OFB_IGN_BIT_ACC_Y,
|
||||
OFB_IGN_BIT_ACC_Z,
|
||||
OFB_IGN_BIT_BODYRATE_X,
|
||||
OFB_IGN_BIT_BODYRATE_Y,
|
||||
OFB_IGN_BIT_BODYRATE_Z,
|
||||
OFB_IGN_BIT_ATT,
|
||||
OFB_IGN_BIT_THRUST,
|
||||
OFB_IGN_BIT_YAW,
|
||||
OFB_IGN_BIT_YAWRATE,
|
||||
};
|
||||
|
||||
/**
|
||||
|
@ -70,10 +101,21 @@ struct offboard_control_setpoint_s {
|
|||
|
||||
enum OFFBOARD_CONTROL_MODE mode; /**< The current control inputs mode */
|
||||
|
||||
float p1; /**< ailerons roll / roll rate input */
|
||||
float p2; /**< elevator / pitch / pitch rate */
|
||||
float p3; /**< rudder / yaw rate / yaw */
|
||||
float p4; /**< throttle / collective thrust / altitude */
|
||||
double position[3]; /**< lat, lon, alt / x, y, z */
|
||||
float velocity[3]; /**< x vel, y vel, z vel */
|
||||
float acceleration[3]; /**< x acc, y acc, z acc */
|
||||
float attitude[4]; /**< attitude of vehicle (quaternion) */
|
||||
float attitude_rate[3]; /**< body angular rates (x, y, z) */
|
||||
float thrust; /**< thrust */
|
||||
float yaw; /**< yaw: this is the yaw from the position_target message
|
||||
(not from the full attitude_target message) */
|
||||
float yaw_rate; /**< yaw rate: this is the yaw from the position_target message
|
||||
(not from the full attitude_target message) */
|
||||
|
||||
uint16_t ignore; /**< if field i is set to true, the value should be ignored, see definition at top of file
|
||||
for mapping */
|
||||
|
||||
bool isForceSetpoint; /**< the acceleration vector should be interpreted as force */
|
||||
|
||||
float override_mode_switch;
|
||||
|
||||
|
@ -87,6 +129,147 @@ struct offboard_control_setpoint_s {
|
|||
* @}
|
||||
*/
|
||||
|
||||
/**
|
||||
* Returns true if the position setpoint at index should be ignored
|
||||
*/
|
||||
inline bool offboard_control_sp_ignore_position(const struct offboard_control_setpoint_s &offboard_control_sp, int index) {
|
||||
return (bool)(offboard_control_sp.ignore & (1 << (OFB_IGN_BIT_POS_X + index)));
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns true if all position setpoints should be ignored
|
||||
*/
|
||||
inline bool offboard_control_sp_ignore_position_all(const struct offboard_control_setpoint_s &offboard_control_sp) {
|
||||
for (int i = 0; i < 3; i++) {
|
||||
if (!offboard_control_sp_ignore_position(offboard_control_sp, i)) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns true if some position setpoints should be ignored
|
||||
*/
|
||||
inline bool offboard_control_sp_ignore_position_some(const struct offboard_control_setpoint_s &offboard_control_sp) {
|
||||
for (int i = 0; i < 3; i++) {
|
||||
if (offboard_control_sp_ignore_position(offboard_control_sp, i)) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns true if the velocity setpoint at index should be ignored
|
||||
*/
|
||||
inline bool offboard_control_sp_ignore_velocity(const struct offboard_control_setpoint_s &offboard_control_sp, int index) {
|
||||
return (bool)(offboard_control_sp.ignore & (1 << (OFB_IGN_BIT_VEL_X + index)));
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns true if all velocity setpoints should be ignored
|
||||
*/
|
||||
inline bool offboard_control_sp_ignore_velocity_all(const struct offboard_control_setpoint_s &offboard_control_sp) {
|
||||
for (int i = 0; i < 3; i++) {
|
||||
if (!offboard_control_sp_ignore_velocity(offboard_control_sp, i)) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns true if some velocity setpoints should be ignored
|
||||
*/
|
||||
inline bool offboard_control_sp_ignore_velocity_some(const struct offboard_control_setpoint_s &offboard_control_sp) {
|
||||
for (int i = 0; i < 3; i++) {
|
||||
if (offboard_control_sp_ignore_velocity(offboard_control_sp, i)) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns true if the acceleration setpoint at index should be ignored
|
||||
*/
|
||||
inline bool offboard_control_sp_ignore_acceleration(const struct offboard_control_setpoint_s &offboard_control_sp, int index) {
|
||||
return (bool)(offboard_control_sp.ignore & (1 << (OFB_IGN_BIT_ACC_X + index)));
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns true if all acceleration setpoints should be ignored
|
||||
*/
|
||||
inline bool offboard_control_sp_ignore_acceleration_all(const struct offboard_control_setpoint_s &offboard_control_sp) {
|
||||
for (int i = 0; i < 3; i++) {
|
||||
if (!offboard_control_sp_ignore_acceleration(offboard_control_sp, i)) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns true if some acceleration setpoints should be ignored
|
||||
*/
|
||||
inline bool offboard_control_sp_ignore_acceleration_some(const struct offboard_control_setpoint_s &offboard_control_sp) {
|
||||
for (int i = 0; i < 3; i++) {
|
||||
if (offboard_control_sp_ignore_acceleration(offboard_control_sp, i)) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns true if the bodyrate setpoint at index should be ignored
|
||||
*/
|
||||
inline bool offboard_control_sp_ignore_bodyrates(const struct offboard_control_setpoint_s &offboard_control_sp, int index) {
|
||||
return (bool)(offboard_control_sp.ignore & (1 << (OFB_IGN_BIT_BODYRATE_X + index)));
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns true if some of the bodyrate setpoints should be ignored
|
||||
*/
|
||||
inline bool offboard_control_sp_ignore_bodyrates_some(const struct offboard_control_setpoint_s &offboard_control_sp) {
|
||||
for (int i = 0; i < 3; i++) {
|
||||
if (offboard_control_sp_ignore_bodyrates(offboard_control_sp, i)) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns true if the attitude setpoint should be ignored
|
||||
*/
|
||||
inline bool offboard_control_sp_ignore_attitude(const struct offboard_control_setpoint_s &offboard_control_sp) {
|
||||
return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_ATT));
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns true if the thrust setpoint should be ignored
|
||||
*/
|
||||
inline bool offboard_control_sp_ignore_thrust(const struct offboard_control_setpoint_s &offboard_control_sp) {
|
||||
return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_THRUST));
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns true if the yaw setpoint should be ignored
|
||||
*/
|
||||
inline bool offboard_control_sp_ignore_yaw(const struct offboard_control_setpoint_s &offboard_control_sp) {
|
||||
return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_YAW));
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns true if the yaw rate setpoint should be ignored
|
||||
*/
|
||||
inline bool offboard_control_sp_ignore_yawrate(const struct offboard_control_setpoint_s &offboard_control_sp) {
|
||||
return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_YAWRATE));
|
||||
}
|
||||
|
||||
|
||||
/* register this as object request broker structure */
|
||||
ORB_DECLARE(offboard_control_setpoint);
|
||||
|
||||
|
|
|
@ -79,10 +79,17 @@ struct position_setpoint_s
|
|||
double lon; /**< longitude, in deg */
|
||||
float alt; /**< altitude AMSL, in m */
|
||||
float yaw; /**< yaw (only for multirotors), in rad [-PI..PI), NaN = hold current yaw */
|
||||
bool yaw_valid; /**< true if yaw setpoint valid */
|
||||
float yawspeed; /**< yawspeed (only for multirotors, in rad/s) */
|
||||
bool yawspeed_valid; /**< true if yawspeed setpoint valid */
|
||||
float loiter_radius; /**< loiter radius (only for fixed wing), in m */
|
||||
int8_t loiter_direction; /**< loiter direction: 1 = CW, -1 = CCW */
|
||||
float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */
|
||||
float a_x; //**< acceleration x setpoint */
|
||||
float a_y; //**< acceleration y setpoint */
|
||||
float a_z; //**< acceleration z setpoint */
|
||||
bool acceleration_valid; //*< true if acceleration setpoint is valid/should be used */
|
||||
bool acceleration_is_force; //*< interprete acceleration as force */
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
|
@ -65,6 +65,9 @@ enum VEHICLE_CMD {
|
|||
VEHICLE_CMD_NAV_TAKEOFF = 22, /* Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| */
|
||||
VEHICLE_CMD_NAV_ROI = 80, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */
|
||||
VEHICLE_CMD_NAV_PATHPLANNING = 81, /* Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */
|
||||
VEHICLE_CMD_NAV_GUIDED_LIMITS=90, /* set limits for external control |timeout - maximum time (in seconds) that external controller will be allowed to control vehicle. 0 means no timeout| absolute altitude min (in meters, WGS84) - if vehicle moves below this alt, the command will be aborted and the mission will continue. 0 means no lower altitude limit| absolute altitude max (in meters)- if vehicle moves above this alt, the command will be aborted and the mission will continue. 0 means no upper altitude limit| horizontal move limit (in meters, WGS84) - if vehicle moves more than this distance from it's location at the moment the command was executed, the command will be aborted and the mission will continue. 0 means no horizontal altitude limit| Empty| Empty| Empty| */
|
||||
VEHICLE_CMD_NAV_GUIDED_MASTER=91, /* set id of master controller |System ID| Component ID| Empty| Empty| Empty| Empty| Empty| */
|
||||
VEHICLE_CMD_NAV_GUIDED_ENABLE=92, /* hand control over to an external controller |On / Off (> 0.5f on)| Empty| Empty| Empty| Empty| Empty| Empty| */
|
||||
VEHICLE_CMD_NAV_LAST = 95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */
|
||||
VEHICLE_CMD_CONDITION_DELAY = 112, /* Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty| */
|
||||
VEHICLE_CMD_CONDITION_CHANGE_ALT = 113, /* Ascend/descend at rate. Delay mission state machine until desired altitude reached. |Descent / Ascend rate (m/s)| Empty| Empty| Empty| Empty| Empty| Finish Altitude| */
|
||||
|
|
|
@ -77,6 +77,7 @@ struct vehicle_control_mode_s {
|
|||
bool flag_control_offboard_enabled; /**< true if offboard control should be used */
|
||||
bool flag_control_rates_enabled; /**< true if rates are stabilized */
|
||||
bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */
|
||||
bool flag_control_force_enabled; /**< true if force control is mixed in */
|
||||
bool flag_control_velocity_enabled; /**< true if horizontal velocity (implies direction) is controlled */
|
||||
bool flag_control_position_enabled; /**< true if position is controlled */
|
||||
bool flag_control_altitude_enabled; /**< true if altitude is controlled */
|
||||
|
|
|
@ -218,6 +218,8 @@ struct vehicle_status_s {
|
|||
bool offboard_control_signal_lost;
|
||||
bool offboard_control_signal_weak;
|
||||
uint64_t offboard_control_signal_lost_interval; /**< interval in microseconds without an offboard control message */
|
||||
bool offboard_control_set_by_command; /**< true if the offboard mode was set by a mavlink command
|
||||
and should not be overridden by RC */
|
||||
|
||||
/* see SYS_STATUS mavlink message for the following */
|
||||
uint32_t onboard_control_sensors_present;
|
||||
|
@ -239,6 +241,8 @@ struct vehicle_status_s {
|
|||
|
||||
bool circuit_breaker_engaged_power_check;
|
||||
bool circuit_breaker_engaged_airspd_check;
|
||||
bool circuit_breaker_engaged_enginefailure_check;
|
||||
bool circuit_breaker_engaged_gpsfailure_check;
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
|
@ -43,8 +43,6 @@
|
|||
#include <systemlib/err.h>
|
||||
#include <mathlib/mathlib.h>
|
||||
|
||||
#define MM_PER_CM 10 // Millimeters per centimeter
|
||||
|
||||
const char *const UavcanGnssBridge::NAME = "gnss";
|
||||
|
||||
UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) :
|
||||
|
@ -95,9 +93,9 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavca
|
|||
auto report = ::vehicle_gps_position_s();
|
||||
|
||||
report.timestamp_position = hrt_absolute_time();
|
||||
report.lat = msg.lat_1e7;
|
||||
report.lon = msg.lon_1e7;
|
||||
report.alt = msg.alt_1e2 * MM_PER_CM; // Convert from centimeter (1e2) to millimeters (1e3)
|
||||
report.lat = msg.latitude_deg_1e8 / 10;
|
||||
report.lon = msg.longitude_deg_1e8 / 10;
|
||||
report.alt = msg.height_msl_mm;
|
||||
|
||||
report.timestamp_variance = report.timestamp_position;
|
||||
|
||||
|
|
|
@ -36,7 +36,11 @@
|
|||
|
||||
#include <systemlib/err.h>
|
||||
|
||||
UnitTest::UnitTest()
|
||||
UnitTest::UnitTest() :
|
||||
_tests_run(0),
|
||||
_tests_failed(0),
|
||||
_tests_passed(0),
|
||||
_assertions(0)
|
||||
{
|
||||
}
|
||||
|
||||
|
@ -44,15 +48,22 @@ UnitTest::~UnitTest()
|
|||
{
|
||||
}
|
||||
|
||||
void UnitTest::printResults(void)
|
||||
void UnitTest::print_results(void)
|
||||
{
|
||||
warnx(mu_tests_failed() ? "SOME TESTS FAILED" : "ALL TESTS PASSED");
|
||||
warnx(" Tests passed : %d", mu_tests_passed());
|
||||
warnx(" Tests failed : %d", mu_tests_failed());
|
||||
warnx(" Assertions : %d", mu_assertion());
|
||||
warnx(_tests_failed ? "SOME TESTS FAILED" : "ALL TESTS PASSED");
|
||||
warnx(" Tests passed : %d", _tests_passed);
|
||||
warnx(" Tests failed : %d", _tests_failed);
|
||||
warnx(" Assertions : %d", _assertions);
|
||||
}
|
||||
|
||||
void UnitTest::printAssert(const char* msg, const char* test, const char* file, int line)
|
||||
/// @brief Used internally to the ut_assert macro to print assert failures.
|
||||
void UnitTest::_print_assert(const char* msg, const char* test, const char* file, int line)
|
||||
{
|
||||
warnx("Assertion failed: %s - %s (%s:%d)", msg, test, file, line);
|
||||
warnx("Assertion failed: %s - %s (%s:%d)", msg, test, file, line);
|
||||
}
|
||||
|
||||
/// @brief Used internally to the ut_compare macro to print assert failures.
|
||||
void UnitTest::_print_compare(const char* msg, const char *v1_text, int v1, const char *v2_text, int v2, const char* file, int line)
|
||||
{
|
||||
warnx("Compare failed: %s - (%s:%d) (%s:%d) (%s:%d)", msg, v1_text, v1, v2_text, v2, file, line);
|
||||
}
|
||||
|
|
|
@ -37,50 +37,85 @@
|
|||
|
||||
#include <systemlib/err.h>
|
||||
|
||||
/// @brief Base class to be used for unit tests.
|
||||
class __EXPORT UnitTest
|
||||
{
|
||||
|
||||
public:
|
||||
#define INLINE_GLOBAL(type,func) inline type& func() { static type x; return x; }
|
||||
|
||||
INLINE_GLOBAL(int, mu_tests_run)
|
||||
INLINE_GLOBAL(int, mu_tests_failed)
|
||||
INLINE_GLOBAL(int, mu_tests_passed)
|
||||
INLINE_GLOBAL(int, mu_assertion)
|
||||
INLINE_GLOBAL(int, mu_line)
|
||||
INLINE_GLOBAL(const char*, mu_last_test)
|
||||
|
||||
UnitTest();
|
||||
virtual ~UnitTest();
|
||||
|
||||
virtual void runTests(void) = 0;
|
||||
void printResults(void);
|
||||
|
||||
void printAssert(const char* msg, const char* test, const char* file, int line);
|
||||
|
||||
#define ut_assert(message, test) \
|
||||
do { \
|
||||
if (!(test)) { \
|
||||
printAssert(message, #test, __FILE__, __LINE__); \
|
||||
return false; \
|
||||
} else { \
|
||||
mu_assertion()++; \
|
||||
} \
|
||||
} while (0)
|
||||
|
||||
#define ut_run_test(test) \
|
||||
do { \
|
||||
warnx("RUNNING TEST: %s", #test); \
|
||||
mu_tests_run()++; \
|
||||
if (!test()) { \
|
||||
warnx("TEST FAILED: %s", #test); \
|
||||
mu_tests_failed()++; \
|
||||
} else { \
|
||||
warnx("TEST PASSED: %s", #test); \
|
||||
mu_tests_passed()++; \
|
||||
} \
|
||||
} while (0)
|
||||
virtual ~UnitTest();
|
||||
|
||||
/// @brief Override to run your unit tests. Unit tests should be called using ut_run_test macro.
|
||||
/// @return true: all unit tests succeeded, false: one or more unit tests failed
|
||||
virtual bool run_tests(void) = 0;
|
||||
|
||||
/// @brief Prints results from running of unit tests.
|
||||
void print_results(void);
|
||||
|
||||
/// @brief Macro to create a function which will run a unit test class and print results.
|
||||
#define ut_declare_test(test_function, test_class) \
|
||||
bool test_function(void) \
|
||||
{ \
|
||||
test_class* test = new test_class(); \
|
||||
bool success = test->run_tests(); \
|
||||
test->print_results(); \
|
||||
return success; \
|
||||
}
|
||||
|
||||
protected:
|
||||
|
||||
/// @brief Runs a single unit test. Unit tests must have the function signature of bool test(void). The unit
|
||||
/// test should return true if it succeeded, false for fail.
|
||||
#define ut_run_test(test) \
|
||||
do { \
|
||||
warnx("RUNNING TEST: %s", #test); \
|
||||
_tests_run++; \
|
||||
_init(); \
|
||||
if (!test()) { \
|
||||
warnx("TEST FAILED: %s", #test); \
|
||||
_tests_failed++; \
|
||||
} else { \
|
||||
warnx("TEST PASSED: %s", #test); \
|
||||
_tests_passed++; \
|
||||
} \
|
||||
_cleanup(); \
|
||||
} while (0)
|
||||
|
||||
/// @brief Used to assert a value within a unit test.
|
||||
#define ut_assert(message, test) \
|
||||
do { \
|
||||
if (!(test)) { \
|
||||
_print_assert(message, #test, __FILE__, __LINE__); \
|
||||
return false; \
|
||||
} else { \
|
||||
_assertions++; \
|
||||
} \
|
||||
} while (0)
|
||||
|
||||
/// @brief Used to compare two integer values within a unit test. If possible use ut_compare instead of ut_assert
|
||||
/// since it will give you better error reporting of the actual values being compared.
|
||||
#define ut_compare(message, v1, v2) \
|
||||
do { \
|
||||
int _v1 = v1; \
|
||||
int _v2 = v2; \
|
||||
if (_v1 != _v2) { \
|
||||
_print_compare(message, #v1, _v1, #v2, _v2, __FILE__, __LINE__); \
|
||||
return false; \
|
||||
} else { \
|
||||
_assertions++; \
|
||||
} \
|
||||
} while (0)
|
||||
|
||||
virtual void _init(void) { }; ///< Run before each unit test. Override to provide custom behavior.
|
||||
virtual void _cleanup(void) { }; ///< Run after each unit test. Override to provide custom behavior.
|
||||
|
||||
void _print_assert(const char* msg, const char* test, const char* file, int line);
|
||||
void _print_compare(const char* msg, const char *v1_text, int v1, const char *v2_text, int v2, const char* file, int line);
|
||||
|
||||
int _tests_run; ///< The number of individual unit tests run
|
||||
int _tests_failed; ///< The number of unit tests which failed
|
||||
int _tests_passed; ///< The number of unit tests which passed
|
||||
int _assertions; ///< Total number of assertions tested by all unit tests
|
||||
};
|
||||
|
||||
#endif /* UNIT_TEST_H_ */
|
||||
|
|
2
uavcan
2
uavcan
|
@ -1 +1 @@
|
|||
Subproject commit c4c14c60fbbd9acd281ee97d5bb2a4027d0ae2d9
|
||||
Subproject commit 286adbcc56c4b093143b647ec7546abb149cd53b
|
Loading…
Reference in New Issue