From 2bc74fd5d956a22636af71451c748f0906936b78 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 22 Apr 2016 15:15:37 -0400 Subject: [PATCH] restore px4fmu-v2_test --- Makefile | 5 +- ROMFS/px4fmu_test/init.d/rc.sensors | 201 ++++++++ ROMFS/px4fmu_test/init.d/rcS | 87 +--- Tools/check_code_style_all.sh | 1 + Tools/px_process_airframes.py | 2 +- Tools/px_process_params.py | 2 +- cmake/configs/nuttx_px4fmu-v2_test.cmake | 202 ++++++++ src/firmware/nuttx/CMakeLists.txt | 11 +- src/modules/commander/commander.cpp | 6 +- .../state_machine_helper_test.cpp | 98 ++-- src/modules/controllib_test/test_params.c | 44 ++ .../mavlink/mavlink_tests/CMakeLists.txt | 1 - .../mavlink_tests/mavlink_ftp_test.cpp | 445 ++++++++++-------- .../mavlink/mavlink_tests/mavlink_ftp_test.h | 61 +-- src/systemcmds/tests/test_params.c | 12 +- src/systemcmds/tests/tests_main.c | 9 +- 16 files changed, 818 insertions(+), 369 deletions(-) create mode 100644 ROMFS/px4fmu_test/init.d/rc.sensors create mode 100644 cmake/configs/nuttx_px4fmu-v2_test.cmake diff --git a/Makefile b/Makefile index 437864293a..61b9a05337 100644 --- a/Makefile +++ b/Makefile @@ -136,6 +136,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") diff --git a/ROMFS/px4fmu_test/init.d/rc.sensors b/ROMFS/px4fmu_test/init.d/rc.sensors new file mode 100644 index 0000000000..9a4d12192e --- /dev/null +++ b/ROMFS/px4fmu_test/init.d/rc.sensors @@ -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 diff --git a/ROMFS/px4fmu_test/init.d/rcS b/ROMFS/px4fmu_test/init.d/rcS index 0df6c29071..abb4f3dfb8 100644 --- a/ROMFS/px4fmu_test/init.d/rcS +++ b/ROMFS/px4fmu_test/init.d/rcS @@ -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 diff --git a/Tools/check_code_style_all.sh b/Tools/check_code_style_all.sh index 20124e00a2..eba8648f25 100755 --- a/Tools/check_code_style_all.sh +++ b/Tools/check_code_style_all.sh @@ -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 \ diff --git a/Tools/px_process_airframes.py b/Tools/px_process_airframes.py index aa7e57a8c8..05f9c85f2a 100755 --- a/Tools/px_process_airframes.py +++ b/Tools/px_process_airframes.py @@ -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 diff --git a/Tools/px_process_params.py b/Tools/px_process_params.py index 29206275bf..7bac8834d3 100644 --- a/Tools/px_process_params.py +++ b/Tools/px_process_params.py @@ -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 diff --git a/cmake/configs/nuttx_px4fmu-v2_test.cmake b/cmake/configs/nuttx_px4fmu-v2_test.cmake new file mode 100644 index 0000000000..0adab48b85 --- /dev/null +++ b/cmake/configs/nuttx_px4fmu-v2_test.cmake @@ -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") diff --git a/src/firmware/nuttx/CMakeLists.txt b/src/firmware/nuttx/CMakeLists.txt index 926d331ec1..73215f797f 100644 --- a/src/firmware/nuttx/CMakeLists.txt +++ b/src/firmware/nuttx/CMakeLists.txt @@ -45,16 +45,21 @@ if(NOT ${BOARD} STREQUAL "sim") if (config_io_board) 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} diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3df0fbedeb..8f6452ee85 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -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; diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index d50d0a4d92..2c8b4675dd 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -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; icurrent_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); @@ -304,10 +310,10 @@ bool StateMachineHelperTest::mainStateTransitionTest(void) { // This structure represent a single test case for testing Main State transitions. 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 - main_state_t to_state; // State to transition to + const char* assertMsg; // Text to show when test case fails + uint8_t condition_bits; // Bits for various condition_* values + 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); } } } diff --git a/src/modules/controllib_test/test_params.c b/src/modules/controllib_test/test_params.c index 7c609cad39..5ef469b3e2 100644 --- a/src/modules/controllib_test/test_params.c +++ b/src/modules/controllib_test/test_params.c @@ -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); diff --git a/src/modules/mavlink/mavlink_tests/CMakeLists.txt b/src/modules/mavlink/mavlink_tests/CMakeLists.txt index 710f15cd8f..8096965c3d 100644 --- a/src/modules/mavlink/mavlink_tests/CMakeLists.txt +++ b/src/modules/mavlink/mavlink_tests/CMakeLists.txt @@ -35,7 +35,6 @@ px4_add_module( MAIN mavlink_tests STACK_MAIN 5000 COMPILE_FLAGS - -Weffc++ -DMAVLINK_FTP_UNIT_TEST -Wno-attributes -Wno-packed diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp index 6fe0ed9c8f..a82c4d45a1 100644 --- a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp +++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp @@ -61,7 +61,7 @@ MavlinkFtpTest::MavlinkFtpTest() : MavlinkFtpTest::~MavlinkFtpTest() { - + } /// @brief Called before every test to initialize the FTP Server. @@ -78,7 +78,7 @@ void MavlinkFtpTest::_init(void) void MavlinkFtpTest::_cleanup(void) { delete _ftp_server; - + _cleanup_microsd(); } @@ -87,20 +87,21 @@ bool MavlinkFtpTest::_ack_test(void) { MavlinkFTP::PayloadHeader payload; const 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 &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; } @@ -109,21 +110,22 @@ bool MavlinkFtpTest::_bad_opcode_test(void) { MavlinkFTP::PayloadHeader payload; const 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 &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; } @@ -133,24 +135,25 @@ bool MavlinkFtpTest::_bad_datasize_test(void) mavlink_message_t msg; MavlinkFTP::PayloadHeader payload; const 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; - + ((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); - + if (!_decode_message(&_reply_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; } @@ -158,10 +161,10 @@ bool MavlinkFtpTest::_list_test(void) { MavlinkFTP::PayloadHeader payload; const 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 @@ -174,56 +177,62 @@ bool MavlinkFtpTest::_list_test(void) { "/", response2, 4, true }, }; - for (size_t i=0; idir)+1, // size in bytes of data - (uint8_t*)test->dir, // Data to start into FTP message payload - &reply); // Payload inside FTP message response + 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; } - + 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 char list_entry[256]; - for (uint8_t j=0; jsize-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++; 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; } @@ -234,14 +243,15 @@ bool MavlinkFtpTest::_list_eof_test(void) MavlinkFTP::PayloadHeader payload; const 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 - &reply); // Payload inside FTP message response + 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; } @@ -249,7 +259,7 @@ bool MavlinkFtpTest::_list_eof_test(void) 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; } @@ -259,22 +269,23 @@ bool MavlinkFtpTest::_open_badfile_test(void) MavlinkFTP::PayloadHeader payload; const 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 - &reply); // Payload inside FTP message response + 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; } - + 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; } @@ -283,41 +294,43 @@ bool MavlinkFtpTest::_open_terminate_test(void) { MavlinkFTP::PayloadHeader payload; const MavlinkFTP::PayloadHeader *reply; - - for (size_t i=0; ifile)+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; } - + 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); + 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 &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); } @@ -331,36 +344,38 @@ bool MavlinkFtpTest::_terminate_badsession_test(void) MavlinkFTP::PayloadHeader payload; const MavlinkFTP::PayloadHeader *reply; const char *file = _rgDownloadTestCases[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 - &reply); // Payload inside FTP message response + 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; } - + 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 - &reply); // Payload inside FTP message response + 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); ut_compare("Incorrect payload size", reply->size, 1); ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrInvalidSession); - + return true; } @@ -369,11 +384,11 @@ bool MavlinkFtpTest::_read_test(void) { MavlinkFTP::PayloadHeader payload; const MavlinkFTP::PayloadHeader *reply; - - for (size_t i=0; ifile, &st), 0); uint8_t *bytes = new uint8_t[st.st_size]; @@ -383,35 +398,37 @@ bool MavlinkFtpTest::_read_test(void) 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 + 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; } - + 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 &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); @@ -419,29 +436,32 @@ bool MavlinkFtpTest::_read_test(void) uint32_t expected_bytes = test->singlePacketRead ? (uint32_t)st.st_size : full_packet_bytes; ut_compare("Payload size incorrect", reply->size, expected_bytes); ut_compare("File contents differ", memcmp(reply->data, bytes, expected_bytes), 0); - + payload.offset += expected_bytes; - + if (test->singlePacketRead) { // Try going past EOF 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; } - + 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; } - + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); ut_compare("Offset incorrect", reply->offset, payload.offset); @@ -449,23 +469,24 @@ bool MavlinkFtpTest::_read_test(void) ut_compare("Payload size incorrect", reply->size, expected_bytes); ut_compare("File contents differ", memcmp(reply->data, &bytes[payload.offset], expected_bytes), 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 &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; } @@ -477,11 +498,11 @@ bool MavlinkFtpTest::_burst_test(void) BurstInfo burst_info; - - for (size_t i=0; ifile, &st), 0); uint8_t *bytes = new uint8_t[st.st_size]; @@ -491,23 +512,24 @@ bool MavlinkFtpTest::_burst_test(void) 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 + 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; } ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); - + // Setup for burst response handler burst_info.burst_state = burst_state_first_ack; burst_info.single_packet_file = test->singlePacketRead; @@ -520,7 +542,7 @@ bool MavlinkFtpTest::_burst_test(void) payload.opcode = MavlinkFTP::kCmdBurstReadFile; payload.session = reply->session; payload.offset = 0; - + mavlink_message_t msg; _setup_ftp_msg(&payload, 0, nullptr, &msg); _ftp_server->handle_message(&msg); @@ -528,29 +550,30 @@ bool MavlinkFtpTest::_burst_test(void) // First packet is sent using stream mechanism, so we need to force it out ourselves hrt_abstime t = 0; _ftp_server->send(t); - + ut_compare("Incorrect sequence of messages", burst_info.burst_state, burst_state_complete); - + // Put back generic message handler _ftp_server->set_unittest_worker(MavlinkFtpTest::receive_message_handler_generic, this); - + // Terminate session 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 &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; } @@ -560,36 +583,38 @@ bool MavlinkFtpTest::_read_badsession_test(void) MavlinkFTP::PayloadHeader payload; const MavlinkFTP::PayloadHeader *reply; const char *file = _rgDownloadTestCases[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 + 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; } - + 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 &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; } @@ -598,7 +623,7 @@ bool MavlinkFtpTest::_removedirectory_test(void) MavlinkFTP::PayloadHeader payload; const MavlinkFTP::PayloadHeader *reply; int fd; - + struct _testCase { const char *dir; bool success; @@ -611,39 +636,41 @@ bool MavlinkFtpTest::_removedirectory_test(void) { _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; ideleteFile) { 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 + 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; } - + 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; } @@ -651,7 +678,7 @@ bool MavlinkFtpTest::_createdirectory_test(void) { MavlinkFTP::PayloadHeader payload; const MavlinkFTP::PayloadHeader *reply; - + struct _testCase { const char *dir; bool success; @@ -662,31 +689,33 @@ bool MavlinkFtpTest::_createdirectory_test(void) { _unittest_microsd_dir, false }, { "/fs/microsd/bogus/bogus", false }, }; - - for (size_t i=0; idir)+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; } - + 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; } @@ -695,7 +724,7 @@ bool MavlinkFtpTest::_removefile_test(void) MavlinkFTP::PayloadHeader payload; const MavlinkFTP::PayloadHeader *reply; int fd; - + struct _testCase { const char *file; bool success; @@ -707,46 +736,48 @@ bool MavlinkFtpTest::_removefile_test(void) { _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; ifile)+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; } - + 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; } /// 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,62 +785,63 @@ 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; - + _decode_message(ftp_msg, &reply); switch (burst_info->burst_state) { - case burst_state_first_ack: - ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); - ut_compare("Offset incorrect", reply->offset, 0); - - expected_bytes = burst_info->single_packet_file ? burst_info->file_size : full_packet_bytes; - ut_compare("Payload size incorrect", reply->size, expected_bytes); - ut_compare("burst_complete incorrect", reply->burst_complete, 0); - ut_compare("File contents differ", memcmp(reply->data, burst_info->file_bytes, expected_bytes), 0); - - // Setup for next expected message - burst_info->burst_state = burst_info->single_packet_file ? burst_state_nak_eof : burst_state_last_ack; - - ut_assert("Remaining stream packets missing", _ftp_server->get_size()); - _ftp_server->send(t); - break; - - case burst_state_last_ack: - ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); - ut_compare("Offset incorrect", reply->offset, full_packet_bytes); - - expected_bytes = burst_info->file_size - full_packet_bytes; - ut_compare("Payload size incorrect", reply->size, expected_bytes); - ut_compare("burst_complete incorrect", reply->burst_complete, 0); - ut_compare("File contents differ", memcmp(reply->data, &burst_info->file_bytes[full_packet_bytes], expected_bytes), 0); - - // Setup for next expected message - burst_info->burst_state = burst_state_nak_eof; - - ut_assert("Remaining stream packets missing", _ftp_server->get_size()); - _ftp_server->send(t); - break; - - case burst_state_nak_eof: - // Signal complete - burst_info->burst_state = burst_state_complete; - ut_compare("All packets should have been seent", _ftp_server->get_size(), 0); - break; - + case burst_state_first_ack: + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Offset incorrect", reply->offset, 0); + + expected_bytes = burst_info->single_packet_file ? burst_info->file_size : full_packet_bytes; + ut_compare("Payload size incorrect", reply->size, expected_bytes); + ut_compare("burst_complete incorrect", reply->burst_complete, 0); + ut_compare("File contents differ", memcmp(reply->data, burst_info->file_bytes, expected_bytes), 0); + + // Setup for next expected message + burst_info->burst_state = burst_info->single_packet_file ? burst_state_nak_eof : burst_state_last_ack; + + ut_assert("Remaining stream packets missing", _ftp_server->get_size()); + _ftp_server->send(t); + break; + + case burst_state_last_ack: + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Offset incorrect", reply->offset, full_packet_bytes); + + expected_bytes = burst_info->file_size - full_packet_bytes; + ut_compare("Payload size incorrect", reply->size, expected_bytes); + ut_compare("burst_complete incorrect", reply->burst_complete, 0); + ut_compare("File contents differ", memcmp(reply->data, &burst_info->file_bytes[full_packet_bytes], expected_bytes), 0); + + // Setup for next expected message + burst_info->burst_state = burst_state_nak_eof; + + ut_assert("Remaining stream packets missing", _ftp_server->get_size()); + _ftp_server->send(t); + break; + + case burst_state_nak_eof: + // Signal complete + burst_info->burst_state = burst_state_complete; + ut_compare("All packets should have been seent", _ftp_server->get_size(), 0); + break; + } - + return true; } @@ -818,18 +850,18 @@ bool MavlinkFtpTest::_decode_message(const mavlink_file_transfer_protocol_t *ftp const MavlinkFTP::PayloadHeader **payload) ///< Payload inside FTP message response { //warnx("_decode_message"); - + // 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(ftp_msg->payload); - + // Make sure we have a good sequence number ut_compare("Sequence number mismatch", (*payload)->seq_number, _expected_seq_number); _expected_seq_number++; - + return true; } @@ -841,18 +873,19 @@ void MavlinkFtpTest::_setup_ftp_msg(const MavlinkFTP::PayloadHeader *payload_hea { uint8_t payload_bytes[MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN]; MavlinkFTP::PayloadHeader *payload = reinterpret_cast(payload_bytes); - + memcpy(payload, payload_header, sizeof(MavlinkFTP::PayloadHeader)); - + payload->seq_number = _expected_seq_number++; payload->size = size; + if (size != 0) { memcpy(payload->data, data, size); } - + payload->burst_complete = 0; payload->padding = 0; - + msg->checksum = 0; mavlink_msg_file_transfer_protocol_pack(clientSystemId, // Sender system id clientComponentId, // Sender component id @@ -865,9 +898,9 @@ void MavlinkFtpTest::_setup_ftp_msg(const MavlinkFTP::PayloadHeader *payload_hea /// @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 - const MavlinkFTP::PayloadHeader **payload_reply) ///< Payload inside FTP message response + uint8_t size, ///< size in bytes of data + const uint8_t *data, ///< Data to start into FTP message payload + const MavlinkFTP::PayloadHeader **payload_reply) ///< Payload inside FTP message response { mavlink_message_t msg; @@ -875,7 +908,7 @@ bool MavlinkFtpTest::_send_receive_msg(MavlinkFTP::PayloadHeader *payload_header _ftp_server->handle_message(&msg); return _decode_message(&_reply_msg, payload_reply); } - + /// @brief Cleans up an files created on microsd during testing void MavlinkFtpTest::_cleanup_microsd(void) { @@ -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); @@ -900,7 +933,7 @@ bool MavlinkFtpTest::run_tests(void) ut_run_test(_removedirectory_test); ut_run_test(_createdirectory_test); ut_run_test(_removefile_test); - + return (_tests_failed == 0); } diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h index 14c9369b05..7708d2a630 100644 --- a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h +++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h @@ -45,37 +45,37 @@ class MavlinkFtpTest : public UnitTest public: MavlinkFtpTest(); virtual ~MavlinkFtpTest(); - + 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 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&); - + 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); @@ -90,16 +90,17 @@ private: bool _removedirectory_test(void); 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, - const uint8_t *data, - const MavlinkFTP::PayloadHeader **payload_reply); + uint8_t size, + const uint8_t *data, + const MavlinkFTP::PayloadHeader **payload_reply); void _cleanup_microsd(void); - + /// A single download test case struct DownloadTestCase { const char *file; @@ -107,10 +108,10 @@ private: bool singlePacketRead; bool exactlyFillPacket; }; - + /// The set of test cases for download testing static const DownloadTestCase _rgDownloadTestCases[]; - + /// States for stream download handler enum { burst_state_first_ack, @@ -118,14 +119,14 @@ private: burst_state_nak_eof, burst_state_complete }; - - bool _receive_message_handler_burst(const mavlink_file_transfer_protocol_t* ftp_req, BurstInfo* burst_info); - - MavlinkFTP* _ftp_server; + + bool _receive_message_handler_burst(const mavlink_file_transfer_protocol_t *ftp_req, BurstInfo *burst_info); + + MavlinkFTP *_ftp_server; uint16_t _expected_seq_number; - + mavlink_file_transfer_protocol_t _reply_msg; - + static const char _unittest_microsd_dir[]; static const char _unittest_microsd_file[]; }; diff --git a/src/systemcmds/tests/test_params.c b/src/systemcmds/tests/test_params.c index 09845ee25c..228d88fb9e 100644 --- a/src/systemcmds/tests/test_params.c +++ b/src/systemcmds/tests/test_params.c @@ -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"); diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index 7c7cbeb28f..453705eaf4 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -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("-"); }