forked from Archive/PX4-Autopilot
restore px4fmu-v2_test
This commit is contained in:
parent
848e87ff76
commit
2bc74fd5d9
5
Makefile
5
Makefile
|
@ -137,6 +137,9 @@ px4fmu-v1_default:
|
|||
px4fmu-v2_default:
|
||||
$(call cmake-build,nuttx_px4fmu-v2_default)
|
||||
|
||||
px4fmu-v2_test:
|
||||
$(call cmake-build,nuttx_px4fmu-v2_test)
|
||||
|
||||
px4fmu-v4_default:
|
||||
$(call cmake-build,nuttx_px4fmu-v4_default)
|
||||
|
||||
|
@ -222,7 +225,7 @@ ifeq ($(VECTORCONTROL),1)
|
|||
@(rm -rf vectorcontrol && git clone --quiet --depth 1 https://github.com/thiemar/vectorcontrol.git && cd vectorcontrol && BOARD=s2740vc_1_0 make --silent --no-print-directory && BOARD=px4esc_1_6 make --silent --no-print-directory && ../Tools/uavcan_copy.sh)
|
||||
endif
|
||||
|
||||
check: check_px4fmu-v1_default check_px4fmu-v2_default check_px4fmu-v2_ekf2 check_px4fmu-v2_lpe check_px4fmu-v4_default_and_uavcan check_mindpx-v2_default check_px4-stm32f4discovery_default check_posix_sitl_default check_unittest check_format
|
||||
check: check_px4fmu-v1_default check_px4fmu-v2_default check_px4fmu-v2_test check_px4fmu-v2_ekf2 check_px4fmu-v2_lpe check_px4fmu-v4_default_and_uavcan check_mindpx-v2_default check_px4-stm32f4discovery_default check_posix_sitl_default check_unittest check_format
|
||||
|
||||
check_format:
|
||||
$(call colorecho,"Checking formatting with astyle")
|
||||
|
|
|
@ -0,0 +1,201 @@
|
|||
#!nsh
|
||||
#
|
||||
# Standard startup script for PX4FMU v1, v2, v3, v4 onboard sensor drivers.
|
||||
#
|
||||
|
||||
if ver hwcmp PX4FMU_V1
|
||||
then
|
||||
if ms5611 start
|
||||
then
|
||||
fi
|
||||
else
|
||||
# Configure all I2C buses to 100 KHz as they
|
||||
# are all external or slow
|
||||
fmu i2c 1 100000
|
||||
fmu i2c 2 100000
|
||||
|
||||
if ms5611 -s start
|
||||
then
|
||||
fi
|
||||
|
||||
# Blacksheep telemetry
|
||||
if bst start
|
||||
then
|
||||
fi
|
||||
fi
|
||||
|
||||
if adc start
|
||||
then
|
||||
fi
|
||||
|
||||
if ver hwcmp PX4FMU_V2
|
||||
then
|
||||
# External I2C bus
|
||||
if hmc5883 -C -T -X start
|
||||
then
|
||||
fi
|
||||
|
||||
# External I2C bus
|
||||
if lis3mdl -X start
|
||||
then
|
||||
fi
|
||||
|
||||
# Internal I2C bus
|
||||
if hmc5883 -C -T -I -R 4 start
|
||||
then
|
||||
fi
|
||||
|
||||
# external MPU6K is rotated 180 degrees yaw
|
||||
if mpu6000 -X -R 4 start
|
||||
then
|
||||
set BOARD_FMUV3 true
|
||||
else
|
||||
set BOARD_FMUV3 false
|
||||
fi
|
||||
|
||||
if [ $BOARD_FMUV3 == true ]
|
||||
then
|
||||
# external L3GD20H is rotated 180 degrees yaw
|
||||
if l3gd20 -X -R 4 start
|
||||
then
|
||||
fi
|
||||
|
||||
# external LSM303D is rotated 270 degrees yaw
|
||||
if lsm303d -X -R 6 start
|
||||
then
|
||||
fi
|
||||
|
||||
# internal MPU6000 is rotated 180 deg roll, 270 deg yaw
|
||||
if mpu6000 -R 14 start
|
||||
then
|
||||
fi
|
||||
|
||||
if hmc5883 -C -T -S -R 8 start
|
||||
then
|
||||
fi
|
||||
|
||||
if meas_airspeed start -b 2
|
||||
then
|
||||
fi
|
||||
|
||||
else
|
||||
# FMUv2
|
||||
if mpu6000 start
|
||||
then
|
||||
fi
|
||||
|
||||
if l3gd20 start
|
||||
then
|
||||
fi
|
||||
|
||||
if lsm303d start
|
||||
then
|
||||
fi
|
||||
fi
|
||||
fi
|
||||
|
||||
if ver hwcmp PX4FMU_V4
|
||||
then
|
||||
# External I2C bus
|
||||
if hmc5883 -C -T -X start
|
||||
then
|
||||
fi
|
||||
|
||||
if lis3mdl -R 2 start
|
||||
then
|
||||
fi
|
||||
|
||||
# Internal SPI bus is rotated 90 deg yaw
|
||||
if hmc5883 -C -T -S -R 2 start
|
||||
then
|
||||
fi
|
||||
|
||||
# Internal SPI bus ICM-20608-G is rotated 90 deg yaw
|
||||
if mpu6000 -R 2 -T 20608 start
|
||||
then
|
||||
fi
|
||||
|
||||
# Internal SPI bus mpu9250 is rotated 90 deg yaw
|
||||
if mpu9250 -R 2 start
|
||||
then
|
||||
fi
|
||||
fi
|
||||
|
||||
if ver hwcmp PX4FMU_V1
|
||||
then
|
||||
# FMUv1
|
||||
if mpu6000 start
|
||||
then
|
||||
fi
|
||||
|
||||
if l3gd20 start
|
||||
then
|
||||
fi
|
||||
|
||||
# MAG selection
|
||||
if param compare SENS_EXT_MAG 2
|
||||
then
|
||||
if hmc5883 -C -I start
|
||||
then
|
||||
fi
|
||||
else
|
||||
# Use only external as primary
|
||||
if param compare SENS_EXT_MAG 1
|
||||
then
|
||||
if hmc5883 -C -X start
|
||||
then
|
||||
fi
|
||||
else
|
||||
# auto-detect the primary, prefer external
|
||||
if hmc5883 start
|
||||
then
|
||||
fi
|
||||
fi
|
||||
fi
|
||||
fi
|
||||
|
||||
if ver hwcmp MINDPX_V2
|
||||
then
|
||||
if mpu6500 start
|
||||
then
|
||||
fi
|
||||
|
||||
if lsm303d start
|
||||
then
|
||||
fi
|
||||
|
||||
if l3gd20 start
|
||||
then
|
||||
fi
|
||||
|
||||
# External I2C bus
|
||||
if hmc5883 -C -T -X start
|
||||
then
|
||||
fi
|
||||
|
||||
if lis3mdl -R 2 start
|
||||
then
|
||||
fi
|
||||
fi
|
||||
|
||||
if meas_airspeed start
|
||||
then
|
||||
else
|
||||
if ets_airspeed start
|
||||
then
|
||||
else
|
||||
if ets_airspeed start -b 1
|
||||
then
|
||||
fi
|
||||
fi
|
||||
fi
|
||||
|
||||
if sf10a start
|
||||
then
|
||||
fi
|
||||
|
||||
# Wait 20 ms for sensors (because we need to wait for the HRT and work queue callbacks to fire)
|
||||
usleep 20000
|
||||
if sensors start
|
||||
then
|
||||
fi
|
|
@ -116,11 +116,19 @@ else
|
|||
set unit_test_failure_list "${unit_test_failure_list} mavlink_tests"
|
||||
fi
|
||||
|
||||
if commander_tests
|
||||
# TODO: commander_tests is currently broken
|
||||
#if commander_tests
|
||||
#then
|
||||
#else
|
||||
# set unit_test_failure 1
|
||||
# set unit_test_failure_list "${unit_test_failure_list} commander_tests"
|
||||
#fi
|
||||
|
||||
if controllib_test
|
||||
then
|
||||
else
|
||||
set unit_test_failure 1
|
||||
set unit_test_failure_list "${unit_test_failure_list} commander_tests"
|
||||
set unit_test_failure_list "${unit_test_failure_list} controllib_tests"
|
||||
fi
|
||||
|
||||
if uorb test
|
||||
|
@ -130,77 +138,10 @@ else
|
|||
set unit_test_failure_list "${unit_test_failure_list} uorb_tests"
|
||||
fi
|
||||
|
||||
# Start all sensors on all boards
|
||||
ms5611 start
|
||||
adc start
|
||||
|
||||
if mpu6000 -X start
|
||||
then
|
||||
fi
|
||||
|
||||
if mpu6000 start
|
||||
then
|
||||
fi
|
||||
|
||||
if l3gd20 -X start
|
||||
then
|
||||
fi
|
||||
|
||||
if l3gd20 start
|
||||
then
|
||||
fi
|
||||
|
||||
# MAG selection
|
||||
if param compare SENS_EXT_MAG 2
|
||||
then
|
||||
if hmc5883 -I start
|
||||
then
|
||||
fi
|
||||
else
|
||||
# Use only external as primary
|
||||
if param compare SENS_EXT_MAG 1
|
||||
then
|
||||
if hmc5883 -X start
|
||||
then
|
||||
fi
|
||||
else
|
||||
# auto-detect the primary, prefer external
|
||||
if hmc5883 start
|
||||
then
|
||||
fi
|
||||
fi
|
||||
fi
|
||||
|
||||
if ver hwcmp PX4FMU_V2
|
||||
then
|
||||
if lsm303d -X start
|
||||
then
|
||||
fi
|
||||
|
||||
if lsm303d start
|
||||
then
|
||||
fi
|
||||
|
||||
if ms5611 -X start
|
||||
then
|
||||
fi
|
||||
fi
|
||||
|
||||
if meas_airspeed start
|
||||
then
|
||||
else
|
||||
if ets_airspeed start
|
||||
then
|
||||
else
|
||||
if ets_airspeed start -b 1
|
||||
then
|
||||
fi
|
||||
fi
|
||||
fi
|
||||
|
||||
if sensors start
|
||||
then
|
||||
fi
|
||||
#
|
||||
# Sensors System (start before Commander so Preflight checks are properly run)
|
||||
#
|
||||
sh /etc/init.d/rc.sensors
|
||||
|
||||
# Check for flow sensor
|
||||
if px4flow start
|
||||
|
|
|
@ -26,6 +26,7 @@ find \
|
|||
src/modules/gpio_led \
|
||||
src/modules/land_detector \
|
||||
src/modules/local_position_estimator \
|
||||
src/modules/mavlink/mavlink_tests \
|
||||
src/modules/muorb \
|
||||
src/modules/param \
|
||||
src/modules/px4iofirmware \
|
||||
|
|
|
@ -71,7 +71,7 @@ def main():
|
|||
const="",
|
||||
metavar="BOARD",
|
||||
help="Board to create airframes xml for")
|
||||
parser.add_argument("-v", "--verbose", help="verbose output")
|
||||
parser.add_argument('-v', '--verbose', action='store_true', help="verbose output")
|
||||
args = parser.parse_args()
|
||||
|
||||
# Check for valid command
|
||||
|
|
|
@ -107,7 +107,7 @@ def main():
|
|||
metavar="SUMMARY",
|
||||
default="Automagically updated parameter documentation from code.",
|
||||
help="DokuWiki page edit summary")
|
||||
parser.add_argument("-v", "--verbose", help="verbose output")
|
||||
parser.add_argument('-v', '--verbose', action='store_true', help="verbose output")
|
||||
args = parser.parse_args()
|
||||
|
||||
# Check for valid command
|
||||
|
|
|
@ -0,0 +1,202 @@
|
|||
include(nuttx/px4_impl_nuttx)
|
||||
|
||||
set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake)
|
||||
|
||||
set(config_uavcan_num_ifaces 2)
|
||||
|
||||
set(config_module_list
|
||||
#
|
||||
# Board support modules
|
||||
#
|
||||
drivers/device
|
||||
drivers/stm32
|
||||
drivers/stm32/adc
|
||||
drivers/stm32/tone_alarm
|
||||
drivers/led
|
||||
drivers/px4fmu
|
||||
drivers/px4io
|
||||
drivers/boards/px4fmu-v2
|
||||
drivers/rgbled
|
||||
drivers/mpu6000
|
||||
#drivers/mpu9250
|
||||
drivers/lsm303d
|
||||
drivers/l3gd20
|
||||
drivers/hmc5883
|
||||
drivers/ms5611
|
||||
#drivers/mb12xx
|
||||
drivers/srf02
|
||||
drivers/sf0x
|
||||
drivers/ll40ls
|
||||
drivers/trone
|
||||
drivers/gps
|
||||
drivers/pwm_out_sim
|
||||
#drivers/hott
|
||||
#drivers/hott/hott_telemetry
|
||||
#drivers/hott/hott_sensors
|
||||
drivers/blinkm
|
||||
drivers/airspeed
|
||||
drivers/ets_airspeed
|
||||
drivers/meas_airspeed
|
||||
drivers/frsky_telemetry
|
||||
modules/sensors
|
||||
#drivers/mkblctrl
|
||||
drivers/px4flow
|
||||
#drivers/oreoled
|
||||
drivers/gimbal
|
||||
drivers/pwm_input
|
||||
drivers/camera_trigger
|
||||
drivers/bst
|
||||
drivers/snapdragon_rc_pwm
|
||||
drivers/lis3mdl
|
||||
|
||||
#
|
||||
# System commands
|
||||
#
|
||||
systemcmds/bl_update
|
||||
systemcmds/mixer
|
||||
systemcmds/param
|
||||
systemcmds/perf
|
||||
systemcmds/pwm
|
||||
systemcmds/esc_calib
|
||||
systemcmds/reboot
|
||||
#systemcmds/topic_listener
|
||||
systemcmds/top
|
||||
systemcmds/config
|
||||
systemcmds/nshterm
|
||||
systemcmds/mtd
|
||||
systemcmds/dumpfile
|
||||
systemcmds/ver
|
||||
|
||||
#
|
||||
# Testing
|
||||
#
|
||||
modules/commander/commander_tests
|
||||
modules/controllib_test
|
||||
modules/mavlink/mavlink_tests
|
||||
modules/unit_test
|
||||
systemcmds/tests
|
||||
|
||||
#
|
||||
# General system control
|
||||
#
|
||||
modules/commander
|
||||
modules/navigator
|
||||
modules/mavlink
|
||||
modules/gpio_led
|
||||
modules/uavcan
|
||||
modules/land_detector
|
||||
|
||||
#
|
||||
# Estimation modules (EKF/ SO3 / other filters)
|
||||
#
|
||||
# Too high RAM usage due to static allocations
|
||||
# modules/attitude_estimator_ekf
|
||||
modules/attitude_estimator_q
|
||||
modules/ekf_att_pos_estimator
|
||||
modules/position_estimator_inav
|
||||
|
||||
#
|
||||
# Vehicle Control
|
||||
#
|
||||
# modules/segway # XXX Needs GCC 4.7 fix
|
||||
modules/fw_pos_control_l1
|
||||
modules/fw_att_control
|
||||
modules/mc_att_control
|
||||
modules/mc_pos_control
|
||||
modules/vtol_att_control
|
||||
|
||||
#
|
||||
# Logging
|
||||
#
|
||||
modules/sdlog2
|
||||
|
||||
#
|
||||
# Library modules
|
||||
#
|
||||
modules/param
|
||||
modules/systemlib
|
||||
modules/systemlib/mixer
|
||||
modules/uORB
|
||||
modules/dataman
|
||||
|
||||
#
|
||||
# Libraries
|
||||
#
|
||||
lib/controllib
|
||||
lib/mathlib
|
||||
lib/mathlib/math/filter
|
||||
lib/ecl
|
||||
lib/external_lgpl
|
||||
lib/geo
|
||||
lib/geo_lookup
|
||||
lib/conversion
|
||||
lib/launchdetection
|
||||
lib/terrain_estimation
|
||||
lib/runway_takeoff
|
||||
lib/tailsitter_recovery
|
||||
platforms/nuttx
|
||||
|
||||
# had to add for cmake, not sure why wasn't in original config
|
||||
platforms/common
|
||||
platforms/nuttx/px4_layer
|
||||
|
||||
#
|
||||
# OBC challenge
|
||||
#
|
||||
#modules/bottle_drop
|
||||
|
||||
#
|
||||
# Rover apps
|
||||
#
|
||||
#examples/rover_steering_control
|
||||
|
||||
#
|
||||
# Demo apps
|
||||
#
|
||||
#examples/math_demo
|
||||
# Tutorial code from
|
||||
# https://px4.io/dev/px4_simple_app
|
||||
#examples/px4_simple_app
|
||||
|
||||
# Tutorial code from
|
||||
# https://px4.io/dev/daemon
|
||||
#examples/px4_daemon_app
|
||||
|
||||
# Tutorial code from
|
||||
# https://px4.io/dev/debug_values
|
||||
#examples/px4_mavlink_debug
|
||||
|
||||
# Tutorial code from
|
||||
# https://px4.io/dev/example_fixedwing_control
|
||||
#examples/fixedwing_control
|
||||
|
||||
# Hardware test
|
||||
#examples/hwtest
|
||||
)
|
||||
|
||||
set(config_extra_builtin_cmds
|
||||
serdis
|
||||
sercon
|
||||
)
|
||||
|
||||
set(config_io_board
|
||||
px4io-v2
|
||||
)
|
||||
|
||||
set(config_extra_libs
|
||||
uavcan
|
||||
uavcan_stm32_driver
|
||||
)
|
||||
|
||||
set(config_io_extra_libs
|
||||
)
|
||||
|
||||
add_custom_target(sercon)
|
||||
set_target_properties(sercon PROPERTIES
|
||||
PRIORITY "SCHED_PRIORITY_DEFAULT"
|
||||
MAIN "sercon" STACK_MAIN "2048")
|
||||
|
||||
add_custom_target(serdis)
|
||||
set_target_properties(serdis PROPERTIES
|
||||
PRIORITY "SCHED_PRIORITY_DEFAULT"
|
||||
MAIN "serdis" STACK_MAIN "2048")
|
|
@ -46,15 +46,20 @@ if(NOT ${BOARD} STREQUAL "sim")
|
|||
set(extras "${CMAKE_BINARY_DIR}/src/modules/px4iofirmware/${config_io_board}.bin")
|
||||
endif()
|
||||
|
||||
set(romfs_dir "ROMFS/px4fmu_common")
|
||||
if (${BOARD} STREQUAL "px4fmu-v2" AND ${LABEL} STREQUAL "test")
|
||||
set(romfs_dir "ROMFS/px4fmu_test")
|
||||
endif()
|
||||
|
||||
px4_nuttx_add_romfs(OUT romfs
|
||||
ROOT ROMFS/px4fmu_common
|
||||
ROOT ${romfs_dir}
|
||||
EXTRAS ${extras}
|
||||
)
|
||||
if (config_io_board)
|
||||
add_dependencies(romfs fw_io)
|
||||
endif()
|
||||
set(fw_file
|
||||
${CMAKE_CURRENT_BINARY_DIR}/${OS}-${BOARD}-${LABEL}.px4)
|
||||
|
||||
set(fw_file ${CMAKE_CURRENT_BINARY_DIR}/${OS}-${BOARD}-${LABEL}.px4)
|
||||
|
||||
px4_nuttx_add_firmware(OUT ${fw_file}
|
||||
BOARD ${BOARD}
|
||||
|
|
|
@ -509,9 +509,7 @@ int commander_main(int argc, char *argv[])
|
|||
warnx("argument %s unsupported.", argv[2]);
|
||||
}
|
||||
|
||||
if (TRANSITION_DENIED == main_state_transition(&status, new_main_state, main_state_prev,
|
||||
&status_flags, &internal_state)) {
|
||||
;
|
||||
if (TRANSITION_DENIED == main_state_transition(&status, new_main_state, main_state_prev, &status_flags, &internal_state)) {
|
||||
warnx("mode change failed");
|
||||
}
|
||||
return 0;
|
||||
|
@ -1260,7 +1258,7 @@ int commander_thread_main(int argc, char *argv[])
|
|||
// We want to accept RC inputs as default
|
||||
status_flags.rc_input_blocked = false;
|
||||
status.rc_input_mode = vehicle_status_s::RC_IN_MODE_DEFAULT;
|
||||
internal_state.main_state =commander_state_s::MAIN_STATE_MANUAL;
|
||||
internal_state.main_state = commander_state_s::MAIN_STATE_MANUAL;
|
||||
main_state_prev = commander_state_s::MAIN_STATE_MAX;
|
||||
status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
|
||||
status.arming_state = vehicle_status_s::ARMING_STATE_INIT;
|
||||
|
|
|
@ -268,9 +268,11 @@ bool StateMachineHelperTest::armingStateTransitionTest(void)
|
|||
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
|
||||
};
|
||||
|
||||
struct vehicle_status_s status;
|
||||
struct safety_s safety;
|
||||
struct actuator_armed_s armed;
|
||||
struct vehicle_status_s status = {};
|
||||
struct status_flags_s status_flags = {};
|
||||
struct safety_s safety = {};
|
||||
struct actuator_armed_s armed = {};
|
||||
struct battery_status_s battery = {};
|
||||
|
||||
size_t cArmingTransitionTests = sizeof(rgArmingTransitionTests) / sizeof(rgArmingTransitionTests[0]);
|
||||
for (size_t i=0; i<cArmingTransitionTests; i++) {
|
||||
|
@ -278,17 +280,21 @@ bool StateMachineHelperTest::armingStateTransitionTest(void)
|
|||
|
||||
// Setup initial machine state
|
||||
status.arming_state = test->current_state.arming_state;
|
||||
status.condition_system_sensors_initialized = test->condition_system_sensors_initialized;
|
||||
status_flags.condition_system_sensors_initialized = test->condition_system_sensors_initialized;
|
||||
status.hil_state = test->hil_state;
|
||||
// The power status of the test unit is not relevant for the unit test
|
||||
status.circuit_breaker_engaged_power_check = true;
|
||||
status_flags.circuit_breaker_engaged_power_check = true;
|
||||
safety.safety_switch_available = test->safety_switch_available;
|
||||
safety.safety_off = test->safety_off;
|
||||
armed.armed = test->current_state.armed;
|
||||
armed.ready_to_arm = test->current_state.ready_to_arm;
|
||||
|
||||
// Attempt transition
|
||||
transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed, false /* no pre-arm checks */, nullptr /* no mavlink_log_pub */);
|
||||
transition_result_t result = arming_state_transition(&status, &battery, &safety, test->requested_state, &armed,
|
||||
false /* no pre-arm checks */,
|
||||
nullptr /* no mavlink_log_pub */,
|
||||
&status_flags,
|
||||
5.0f);
|
||||
|
||||
// Validate result of transition
|
||||
ut_assert(test->assertMsg, test->expected_transition_result == result);
|
||||
|
@ -306,7 +312,7 @@ bool StateMachineHelperTest::mainStateTransitionTest(void)
|
|||
typedef struct {
|
||||
const char* assertMsg; // Text to show when test case fails
|
||||
uint8_t condition_bits; // Bits for various condition_* values
|
||||
main_state_t from_state; // State prior to transition request
|
||||
uint8_t from_state; // State prior to transition request
|
||||
main_state_t to_state; // State to transition to
|
||||
transition_result_t expected_transition_result; // Expected result from main_state_transition call
|
||||
} MainTransitionTest_t;
|
||||
|
@ -325,99 +331,99 @@ bool StateMachineHelperTest::mainStateTransitionTest(void)
|
|||
|
||||
{ "no transition: identical states",
|
||||
MTT_ALL_NOT_VALID,
|
||||
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_NOT_CHANGED },
|
||||
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_NOT_CHANGED },
|
||||
|
||||
// TRANSITION_CHANGED tests
|
||||
|
||||
{ "transition: MANUAL to ACRO",
|
||||
MTT_ALL_NOT_VALID,
|
||||
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ACRO, TRANSITION_CHANGED },
|
||||
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ACRO, TRANSITION_CHANGED },
|
||||
|
||||
{ "transition: ACRO to MANUAL",
|
||||
MTT_ALL_NOT_VALID,
|
||||
vehicle_status_s::MAIN_STATE_ACRO, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
|
||||
commander_state_s::MAIN_STATE_ACRO, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
|
||||
|
||||
{ "transition: MANUAL to AUTO_MISSION - global position valid, home position valid",
|
||||
MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
|
||||
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_MISSION, TRANSITION_CHANGED },
|
||||
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_MISSION, TRANSITION_CHANGED },
|
||||
|
||||
{ "transition: AUTO_MISSION to MANUAL - global position valid, home position valid",
|
||||
MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
|
||||
vehicle_status_s::MAIN_STATE_AUTO_MISSION, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
|
||||
commander_state_s::MAIN_STATE_AUTO_MISSION, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
|
||||
|
||||
{ "transition: MANUAL to AUTO_LOITER - global position valid",
|
||||
MTT_GLOBAL_POS_VALID,
|
||||
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_LOITER, TRANSITION_CHANGED },
|
||||
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_LOITER, TRANSITION_CHANGED },
|
||||
|
||||
{ "transition: AUTO_LOITER to MANUAL - global position valid",
|
||||
MTT_GLOBAL_POS_VALID,
|
||||
vehicle_status_s::MAIN_STATE_AUTO_LOITER, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
|
||||
commander_state_s::MAIN_STATE_AUTO_LOITER, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
|
||||
|
||||
{ "transition: MANUAL to AUTO_RTL - global position valid, home position valid",
|
||||
MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
|
||||
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_RTL, TRANSITION_CHANGED },
|
||||
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_RTL, TRANSITION_CHANGED },
|
||||
|
||||
{ "transition: AUTO_RTL to MANUAL - global position valid, home position valid",
|
||||
MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
|
||||
vehicle_status_s::MAIN_STATE_AUTO_RTL, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
|
||||
commander_state_s::MAIN_STATE_AUTO_RTL, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
|
||||
|
||||
{ "transition: MANUAL to ALTCTL - not rotary",
|
||||
MTT_ALL_NOT_VALID,
|
||||
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
|
||||
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
|
||||
|
||||
{ "transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude valid",
|
||||
MTT_ROTARY_WING | MTT_LOC_ALT_VALID,
|
||||
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
|
||||
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
|
||||
|
||||
{ "transition: MANUAL to ALTCTL - rotary, global position valid, local altitude not valid",
|
||||
MTT_ROTARY_WING | MTT_GLOBAL_POS_VALID,
|
||||
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
|
||||
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
|
||||
|
||||
{ "transition: ALTCTL to MANUAL - local altitude valid",
|
||||
MTT_LOC_ALT_VALID,
|
||||
vehicle_status_s::MAIN_STATE_ALTCTL, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
|
||||
commander_state_s::MAIN_STATE_ALTCTL, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
|
||||
|
||||
{ "transition: MANUAL to POSCTL - local position not valid, global position valid",
|
||||
MTT_GLOBAL_POS_VALID,
|
||||
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_POSCTL, TRANSITION_CHANGED },
|
||||
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_POSCTL, TRANSITION_CHANGED },
|
||||
|
||||
{ "transition: MANUAL to POSCTL - local position valid, global position not valid",
|
||||
MTT_LOC_POS_VALID,
|
||||
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_POSCTL, TRANSITION_CHANGED },
|
||||
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_POSCTL, TRANSITION_CHANGED },
|
||||
|
||||
{ "transition: POSCTL to MANUAL - local position valid, global position valid",
|
||||
MTT_LOC_POS_VALID,
|
||||
vehicle_status_s::MAIN_STATE_POSCTL, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
|
||||
commander_state_s::MAIN_STATE_POSCTL, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
|
||||
|
||||
// TRANSITION_DENIED tests
|
||||
|
||||
{ "no transition: MANUAL to AUTO_MISSION - global position not valid",
|
||||
MTT_ALL_NOT_VALID,
|
||||
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_MISSION, TRANSITION_DENIED },
|
||||
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_MISSION, TRANSITION_DENIED },
|
||||
|
||||
{ "no transition: MANUAL to AUTO_LOITER - global position not valid",
|
||||
MTT_ALL_NOT_VALID,
|
||||
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_LOITER, TRANSITION_DENIED },
|
||||
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_LOITER, TRANSITION_DENIED },
|
||||
|
||||
{ "no transition: MANUAL to AUTO_RTL - global position not valid, home position not valid",
|
||||
MTT_ALL_NOT_VALID,
|
||||
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
|
||||
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
|
||||
|
||||
{ "no transition: MANUAL to AUTO_RTL - global position not valid, home position valid",
|
||||
MTT_HOME_POS_VALID,
|
||||
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
|
||||
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
|
||||
|
||||
{ "no transition: MANUAL to AUTO_RTL - global position valid, home position not valid",
|
||||
MTT_GLOBAL_POS_VALID,
|
||||
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
|
||||
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
|
||||
|
||||
{ "no transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude not valid",
|
||||
MTT_ROTARY_WING,
|
||||
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ALTCTL, TRANSITION_DENIED },
|
||||
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ALTCTL, TRANSITION_DENIED },
|
||||
|
||||
{ "no transition: MANUAL to POSCTL - local position not valid, global position not valid",
|
||||
MTT_ALL_NOT_VALID,
|
||||
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_POSCTL, TRANSITION_DENIED },
|
||||
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_POSCTL, TRANSITION_DENIED },
|
||||
};
|
||||
|
||||
size_t cMainTransitionTests = sizeof(rgMainTransitionTests) / sizeof(rgMainTransitionTests[0]);
|
||||
|
@ -425,24 +431,30 @@ bool StateMachineHelperTest::mainStateTransitionTest(void)
|
|||
const MainTransitionTest_t* test = &rgMainTransitionTests[i];
|
||||
|
||||
// Setup initial machine state
|
||||
struct vehicle_status_s current_state;
|
||||
current_state.main_state = test->from_state;
|
||||
current_state.is_rotary_wing = test->condition_bits & MTT_ROTARY_WING;
|
||||
current_state.condition_local_altitude_valid = test->condition_bits & MTT_LOC_ALT_VALID;
|
||||
current_state.condition_local_position_valid = test->condition_bits & MTT_LOC_POS_VALID;
|
||||
current_state.condition_home_position_valid = test->condition_bits & MTT_HOME_POS_VALID;
|
||||
current_state.condition_global_position_valid = test->condition_bits & MTT_GLOBAL_POS_VALID;
|
||||
struct vehicle_status_s current_vehicle_status = {};
|
||||
struct commander_state_s current_commander_state = {};
|
||||
struct status_flags_s current_status_flags = {};
|
||||
|
||||
uint8_t main_state_prev = 0;
|
||||
|
||||
current_commander_state.main_state = test->from_state;
|
||||
current_vehicle_status.is_rotary_wing = test->condition_bits & MTT_ROTARY_WING;
|
||||
current_status_flags.condition_local_altitude_valid = test->condition_bits & MTT_LOC_ALT_VALID;
|
||||
current_status_flags.condition_local_position_valid = test->condition_bits & MTT_LOC_POS_VALID;
|
||||
current_status_flags.condition_home_position_valid = test->condition_bits & MTT_HOME_POS_VALID;
|
||||
current_status_flags.condition_global_position_valid = test->condition_bits & MTT_GLOBAL_POS_VALID;
|
||||
|
||||
// Attempt transition
|
||||
transition_result_t result = main_state_transition(¤t_state, test->to_state);
|
||||
transition_result_t result = main_state_transition(¤t_vehicle_status, test->to_state, main_state_prev,
|
||||
¤t_status_flags, ¤t_commander_state);
|
||||
|
||||
// Validate result of transition
|
||||
ut_assert(test->assertMsg, test->expected_transition_result == result);
|
||||
if (test->expected_transition_result == result) {
|
||||
if (test->expected_transition_result == TRANSITION_CHANGED) {
|
||||
ut_assert(test->assertMsg, test->to_state == current_state.main_state);
|
||||
ut_assert(test->assertMsg, test->to_state == current_commander_state.main_state);
|
||||
} else {
|
||||
ut_assert(test->assertMsg, test->from_state == current_state.main_state);
|
||||
ut_assert(test->assertMsg, test->from_state == current_commander_state.main_state);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
@ -4,19 +4,63 @@
|
|||
// you want to recompute the
|
||||
// answers for all of the unit tests
|
||||
|
||||
|
||||
/**
|
||||
* @group Testing
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(TEST_MIN, -1.0f);
|
||||
|
||||
/**
|
||||
* @group Testing
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(TEST_MAX, 1.0f);
|
||||
|
||||
/**
|
||||
* @group Testing
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(TEST_TRIM, 0.5f);
|
||||
|
||||
/**
|
||||
* @group Testing
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(TEST_HP, 10.0f);
|
||||
|
||||
/**
|
||||
* @group Testing
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(TEST_LP, 10.0f);
|
||||
|
||||
/**
|
||||
* @group Testing
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(TEST_P, 0.2f);
|
||||
|
||||
/**
|
||||
* @group Testing
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(TEST_I, 0.1f);
|
||||
|
||||
/**
|
||||
* @group Testing
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(TEST_I_MAX, 1.0f);
|
||||
|
||||
/**
|
||||
* @group Testing
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(TEST_D, 0.01f);
|
||||
|
||||
/**
|
||||
* @group Testing
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(TEST_D_LP, 10.0f);
|
||||
|
||||
/**
|
||||
* @group Testing
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(TEST_MEAN, 1.0f);
|
||||
|
||||
/**
|
||||
* @group Testing
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(TEST_DEV, 2.0f);
|
||||
|
|
|
@ -35,7 +35,6 @@ px4_add_module(
|
|||
MAIN mavlink_tests
|
||||
STACK_MAIN 5000
|
||||
COMPILE_FLAGS
|
||||
-Weffc++
|
||||
-DMAVLINK_FTP_UNIT_TEST
|
||||
-Wno-attributes
|
||||
-Wno-packed
|
||||
|
|
|
@ -94,6 +94,7 @@ bool MavlinkFtpTest::_ack_test(void)
|
|||
0, // size in bytes of data
|
||||
nullptr, // Data to start into FTP message payload
|
||||
&reply); // Payload inside FTP message response
|
||||
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
@ -116,6 +117,7 @@ bool MavlinkFtpTest::_bad_opcode_test(void)
|
|||
0, // size in bytes of data
|
||||
nullptr, // Data to start into FTP message payload
|
||||
&reply); // Payload inside FTP message response
|
||||
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
@ -139,7 +141,8 @@ bool MavlinkFtpTest::_bad_datasize_test(void)
|
|||
_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;
|
||||
((MavlinkFTP::PayloadHeader *)((mavlink_file_transfer_protocol_t *)msg.payload64)->payload)->size =
|
||||
MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN + 1;
|
||||
|
||||
_ftp_server->handle_message(&msg);
|
||||
|
||||
|
@ -174,16 +177,17 @@ bool MavlinkFtpTest::_list_test(void)
|
|||
{ "/", response2, 4, true },
|
||||
};
|
||||
|
||||
for (size_t i=0; i<sizeof(rgTestCases)/sizeof(rgTestCases[0]); i++) {
|
||||
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
|
||||
strlen(test->dir) + 1, // size in bytes of data
|
||||
(uint8_t *)test->dir, // Data to start into FTP message payload
|
||||
&reply); // Payload inside FTP message response
|
||||
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
@ -197,19 +201,23 @@ bool MavlinkFtpTest::_list_test(void)
|
|||
|
||||
// Convert null terminators to seperator char so we can use strok to parse returned data
|
||||
char list_entry[256];
|
||||
for (uint8_t j=0; j<reply->size-1; j++) {
|
||||
|
||||
for (uint8_t j = 0; j < reply->size - 1; j++) {
|
||||
if (reply->data[j] == 0) {
|
||||
list_entry[j] = '|';
|
||||
|
||||
} else {
|
||||
list_entry[j] = reply->data[j];
|
||||
}
|
||||
}
|
||||
list_entry[reply->size-1] = 0;
|
||||
|
||||
list_entry[reply->size - 1] = 0;
|
||||
|
||||
// Loop over returned directory entries trying to find then in the response list
|
||||
char *dir;
|
||||
int response_count = 0;
|
||||
dir = strtok(list_entry, "|");
|
||||
|
||||
while (dir != nullptr) {
|
||||
ut_assert("Returned directory not found in expected response", strstr(test->response, dir));
|
||||
response_count++;
|
||||
|
@ -217,6 +225,7 @@ bool MavlinkFtpTest::_list_test(void)
|
|||
}
|
||||
|
||||
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);
|
||||
|
@ -239,9 +248,10 @@ bool MavlinkFtpTest::_list_eof_test(void)
|
|||
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
|
||||
strlen(dir) + 1, // size in bytes of data
|
||||
(uint8_t *)dir, // Data to start into FTP message payload
|
||||
&reply); // Payload inside FTP message response
|
||||
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
@ -264,9 +274,10 @@ bool MavlinkFtpTest::_open_badfile_test(void)
|
|||
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
|
||||
strlen(dir) + 1, // size in bytes of data
|
||||
(uint8_t *)dir, // Data to start into FTP message payload
|
||||
&reply); // Payload inside FTP message response
|
||||
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
@ -284,7 +295,7 @@ bool MavlinkFtpTest::_open_terminate_test(void)
|
|||
MavlinkFTP::PayloadHeader payload;
|
||||
const MavlinkFTP::PayloadHeader *reply;
|
||||
|
||||
for (size_t i=0; i<sizeof(_rgDownloadTestCases)/sizeof(_rgDownloadTestCases[0]); i++) {
|
||||
for (size_t i = 0; i < sizeof(_rgDownloadTestCases) / sizeof(_rgDownloadTestCases[0]); i++) {
|
||||
struct stat st;
|
||||
const DownloadTestCase *test = &_rgDownloadTestCases[i];
|
||||
|
||||
|
@ -292,9 +303,10 @@ bool MavlinkFtpTest::_open_terminate_test(void)
|
|||
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
|
||||
strlen(test->file) + 1, // size in bytes of data
|
||||
(uint8_t *)test->file, // Data to start into FTP message payload
|
||||
&reply); // Payload inside FTP message response
|
||||
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
@ -304,7 +316,7 @@ bool MavlinkFtpTest::_open_terminate_test(void)
|
|||
|
||||
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);
|
||||
ut_compare("File size incorrect", *((uint32_t *)&reply->data[0]), (uint32_t)st.st_size);
|
||||
|
||||
payload.opcode = MavlinkFTP::kCmdTerminateSession;
|
||||
payload.session = reply->session;
|
||||
|
@ -314,6 +326,7 @@ bool MavlinkFtpTest::_open_terminate_test(void)
|
|||
0, // size in bytes of data
|
||||
nullptr, // Data to start into FTP message payload
|
||||
&reply); // Payload inside FTP message response
|
||||
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
@ -336,9 +349,10 @@ bool MavlinkFtpTest::_terminate_badsession_test(void)
|
|||
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
|
||||
strlen(file) + 1, // size in bytes of data
|
||||
(uint8_t *)file, // Data to start into FTP message payload
|
||||
&reply); // Payload inside FTP message response
|
||||
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
@ -353,6 +367,7 @@ bool MavlinkFtpTest::_terminate_badsession_test(void)
|
|||
0, // size in bytes of data
|
||||
nullptr, // Data to start into FTP message payload
|
||||
&reply); // Payload inside FTP message response
|
||||
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
@ -370,7 +385,7 @@ bool MavlinkFtpTest::_read_test(void)
|
|||
MavlinkFTP::PayloadHeader payload;
|
||||
const MavlinkFTP::PayloadHeader *reply;
|
||||
|
||||
for (size_t i=0; i<sizeof(_rgDownloadTestCases)/sizeof(_rgDownloadTestCases[0]); i++) {
|
||||
for (size_t i = 0; i < sizeof(_rgDownloadTestCases) / sizeof(_rgDownloadTestCases[0]); i++) {
|
||||
struct stat st;
|
||||
const DownloadTestCase *test = &_rgDownloadTestCases[i];
|
||||
|
||||
|
@ -391,9 +406,10 @@ bool MavlinkFtpTest::_read_test(void)
|
|||
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
|
||||
strlen(test->file) + 1, // size in bytes of data
|
||||
(uint8_t *)test->file, // Data to start into FTP message payload
|
||||
&reply); // Payload inside FTP message response
|
||||
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
@ -408,6 +424,7 @@ bool MavlinkFtpTest::_read_test(void)
|
|||
0, // size in bytes of data
|
||||
nullptr, // Data to start into FTP message payload
|
||||
&reply); // Payload inside FTP message response
|
||||
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
@ -428,16 +445,19 @@ bool MavlinkFtpTest::_read_test(void)
|
|||
0, // size in bytes of data
|
||||
nullptr, // Data to start into FTP message payload
|
||||
&reply); // Payload inside FTP message response
|
||||
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
||||
ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
|
||||
|
||||
} else {
|
||||
success = _send_receive_msg(&payload, // FTP payload header
|
||||
0, // size in bytes of data
|
||||
nullptr, // Data to start into FTP message payload
|
||||
&reply); // Payload inside FTP message response
|
||||
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
@ -458,6 +478,7 @@ bool MavlinkFtpTest::_read_test(void)
|
|||
0, // size in bytes of data
|
||||
nullptr, // Data to start into FTP message payload
|
||||
&reply); // Payload inside FTP message response
|
||||
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
@ -478,7 +499,7 @@ bool MavlinkFtpTest::_burst_test(void)
|
|||
|
||||
|
||||
|
||||
for (size_t i=0; i<sizeof(_rgDownloadTestCases)/sizeof(_rgDownloadTestCases[0]); i++) {
|
||||
for (size_t i = 0; i < sizeof(_rgDownloadTestCases) / sizeof(_rgDownloadTestCases[0]); i++) {
|
||||
struct stat st;
|
||||
const DownloadTestCase *test = &_rgDownloadTestCases[i];
|
||||
|
||||
|
@ -499,9 +520,10 @@ bool MavlinkFtpTest::_burst_test(void)
|
|||
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
|
||||
strlen(test->file) + 1, // size in bytes of data
|
||||
(uint8_t *)test->file, // Data to start into FTP message payload
|
||||
&reply); // Payload inside FTP message response
|
||||
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
@ -543,6 +565,7 @@ bool MavlinkFtpTest::_burst_test(void)
|
|||
0, // size in bytes of data
|
||||
nullptr, // Data to start into FTP message payload
|
||||
&reply); // Payload inside FTP message response
|
||||
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
@ -565,9 +588,10 @@ bool MavlinkFtpTest::_read_badsession_test(void)
|
|||
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
|
||||
strlen(file) + 1, // size in bytes of data
|
||||
(uint8_t *)file, // Data to start into FTP message payload
|
||||
&reply); // Payload inside FTP message response
|
||||
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
@ -582,6 +606,7 @@ bool MavlinkFtpTest::_read_badsession_test(void)
|
|||
0, // size in bytes of data
|
||||
nullptr, // Data to start into FTP message payload
|
||||
&reply); // Payload inside FTP message response
|
||||
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
@ -616,7 +641,7 @@ bool MavlinkFtpTest::_removedirectory_test(void)
|
|||
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++) {
|
||||
for (size_t i = 0; i < sizeof(rgTestCases) / sizeof(rgTestCases[0]); i++) {
|
||||
const struct _testCase *test = &rgTestCases[i];
|
||||
|
||||
if (test->deleteFile) {
|
||||
|
@ -627,9 +652,10 @@ bool MavlinkFtpTest::_removedirectory_test(void)
|
|||
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
|
||||
strlen(test->dir) + 1, // size in bytes of data
|
||||
(uint8_t *)test->dir, // Data to start into FTP message payload
|
||||
&reply); // Payload inside FTP message response
|
||||
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
@ -637,6 +663,7 @@ bool MavlinkFtpTest::_removedirectory_test(void)
|
|||
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);
|
||||
|
@ -663,16 +690,17 @@ bool MavlinkFtpTest::_createdirectory_test(void)
|
|||
{ "/fs/microsd/bogus/bogus", false },
|
||||
};
|
||||
|
||||
for (size_t i=0; i<sizeof(rgTestCases)/sizeof(rgTestCases[0]); i++) {
|
||||
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
|
||||
strlen(test->dir) + 1, // size in bytes of data
|
||||
(uint8_t *)test->dir, // Data to start into FTP message payload
|
||||
&reply); // Payload inside FTP message response
|
||||
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
@ -680,6 +708,7 @@ bool MavlinkFtpTest::_createdirectory_test(void)
|
|||
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);
|
||||
|
@ -712,16 +741,17 @@ bool MavlinkFtpTest::_removefile_test(void)
|
|||
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++) {
|
||||
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
|
||||
strlen(test->file) + 1, // size in bytes of data
|
||||
(uint8_t *)test->file, // Data to start into FTP message payload
|
||||
&reply); // Payload inside FTP message response
|
||||
|
||||
if (!success) {
|
||||
return false;
|
||||
}
|
||||
|
@ -729,6 +759,7 @@ bool MavlinkFtpTest::_removefile_test(void)
|
|||
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);
|
||||
|
@ -741,12 +772,12 @@ bool MavlinkFtpTest::_removefile_test(void)
|
|||
|
||||
/// Static method used as callback from MavlinkFTP for generic use. This method will be called by MavlinkFTP when
|
||||
/// it needs to send a message out on Mavlink.
|
||||
void MavlinkFtpTest::receive_message_handler_generic(const mavlink_file_transfer_protocol_t* ftp_req, void *worker_data)
|
||||
void MavlinkFtpTest::receive_message_handler_generic(const mavlink_file_transfer_protocol_t *ftp_req, void *worker_data)
|
||||
{
|
||||
((MavlinkFtpTest*)worker_data)->_receive_message_handler_generic(ftp_req);
|
||||
((MavlinkFtpTest *)worker_data)->_receive_message_handler_generic(ftp_req);
|
||||
}
|
||||
|
||||
void MavlinkFtpTest::_receive_message_handler_generic(const mavlink_file_transfer_protocol_t* ftp_req)
|
||||
void MavlinkFtpTest::_receive_message_handler_generic(const mavlink_file_transfer_protocol_t *ftp_req)
|
||||
{
|
||||
// Move the message into our own member variable
|
||||
memcpy(&_reply_msg, ftp_req, sizeof(mavlink_file_transfer_protocol_t));
|
||||
|
@ -754,16 +785,17 @@ void MavlinkFtpTest::_receive_message_handler_generic(const mavlink_file_transfe
|
|||
|
||||
/// Static method used as callback from MavlinkFTP for stream download testing. This method will be called by MavlinkFTP when
|
||||
/// it needs to send a message out on Mavlink.
|
||||
void MavlinkFtpTest::receive_message_handler_burst(const mavlink_file_transfer_protocol_t* ftp_req, void *worker_data)
|
||||
void MavlinkFtpTest::receive_message_handler_burst(const mavlink_file_transfer_protocol_t *ftp_req, void *worker_data)
|
||||
{
|
||||
BurstInfo* burst_info = (BurstInfo*)worker_data;
|
||||
BurstInfo *burst_info = (BurstInfo *)worker_data;
|
||||
burst_info->ftp_test_class->_receive_message_handler_burst(ftp_req, burst_info);
|
||||
}
|
||||
|
||||
bool MavlinkFtpTest::_receive_message_handler_burst(const mavlink_file_transfer_protocol_t* ftp_msg, BurstInfo* burst_info)
|
||||
bool MavlinkFtpTest::_receive_message_handler_burst(const mavlink_file_transfer_protocol_t *ftp_msg,
|
||||
BurstInfo *burst_info)
|
||||
{
|
||||
hrt_abstime t = 0;
|
||||
const MavlinkFTP::PayloadHeader* reply;
|
||||
const MavlinkFTP::PayloadHeader *reply;
|
||||
uint32_t full_packet_bytes = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader);
|
||||
uint32_t expected_bytes;
|
||||
|
||||
|
@ -846,6 +878,7 @@ void MavlinkFtpTest::_setup_ftp_msg(const MavlinkFTP::PayloadHeader *payload_hea
|
|||
|
||||
payload->seq_number = _expected_seq_number++;
|
||||
payload->size = size;
|
||||
|
||||
if (size != 0) {
|
||||
memcpy(payload->data, data, size);
|
||||
}
|
||||
|
@ -889,7 +922,7 @@ 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_test); // TODO: cmake build system needs to run mavlink_ftp_test_data.py
|
||||
ut_run_test(_list_eof_test);
|
||||
ut_run_test(_open_badfile_test);
|
||||
ut_run_test(_open_terminate_test);
|
||||
|
|
|
@ -48,18 +48,18 @@ public:
|
|||
|
||||
virtual bool run_tests(void);
|
||||
|
||||
static void receive_message_handler_generic(const mavlink_file_transfer_protocol_t* ftp_req, void *worker_data);
|
||||
static void receive_message_handler_generic(const mavlink_file_transfer_protocol_t *ftp_req, void *worker_data);
|
||||
|
||||
/// Worker data for stream handler
|
||||
struct BurstInfo {
|
||||
MavlinkFtpTest* ftp_test_class;
|
||||
MavlinkFtpTest *ftp_test_class;
|
||||
int burst_state;
|
||||
bool single_packet_file;
|
||||
uint32_t file_size;
|
||||
uint8_t* file_bytes;
|
||||
uint8_t *file_bytes;
|
||||
};
|
||||
|
||||
static void receive_message_handler_burst(const mavlink_file_transfer_protocol_t* ftp_req, void *worker_data);
|
||||
static void receive_message_handler_burst(const mavlink_file_transfer_protocol_t *ftp_req, void *worker_data);
|
||||
|
||||
static const uint8_t serverSystemId = 50; ///< System ID for server
|
||||
static const uint8_t serverComponentId = 1; ///< Component ID for server
|
||||
|
@ -69,8 +69,8 @@ public:
|
|||
static const uint8_t clientComponentId = 0; ///< Component ID for client
|
||||
|
||||
// We don't want any of these
|
||||
MavlinkFtpTest(const MavlinkFtpTest&);
|
||||
MavlinkFtpTest& operator=(const MavlinkFtpTest&);
|
||||
MavlinkFtpTest(const MavlinkFtpTest &);
|
||||
MavlinkFtpTest &operator=(const MavlinkFtpTest &);
|
||||
|
||||
private:
|
||||
virtual void _init(void);
|
||||
|
@ -91,8 +91,9 @@ private:
|
|||
bool _createdirectory_test(void);
|
||||
bool _removefile_test(void);
|
||||
|
||||
void _receive_message_handler_generic(const mavlink_file_transfer_protocol_t* ftp_req);
|
||||
void _setup_ftp_msg(const MavlinkFTP::PayloadHeader *payload_header, uint8_t size, const uint8_t *data, mavlink_message_t *msg);
|
||||
void _receive_message_handler_generic(const mavlink_file_transfer_protocol_t *ftp_req);
|
||||
void _setup_ftp_msg(const MavlinkFTP::PayloadHeader *payload_header, uint8_t size, const uint8_t *data,
|
||||
mavlink_message_t *msg);
|
||||
bool _decode_message(const mavlink_file_transfer_protocol_t *ftp_msg, const MavlinkFTP::PayloadHeader **payload);
|
||||
bool _send_receive_msg(MavlinkFTP::PayloadHeader *payload_header,
|
||||
uint8_t size,
|
||||
|
@ -119,9 +120,9 @@ private:
|
|||
burst_state_complete
|
||||
};
|
||||
|
||||
bool _receive_message_handler_burst(const mavlink_file_transfer_protocol_t* ftp_req, BurstInfo* burst_info);
|
||||
bool _receive_message_handler_burst(const mavlink_file_transfer_protocol_t *ftp_req, BurstInfo *burst_info);
|
||||
|
||||
MavlinkFTP* _ftp_server;
|
||||
MavlinkFTP *_ftp_server;
|
||||
uint16_t _expected_seq_number;
|
||||
|
||||
mavlink_file_transfer_protocol_t _reply_msg;
|
||||
|
|
|
@ -43,16 +43,20 @@
|
|||
#include "systemlib/param/param.h"
|
||||
#include "tests.h"
|
||||
|
||||
#define PARAM_MAGIC1 0x12345678
|
||||
#define PARAM_MAGIC1 12345678
|
||||
#define PARAM_MAGIC2 0xa5a5a5a5
|
||||
PARAM_DEFINE_INT32(test, PARAM_MAGIC1);
|
||||
|
||||
/**
|
||||
* @group Testing
|
||||
*/
|
||||
PARAM_DEFINE_INT32(TEST_PARAMS, 12345678);
|
||||
|
||||
int
|
||||
test_param(int argc, char *argv[])
|
||||
{
|
||||
param_t p;
|
||||
|
||||
p = param_find("test");
|
||||
p = param_find("TEST_PARAMS");
|
||||
|
||||
if (p == PARAM_INVALID) {
|
||||
warnx("test parameter not found");
|
||||
|
@ -71,7 +75,7 @@ test_param(int argc, char *argv[])
|
|||
return 1;
|
||||
}
|
||||
|
||||
int32_t val;
|
||||
int32_t val = -1;
|
||||
|
||||
if (param_get(p, &val) != OK) {
|
||||
warnx("failed to read test parameter");
|
||||
|
|
|
@ -166,15 +166,20 @@ test_all(int argc, char *argv[])
|
|||
passed[i] = true;
|
||||
}
|
||||
|
||||
for (int j = 0; j < 40; j++) {
|
||||
printf("-");
|
||||
}
|
||||
|
||||
printf("\n\n");
|
||||
|
||||
testcount++;
|
||||
}
|
||||
}
|
||||
|
||||
/* Print summary */
|
||||
printf("\n");
|
||||
int j;
|
||||
|
||||
for (j = 0; j < 40; j++) {
|
||||
for (int j = 0; j < 40; j++) {
|
||||
printf("-");
|
||||
}
|
||||
|
||||
|
|
Loading…
Reference in New Issue