dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge branch 'master' of github.com:PX4/Firmware into sf0x_test

This commit is contained in:
Lorenz Meier 2014-10-07 12:09:47 +02:00
parent 7dd81c8cb2 79e5ec6465
commit 8d187cc2fa
64 changed files with 2972 additions and 846 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
LICENSE.md Normal file
View File

@ -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.

10
README.md Normal file
View File

@ -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)

2
ROMFS/px4fmu_common/init.d/3035_viper
View File

@ -9,5 +9,3 @@ sh /etc/init.d/rc.fw_defaults
set MIXER Viper
set FAILSAFE "-c56 -p 1000"
set FAILSAFE "-c7 -p 1200"

30
ROMFS/px4fmu_test/init.d/rcS
View File

@ -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

0
ROMFS/px4fmu_test/unit_test_data/mavlink_tests/empty_dir/.gitignore vendored Normal file
View File

BIN
ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_238.data Normal file
View File

Binary file not shown.

BIN
ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_239.data Normal file
View File

Binary file not shown.

BIN
ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_240.data Normal file
View File

Binary file not shown.

19
makefiles/config_px4fmu-v1_default.mk
View File

@ -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

4
makefiles/config_px4fmu-v2_test.mk
View File

@ -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.
#

12
src/drivers/gps/ubx.cpp
View File

@ -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 */

17
src/drivers/gps/ubx.h
View File

@ -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;

17
src/drivers/mpu6000/mpu6000.cpp
View File

@ -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:

9
src/drivers/px4io/px4io.cpp
View File

@ -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;

2
src/lib/conversion/module.mk
View File

@ -36,3 +36,5 @@
#
SRCS = rotation.cpp
MAXOPTIMIZATION = -Os

1
src/lib/external_lgpl/tecs/tecs.cpp
View File

@ -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;

106
src/modules/commander/commander.cpp
View File

@ -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;

2
src/modules/commander/commander_params.c
View File

@ -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
*

4
src/modules/commander/commander_tests/commander_tests.cpp
View File

@ -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;
}

13
src/modules/commander/commander_tests/state_machine_helper_test.cpp
View File

@ -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)

2
src/modules/commander/commander_tests/state_machine_helper_test.h
View File

@ -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_ */

2
src/modules/dataman/dataman.c
View File

@ -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;
}

2
src/modules/dataman/module.mk
View File

@ -40,3 +40,5 @@ MODULE_COMMAND = dataman
SRCS = dataman.c
MODULE_STACKSIZE = 1200
MAXOPTIMIZATION = -Os

2
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
View File

@ -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();
}

134
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
View File

@ -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> &current_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> &current_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> &current_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> &current_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> &current_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> &current_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> &current_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,

26
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
View File

@ -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);

7
src/modules/mavlink/mavlink.c
View File

@ -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,

2
src/modules/mavlink/mavlink_bridge_header.h
View File

@ -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

714
src/modules/mavlink/mavlink_ftp.cpp
View File

@ -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) {
// 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 (req == nullptr) {
warnx("Dropping FTP request: queue full\n");
return;
}
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
// then pack and send the reply back to the request source
req->reply();
bool success = true;
#ifdef MAVLINK_FTP_UNIT_TEST
// Unit test hook is set, call that instead
_utRcvMsgFunc(&msg, _ftp_test);
#else
Mavlink *mavlink = req->mavlink;
mavlink->lockMessageBufferMutex();
success = mavlink->message_buffer_write(&msg, len);
mavlink->unlockMessageBufferMutex();
#endif
if (!success) {
warnx("FTP TX ERR");
}
#ifdef MAVLINK_FTP_DEBUG
else {
warnx("wrote: sys: %d, comp: %d, chan: %d, checksum: %d",
req->serverSystemId,
req->serverComponentId,
req->serverChannel,
msg.checksum);
}
#endif
}
/// @brief Responds to a List command
MavlinkFTP::ErrorCode
MavlinkFTP::_workList(Request *req)
MavlinkFTP::_workList(PayloadHeader* payload)
{
auto hdr = req->header();
char dirPath[kMaxDataLength];
strncpy(dirPath, req->dataAsCString(), kMaxDataLength);
strncpy(dirPath, _data_as_cstring(payload), kMaxDataLength);
DIR *dp = opendir(dirPath);
if (dp == nullptr) {
warnx("FTP: can't open path '%s'", dirPath);
return kErrNotDir;
return kErrFailErrno;
}
#ifdef MAVLINK_FTP_DEBUG
warnx("FTP: list %s offset %d", dirPath, hdr->offset);
warnx("FTP: list %s offset %d", dirPath, payload->offset);
#endif
ErrorCode errorCode = kErrNone;
@ -239,19 +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:
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;
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
if (lseek(_session_fds[session_index], payload->offset, SEEK_SET) < 0) {
// Unable to see to the specified location
warnx("seek fail");
return kErrFailErrno;
}
MavlinkFTP::ErrorCode
MavlinkFTP::_workRemove(Request *req)
{
//auto hdr = req->header();
// for now, send error reply
return kErrPerm;
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;
}
MavlinkFTP::ErrorCode
MavlinkFTP::_workTerminate(Request *req)
{
auto hdr = req->header();
if (!_validSession(hdr->session)) {
return kErrNoSession;
}
::close(_session_fds[hdr->session]);
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;
}

246
src/modules/mavlink/mavlink_ftp.h
View File

@ -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();
static MavlinkFTP *getServer();
/// @brief Adds the specified message to the work queue.
void handle_message(Mavlink* mavlink, mavlink_message_t *msg);
// static interface
void handle_message(Mavlink* mavlink,
mavlink_message_t *msg);
typedef void (*ReceiveMessageFunc_t)(const mavlink_message_t *msg, MavlinkFtpTest* ftpTest);
private:
/// @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 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
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[];
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?)
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,
kRspNak
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
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
};
int _findUnusedSession(void);
bool _validSession(unsigned index);
static const unsigned kMaxSession = 2;
int _session_fds[kMaxSession];
class 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;
/// @brief Unit of work which is queued to work_queue
struct Request
{
work_s work; ///< work queue entry
Mavlink *mavlink; ///< Mavlink to reply to
uint8_t serverSystemId; ///< System ID to send from
uint8_t serverComponentId; ///< Component ID to send from
uint8_t serverChannel; ///< Channel to send to
uint8_t targetSystemId; ///< System ID to target reply to
mavlink_file_transfer_protocol_t message; ///< Protocol message
};
static const char kDirentFile = 'F';
static const char kDirentDir = 'D';
static const char kDirentUnknown = 'U';
static const uint8_t kMaxDataLength = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(RequestHeader);
Request *_get_request(void);
void _return_request(Request *req);
void _lock_request_queue(void);
void _unlock_request_queue(void);
/// Request worker; runs on the low-priority work queue to service
/// remote requests.
///
static void _workerTrampoline(void *arg);
void _worker(Request *req);
char *_data_as_cstring(PayloadHeader* payload);
/// Reply to a request (XXX should be a Request method)
///
static void _worker_trampoline(void *arg);
void _process_request(Request *req);
void _reply(Request *req);
int _copy_file(const char *src_path, const char *dst_path, ssize_t length);
ErrorCode _workList(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();
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);
// work freelist
Request _workBufs[kRequestQueueSize];
dq_queue_t _workFree;
sem_t _lock;
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
void _qLock() { do {} while (sem_wait(&_lock) != 0); }
void _qUnlock() { sem_post(&_lock); }
int _find_unused_session(void);
bool _valid_session(unsigned index);
void _qFree(Request *req) {
_qLock();
dq_addlast(&req->entry, &_workFree);
_qUnlock();
}
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
Request *_dqFree() {
_qLock();
auto req = reinterpret_cast<Request *>(dq_remfirst(&_workFree));
_qUnlock();
return req;
}
/// @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;
};

19
src/modules/mavlink/mavlink_main.cpp
View File

@ -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:

4
src/modules/mavlink/mavlink_main.h
View File

@ -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; }
@ -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 */

296
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -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)
{

6
src/modules/mavlink/mavlink_receiver.h
View File

@ -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;

761
src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp Normal file
View File

@ -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)

108
src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h Normal file
View File

@ -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);

7
src/modules/mavlink/mavlink_tests/mavlink_ftp_test_data.py Normal file
View File

@ -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()

47
src/modules/mavlink/mavlink_tests/mavlink_tests.cpp Normal file
View File

@ -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;
}

50
src/modules/mavlink/mavlink_tests/module.mk Normal file
View File

@ -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

254
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -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 */

55
src/modules/navigator/mission.cpp
View File

@ -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,9 +149,20 @@ Mission::on_active()
/* lets check if we reached the current mission item */
if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) {
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();
} else {
@ -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,16 +441,29 @@ Mission::set_mission_items()
takeoff_alt = fmaxf(takeoff_alt, _navigator->get_home_position()->alt + _param_takeoff_alt.get());
}
/* 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.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);
_navigator->set_position_setpoint_triplet_updated();
return;
} else {
/* skip takeoff */
_takeoff = false;
}
}
/* set current position setpoint from mission item */
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
@ -445,10 +476,11 @@ Mission::set_mission_items()
reset_mission_item_reached();
if (_mission_type == MISSION_TYPE_OFFBOARD) {
report_current_offboard_mission_item();
set_current_offboard_mission_item();
}
// TODO: report onboard mission item somehow
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->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();
}

17
src/modules/navigator/mission.h
View File

@ -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,

20
src/modules/navigator/mission_feasibility_checker.cpp
View File

@ -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()

3
src/modules/navigator/module.mk
View File

@ -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

6
src/modules/navigator/navigator.h
View File

@ -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!) */

30
src/modules/navigator/navigator_main.cpp
View File

@ -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;

129
src/modules/navigator/offboard.cpp
View File

@ -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);
}

18
src/modules/systemlib/circuit_breaker.c
View File

@ -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)
{

1
src/modules/systemlib/circuit_breaker.h
View File

@ -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>

6
src/modules/systemlib/mixer/mixer.h
View File

@ -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;

22
src/modules/systemlib/mixer/mixer_multirotor.cpp
View File

@ -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;
}

3
src/modules/systemlib/param/param.c
View File

@ -322,7 +322,8 @@ param_get_value_ptr(param_t param)
v = &param_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 {

2
src/modules/systemlib/param/param.h
View File

@ -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 \
}
/**

3
src/modules/uORB/objects_common.cpp
View File

@ -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);

61
src/modules/navigator/offboard.h → src/modules/uORB/topics/multirotor_motor_limits.h
View File

@ -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

201
src/modules/uORB/topics/offboard_control_setpoint.h
View File

@ -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);

7
src/modules/uORB/topics/position_setpoint_triplet.h
View File

@ -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 */
};
/**

3
src/modules/uORB/topics/vehicle_command.h
View File

@ -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| */

1
src/modules/uORB/topics/vehicle_control_mode.h
View File

@ -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 */

4
src/modules/uORB/topics/vehicle_status.h
View File

@ -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;
};
/**

8
src/modules/uavcan/sensors/gnss.cpp
View File

@ -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;

25
src/modules/unit_test/unit_test.cpp
View File

@ -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);
}
/// @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);
}

83
src/modules/unit_test/unit_test.h
View File

@ -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);
/// @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;
void printAssert(const char* msg, const char* test, const char* file, int line);
/// @brief Prints results from running of unit tests.
void print_results(void);
#define ut_assert(message, test) \
do { \
if (!(test)) { \
printAssert(message, #test, __FILE__, __LINE__); \
return false; \
} else { \
mu_assertion()++; \
} \
} while (0)
/// @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); \
mu_tests_run()++; \
_tests_run++; \
_init(); \
if (!test()) { \
warnx("TEST FAILED: %s", #test); \
mu_tests_failed()++; \
_tests_failed++; \
} else { \
warnx("TEST PASSED: %s", #test); \
mu_tests_passed()++; \
_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

@ -1 +1 @@
Subproject commit c4c14c60fbbd9acd281ee97d5bb2a4027d0ae2d9
Subproject commit 286adbcc56c4b093143b647ec7546abb149cd53b