diff --git a/Makefile b/Makefile index e9ab6f06e1..ef7db710dd 100644 --- a/Makefile +++ b/Makefile @@ -154,7 +154,8 @@ posix_sitl_ekf2: $(call cmake-build,$@) ros_sitl_default: - $(call cmake-build,$@) + @echo "This target is deprecated. Use make 'posix_sitl_default gazebo' instead." +# $(call cmake-build,$@) qurt_eagle_travis: $(call cmake-build,$@) @@ -170,6 +171,12 @@ qurt_eagle_default: posix_eagle_default: $(call cmake-build,$@) + +posix_rpi2_default: + $(call cmake-build,$@) + +posix_rpi2_release: + $(call cmake-build,$@) posix: posix_sitl_default diff --git a/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol b/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol index a9e7ff35b2..cb4f4d5ac3 100644 --- a/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol +++ b/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol @@ -2,7 +2,7 @@ # # @name Duorotor Tailsitter # -# @type VTOL Tailsitter +# @type VTOL Duo Tailsitter # # @maintainer Roman Bapst # diff --git a/ROMFS/px4fmu_common/init.d/13008_QuadRanger b/ROMFS/px4fmu_common/init.d/13008_QuadRanger index 16b701c596..404d0acd13 100644 --- a/ROMFS/px4fmu_common/init.d/13008_QuadRanger +++ b/ROMFS/px4fmu_common/init.d/13008_QuadRanger @@ -18,13 +18,13 @@ then param set PWM_AUX_REV1 1 param set PWM_AUX_REV2 1 - param set MC_ROLL_P 6.5 - param set MC_ROLLRATE_P 0.1 + param set MC_ROLL_P 7.0 + param set MC_ROLLRATE_P 0.25 param set MC_ROLLRATE_I 0.002 param set MC_ROLLRATE_D 0.003 param set MC_ROLLRATE_FF 0.0 - param set MC_PITCH_P 6.5 - param set MC_PITCHRATE_P 0.1 + param set MC_PITCH_P 7.0 + param set MC_PITCHRATE_P 0.23 param set MC_PITCHRATE_I 0.002 param set MC_PITCHRATE_D 0.003 param set MC_PITCHRATE_FF 0.0 diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 5e859ae222..8f87cecd79 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -550,7 +550,7 @@ then # Avoid using ttyS1 for MAVLink on FMUv4 if ver hwcmp PX4FMU_V4 then - set MAVLINK_F "-r 1200 -d /dev/ttyS2" + set MAVLINK_F "-r 1200 -d /dev/ttyS1" # Start MAVLink on Wifi (ESP8266 port) mavlink start -r 800000 -b 921600 -d /dev/ttyS0 fi @@ -560,6 +560,12 @@ then mavlink start $MAVLINK_F unset MAVLINK_F + if ver hwcmp PX4FMU_V4 + then + # Start MAVLink on OSD (TELEM2 port) + mavlink start -r 1200 -d /dev/ttyS2 -b 57600 -m osd + fi + # # MAVLink onboard / TELEM2 # @@ -835,6 +841,11 @@ then fi fi +if ver hwcmp PX4FMU_V4 +then + frsky_telemetry start -d /dev/ttyS6 +fi + if ver hwcmp PX4FMU_V2 then # Check for flow sensor - as it is a background task, launch it last diff --git a/Tools/check_code_style.sh b/Tools/check_code_style.sh index cd42b65c80..54ce08c680 100755 --- a/Tools/check_code_style.sh +++ b/Tools/check_code_style.sh @@ -25,7 +25,6 @@ for fn in $(find src/examples \ src/modules/uORB \ src/modules/bottle_drop \ src/modules/dataman \ - src/modules/fixedwing_backside \ src/modules/segway \ src/modules/local_position_estimator \ src/modules/unit_test \ diff --git a/Tools/generate_listener.py b/Tools/generate_listener.py index f097cb741b..057aa9250f 100755 --- a/Tools/generate_listener.py +++ b/Tools/generate_listener.py @@ -136,12 +136,12 @@ extern "C" __EXPORT int listener_main(int argc, char *argv[]); int listener_main(int argc, char *argv[]) { int sub = -1; orb_id_t ID; - if(argc < 3) { - printf("need at least two arguments: topic name, number of messages to print\\n"); + if(argc < 2) { + printf("need at least two arguments: topic name. [optional number of messages to print]\\n"); return 1; } """) -print("\tunsigned num_msgs = atoi(argv[2]);") +print("\tunsigned num_msgs = (argc > 2) ? atoi(argv[2]) : 1;") print("\tif (strncmp(argv[1],\"%s\",50) == 0) {" % messages[0]) print("\t\tsub = orb_subscribe(ORB_ID(%s));" % messages[0]) print("\t\tID = ORB_ID(%s);" % messages[0]) @@ -154,9 +154,11 @@ for index,m in enumerate(messages[1:]): print("\t\tstruct %s_s container;" % m) print("\t\tmemset(&container, 0, sizeof(container));") print("\t\tbool updated;") - print("\t\tfor (unsigned i = 0; i < num_msgs; i++) {") + print("\t\tunsigned i = 0;") + print("\t\twhile(i < num_msgs) {") print("\t\t\torb_check(sub,&updated);") - print("\t\t\tupdated = true;") + print("\t\t\tif (i == 0) { updated = true; } else { usleep(500); }") + print("\t\t\ti++;") print("\t\t\tif (updated) {") print("\t\tprintf(\"\\nTOPIC: %s #%%d\\n\", i);" % m) print("\t\t\torb_copy(ID,sub,&container);") @@ -190,9 +192,15 @@ for index,m in enumerate(messages[1:]): elif item[0] == "int32": print("\t\t\tprintf(\"%s: %%d\\n\",container.%s);" % (item[1], item[1])) elif item[0] == "uint32": - print("\t\t\tprintf(\"%s: %%d\\n\",container.%s);" % (item[1], item[1])) - elif item[0] == "uint8": print("\t\t\tprintf(\"%s: %%u\\n\",container.%s);" % (item[1], item[1])) + elif item[0] == "int16": + print("\t\t\tprintf(\"%s: %%d\\n\",(int)container.%s);" % (item[1], item[1])) + elif item[0] == "uint16": + print("\t\t\tprintf(\"%s: %%u\\n\",(unsigned)container.%s);" % (item[1], item[1])) + elif item[0] == "int8": + print("\t\t\tprintf(\"%s: %%d\\n\",(int)container.%s);" % (item[1], item[1])) + elif item[0] == "uint8": + print("\t\t\tprintf(\"%s: %%u\\n\",(unsigned)container.%s);" % (item[1], item[1])) elif item[0] == "bool": print("\t\t\tprintf(\"%s: %%s\\n\",container.%s ? \"True\" : \"False\");" % (item[1], item[1])) print("\t\t\t}") diff --git a/Tools/px4airframes/xmlout.py b/Tools/px4airframes/xmlout.py index 841fabd4b5..bd3ac1436a 100644 --- a/Tools/px4airframes/xmlout.py +++ b/Tools/px4airframes/xmlout.py @@ -59,7 +59,7 @@ class XMLOutput(): xml_group.attrib["image"] = "VTOLPlane" elif (group.GetName() == "VTOL Quad Tailsitter"): xml_group.attrib["image"] = "VTOLQuadRotorTailSitter" - elif (group.GetName() == "VTOLTiltRotor"): + elif (group.GetName() == "VTOL Tiltrotor"): xml_group.attrib["image"] = "VTOLTiltRotor" elif (group.GetName() == "Coaxial Helicopter"): xml_group.attrib["image"] = "HelicopterCoaxial" diff --git a/Tools/px_romfs_pruner.py b/Tools/px_romfs_pruner.py index a7984c66dd..67f0b57cbf 100644 --- a/Tools/px_romfs_pruner.py +++ b/Tools/px_romfs_pruner.py @@ -1,8 +1,7 @@ #!/usr/bin/env python ############################################################################ # -# Copyright (C) 2014 PX4 Development Team. All rights reserved. -# Author: Julian Oes +# Copyright (C) 2014-2015 PX4 Development Team. All rights reserved. # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -37,48 +36,53 @@ """ px_romfs_pruner.py: Delete all comments and newlines before ROMFS is converted to an image + +@author: Julian Oes """ from __future__ import print_function -import argparse, re +import argparse +import re import os def main(): + # Parse commandline arguments + parser = argparse.ArgumentParser(description="ROMFS pruner.") + parser.add_argument('--folder', action="store", + help="ROMFS scratch folder.") + args = parser.parse_args() - # Parse commandline arguments - parser = argparse.ArgumentParser(description="ROMFS pruner.") - parser.add_argument('--folder', action="store", help="ROMFS scratch folder.") - args = parser.parse_args() + print("Pruning ROMFS files.") - print("Pruning ROMFS files.") + # go through + for (root, dirs, files) in os.walk(args.folder): + for file in files: + # only prune text files + if ".zip" in file or ".bin" in file or ".swp" in file \ + or ".data" in file or ".DS_Store" in file \ + or file.startswith("."): + continue - # go through - for (root, dirs, files) in os.walk(args.folder): - for file in files: - # only prune text files - if ".zip" in file or ".bin" in file or ".swp" in file or ".data" in file or ".DS_Store" in file: - continue + file_path = os.path.join(root, file) - file_path = os.path.join(root, file) - - # read file line by line - pruned_content = "" - with open(file_path, "rU") as f: - for line in f: - - # handle mixer files differently than startup files - if file_path.endswith(".mix"): - if line.startswith(("Z:", "M:", "R: ", "O:", "S:")): - pruned_content += line - else: - if not line.isspace() and not line.strip().startswith("#"): - pruned_content += line - # overwrite old scratch file - with open(file_path, "wb") as f: - pruned_content = re.sub("\r\n", "\n", pruned_content) - f.write(pruned_content.encode("ascii", errors='strict')) + # read file line by line + pruned_content = "" + with open(file_path, "rU") as f: + for line in f: + # handle mixer files differently than startup files + if file_path.endswith(".mix"): + if line.startswith(("Z:", "M:", "R: ", "O:", "S:")): + pruned_content += line + else: + if not line.isspace() \ + and not line.strip().startswith("#"): + pruned_content += line + # overwrite old scratch file + with open(file_path, "wb") as f: + pruned_content = re.sub("\r\n", "\n", pruned_content) + f.write(pruned_content.encode("ascii", errors='strict')) if __name__ == '__main__': - main() + main() diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index 4755eb24ff..596c967407 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -449,8 +449,9 @@ class uploader(object): if self.board_type != fw.property('board_id'): msg = "Firmware not suitable for this board (board_type=%u board_id=%u)" % ( self.board_type, fw.property('board_id')) + print("WARNING: %s" % msg) if args.force: - print("WARNING: %s" % msg) + print("FORCED WRITE, FLASHING ANYWAY!") else: raise IOError(msg) if self.fw_maxsize < fw.property('image_size'): diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo index 57a54ab737..16f7fb3761 160000 --- a/Tools/sitl_gazebo +++ b/Tools/sitl_gazebo @@ -1 +1 @@ -Subproject commit 57a54ab73740f45f7313cc8179e21cc3b0d53b41 +Subproject commit 16f7fb37619e4af772a543f7bac62bef5126347b diff --git a/cmake/configs/posix_rpi2_default.cmake b/cmake/configs/posix_rpi2_default.cmake new file mode 100644 index 0000000000..c745070c9e --- /dev/null +++ b/cmake/configs/posix_rpi2_default.cmake @@ -0,0 +1,47 @@ +include(posix/px4_impl_posix) + +set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-native.cmake) + +set(config_module_list + drivers/device + platforms/common + platforms/posix/px4_layer + platforms/posix/work_queue + systemcmds/param + systemcmds/mixer + systemcmds/ver + systemcmds/esc_calib + systemcmds/reboot + systemcmds/topic_listener + systemcmds/perf + modules/uORB + modules/param + modules/systemlib + modules/systemlib/mixer + modules/sensors + modules/mavlink + modules/attitude_estimator_q + modules/position_estimator_inav + modules/navigator + modules/vtol_att_control + modules/mc_pos_control + modules/mc_att_control + modules/land_detector + modules/fw_att_control + modules/fw_pos_control_l1 + modules/dataman + modules/sdlog2 + modules/commander + modules/controllib + lib/mathlib + lib/mathlib/math/filter + lib/conversion + lib/ecl + lib/external_lgpl + lib/geo + lib/geo_lookup + lib/launchdetection + lib/terrain_estimation + lib/runway_takeoff + lib/tailsitter_recovery +) diff --git a/cmake/configs/posix_rpi2_release.cmake b/cmake/configs/posix_rpi2_release.cmake new file mode 100644 index 0000000000..69813fb1f7 --- /dev/null +++ b/cmake/configs/posix_rpi2_release.cmake @@ -0,0 +1,56 @@ +include(posix/px4_impl_posix) + +if ("${RPI_TOOLCHAIN_DIR}" STREQUAL "") + set(RPI_TOOLCHAIN_DIR /opt/rpi_toolchain) +endif() + +set(CMAKE_PROGRAM_PATH + "${RPI_TOOLCHAIN_DIR}/gcc-linaro-arm-linux-gnueabihf-raspbian/bin" + ${CMAKE_PROGRAM_PATH} + ) + +set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-linux-gnueabihf.cmake) + +set(config_module_list + drivers/device + platforms/common + platforms/posix/px4_layer + platforms/posix/work_queue + systemcmds/param + systemcmds/mixer + systemcmds/ver + systemcmds/esc_calib + systemcmds/reboot + systemcmds/topic_listener + systemcmds/perf + modules/uORB + modules/param + modules/systemlib + modules/systemlib/mixer + modules/sensors + modules/mavlink + modules/attitude_estimator_q + modules/position_estimator_inav + modules/navigator + modules/vtol_att_control + modules/mc_pos_control + modules/mc_att_control + modules/land_detector + modules/fw_att_control + modules/fw_pos_control_l1 + modules/dataman + modules/sdlog2 + modules/commander + modules/controllib + lib/mathlib + lib/mathlib/math/filter + lib/conversion + lib/ecl + lib/external_lgpl + lib/geo + lib/geo_lookup + lib/launchdetection + lib/terrain_estimation + lib/runway_takeoff + lib/tailsitter_recovery +) diff --git a/cmake/configs/posix_sitl_default.cmake b/cmake/configs/posix_sitl_default.cmake index 66eca5205d..2ccdb33daa 100644 --- a/cmake/configs/posix_sitl_default.cmake +++ b/cmake/configs/posix_sitl_default.cmake @@ -34,6 +34,7 @@ set(config_module_list modules/mavlink modules/attitude_estimator_ekf modules/attitude_estimator_q + modules/ekf2 modules/ekf_att_pos_estimator modules/position_estimator_inav modules/navigator diff --git a/msg/ekf2_innovations.msg b/msg/ekf2_innovations.msg new file mode 100644 index 0000000000..cb2ab03146 --- /dev/null +++ b/msg/ekf2_innovations.msg @@ -0,0 +1,7 @@ +uint64 timestamp # Timestamp in microseconds since boot +float32[6] vel_pos_innov # velocity and position innovations +float32[3] mag_innov # earth magnetic field innovations +float32 heading_innov # heading innovation +float32[6] vel_pos_innov_var # velocity and position innovation variances +float32[3] mag_innov_var # earth magnetic field innovation variance +float32 heading_innov_var # heading innovation variance diff --git a/msg/manual_control_setpoint.msg b/msg/manual_control_setpoint.msg index 2ff6fca1f5..424453d699 100644 --- a/msg/manual_control_setpoint.msg +++ b/msg/manual_control_setpoint.msg @@ -38,8 +38,9 @@ float32 aux5 # default function: payload drop uint8 mode_switch # main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO uint8 return_switch # return to launch 2 position switch (mandatory): _NORMAL_, RTL -uint8 rattitude_switch # rattitude control 2 position switch (optional): _MANUAL, RATTITUDE +uint8 rattitude_switch # rattitude control 2 position switch (optional): _MANUAL, RATTITUDE uint8 posctl_switch # position control 2 position switch (optional): _ALTCTL_, POSCTL uint8 loiter_switch # loiter 2 position switch (optional): _MISSION_, LOITER uint8 acro_switch # acro 2 position switch (optional): _MANUAL_, ACRO -uint8 offboard_switch # offboard 2 position switch (optional): _NORMAL_, OFFBOARD +uint8 offboard_switch # offboard 2 position switch (optional): _NORMAL_, OFFBOARD +uint8 kill_switch # throttle kill: _NORMAL_, KILL diff --git a/msg/parameter_update.msg b/msg/parameter_update.msg index 39dc336e05..78cebf7f4f 100644 --- a/msg/parameter_update.msg +++ b/msg/parameter_update.msg @@ -1 +1,2 @@ uint64 timestamp # time at which the latest parameter was updated +bool saved # wether the change has already been saved to disk diff --git a/msg/rc_channels.msg b/msg/rc_channels.msg index 6b30794e00..3171fbc106 100644 --- a/msg/rc_channels.msg +++ b/msg/rc_channels.msg @@ -1,4 +1,4 @@ -int32 RC_CHANNELS_FUNCTION_MAX=20 +int32 RC_CHANNELS_FUNCTION_MAX=21 uint8 RC_CHANNELS_FUNCTION_THROTTLE=0 uint8 RC_CHANNELS_FUNCTION_ROLL=1 uint8 RC_CHANNELS_FUNCTION_PITCH=2 @@ -19,11 +19,12 @@ uint8 RC_CHANNELS_FUNCTION_PARAM_1=16 uint8 RC_CHANNELS_FUNCTION_PARAM_2=17 uint8 RC_CHANNELS_FUNCTION_PARAM_3_5=18 uint8 RC_CHANNELS_FUNCTION_RATTITUDE=19 +uint8 RC_CHANNELS_FUNCTION_KILLSWITCH=20 uint64 timestamp # Timestamp in microseconds since boot time uint64 timestamp_last_valid # Timestamp of last valid RC signal -float32[20] channels # Scaled to -1..1 (throttle: 0..1) +float32[18] channels # Scaled to -1..1 (throttle: 0..1) uint8 channel_count # Number of valid channels -int8[20] function # Functions mapping +int8[21] function # Functions mapping uint8 rssi # Receive signal strength index bool signal_lost # Control signal lost, should be checked together with topic timeout uint32 frame_drop_count # Number of dropped frames diff --git a/nuttx-configs/px4io-v2/nsh/defconfig b/nuttx-configs/px4io-v2/nsh/defconfig index a9b8477948..6871908315 100755 --- a/nuttx-configs/px4io-v2/nsh/defconfig +++ b/nuttx-configs/px4io-v2/nsh/defconfig @@ -164,8 +164,8 @@ CONFIG_USART1_SERIAL_CONSOLE=y CONFIG_USART2_SERIAL_CONSOLE=n CONFIG_USART3_SERIAL_CONSOLE=n -CONFIG_USART1_TXBUFSIZE=64 -CONFIG_USART2_TXBUFSIZE=64 +CONFIG_USART1_TXBUFSIZE=32 +CONFIG_USART2_TXBUFSIZE=32 CONFIG_USART3_TXBUFSIZE=64 CONFIG_USART1_RXBUFSIZE=64 diff --git a/posix-configs/SITL/init/rcS_jmavsim_iris b/posix-configs/SITL/init/rcS_jmavsim_iris index 6fd1d59db1..2b0899f4bb 100644 --- a/posix-configs/SITL/init/rcS_jmavsim_iris +++ b/posix-configs/SITL/init/rcS_jmavsim_iris @@ -51,8 +51,7 @@ sensors start commander start land_detector start multicopter navigator start -attitude_estimator_q start -position_estimator_inav start +ekf2 start mc_pos_control start mc_att_control start mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x.main.mix diff --git a/posix-configs/SITL/init/rcS_lpe_jmavsim_iris b/posix-configs/SITL/init/rcS_lpe_jmavsim_iris index 7d59fe876f..75ca7ea09a 100644 --- a/posix-configs/SITL/init/rcS_lpe_jmavsim_iris +++ b/posix-configs/SITL/init/rcS_lpe_jmavsim_iris @@ -3,8 +3,10 @@ simulator start -s param load param set MAV_TYPE 2 param set MC_PITCHRATE_P 0.15 +param set MC_PITCH_P 7 +param set MC_ROLL_P 7 param set MC_ROLLRATE_P 0.15 -param set MC_YAW_P 2.0 +param set MC_YAW_P 2.8 param set MC_YAWRATE_P 0.35 param set SYS_AUTOSTART 4010 param set SYS_RESTART_TYPE 2 @@ -26,12 +28,22 @@ param set MPC_XY_P 0.4 param set MPC_XY_VEL_P 0.2 param set MPC_XY_VEL_D 0.005 param set SENS_BOARD_ROT 0 +param set COM_RC_IN_MODE 1 +param set NAV_ACC_RAD 2.0 +param set RTL_RETURN_ALT 30.0 +param set RTL_DESCEND_ALT 10.0 +param set MIS_TAKEOFF_ALT 5.0 +param set MPC_HOLD_MAX_Z 2.0 +param set MPC_HOLD_XY_DZ 0.1 +param set MPC_HOLD_Z_DZ 0.1 +param set MPC_Z_VEL_MAX 2.0 +param set MPC_Z_VEL_P 0.4 # changes for LPE param set COM_RC_IN_MODE 1 param set LPE_BETA_MAX 10000 -rgbled start +rgbledsim start tone_alarm start gyrosim start accelsim start @@ -49,6 +61,7 @@ mc_pos_control start mc_att_control start mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x.main.mix mavlink start -u 14556 -r 2000000 +mavlink start -u 14557 -r 2000000 -m onboard mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556 mavlink stream -r 80 -s LOCAL_POSITION_NED_COV -u 14556 mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556 @@ -57,6 +70,7 @@ mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556 mavlink stream -r 20 -s RC_CHANNELS -u 14556 mavlink stream -r 250 -s HIGHRES_IMU -u 14556 mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556 +mavlink stream -r 20 -s MANUAL_CONTROL -u 14556 mavlink boot_complete sdlog2 start -r 100 -e -t -a diff --git a/posix-configs/rpi2/init/rcS_navio b/posix-configs/rpi2/init/rcS_navio new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/drivers/boards/px4fmu-v4/board_config.h b/src/drivers/boards/px4fmu-v4/board_config.h index 062f817998..ea82a7a031 100644 --- a/src/drivers/boards/px4fmu-v4/board_config.h +++ b/src/drivers/boards/px4fmu-v4/board_config.h @@ -154,13 +154,12 @@ __BEGIN_DECLS * * These are the channel numbers of the ADCs of the microcontroller that can be used by the Px4 Firmware in the adc driver */ -#define ADC_CHANNELS (1 << 2) | (1 << 3) | (1 << 4) | (1 << 10) | (1 << 11) | (1 << 12) | (1 << 13) | (1 << 14) | (1 << 15) +#define ADC_CHANNELS (1 << 2) | (1 << 3) | (1 << 4) | (1 << 10) | (1 << 11) | (1 << 12) | (1 << 13) | (1 << 14) // ADC defines to be used in sensors.cpp to read from a particular channel #define ADC_BATTERY_VOLTAGE_CHANNEL 2 #define ADC_BATTERY_CURRENT_CHANNEL 3 #define ADC_5V_RAIL_SENSE 4 -#define ADC_AIRSPEED_VOLTAGE_CHANNEL 15 /* User GPIOs * diff --git a/src/drivers/boards/px4fmu-v4/px4fmu_init.c b/src/drivers/boards/px4fmu-v4/px4fmu_init.c index 37852242fb..21e1777cbd 100644 --- a/src/drivers/boards/px4fmu-v4/px4fmu_init.c +++ b/src/drivers/boards/px4fmu-v4/px4fmu_init.c @@ -219,10 +219,6 @@ __EXPORT int nsh_archinitialize(void) stm32_configgpio(GPIO_ADC1_IN3); /* BATT_CURRENT_SENS */ stm32_configgpio(GPIO_ADC1_IN4); /* VDD_5V_SENS */ - stm32_configgpio(GPIO_ADC1_IN13); /* FMU_AUX_ADC_1 */ - stm32_configgpio(GPIO_ADC1_IN14); /* FMU_AUX_ADC_2 */ - stm32_configgpio(GPIO_ADC1_IN15); /* PRESSURE_SENS */ - /* configure power supply control/sense pins */ stm32_configgpio(GPIO_PERIPH_3V3_EN); stm32_configgpio(GPIO_VDD_BRICK_VALID); @@ -306,7 +302,7 @@ __EXPORT int nsh_archinitialize(void) */ // XXX start with 10.4 MHz and go up to 20 once validated - SPI_SETFREQUENCY(spi2, 12 * 1000 * 1000); + SPI_SETFREQUENCY(spi2, 20 * 1000 * 1000); SPI_SETBITS(spi2, 8); SPI_SETMODE(spi2, SPIDEV_MODE3); SPI_SELECT(spi2, SPIDEV_FLASH, false); diff --git a/src/drivers/boards/px4fmu-v4/px4fmu_pwm_servo.c b/src/drivers/boards/px4fmu-v4/px4fmu_pwm_servo.c index 600dfef412..ef2606d07d 100644 --- a/src/drivers/boards/px4fmu-v4/px4fmu_pwm_servo.c +++ b/src/drivers/boards/px4fmu-v4/px4fmu_pwm_servo.c @@ -70,36 +70,36 @@ __EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = { .gpio = GPIO_TIM1_CH4OUT, .timer_index = 0, .timer_channel = 4, - .default_value = 1000, + .default_value = 900, }, { .gpio = GPIO_TIM1_CH3OUT, .timer_index = 0, .timer_channel = 3, - .default_value = 1000, + .default_value = 900, }, { .gpio = GPIO_TIM1_CH2OUT, .timer_index = 0, .timer_channel = 2, - .default_value = 1000, + .default_value = 900, }, { .gpio = GPIO_TIM1_CH1OUT, .timer_index = 0, .timer_channel = 1, - .default_value = 1000, + .default_value = 900, }, { .gpio = GPIO_TIM4_CH2OUT, .timer_index = 1, .timer_channel = 2, - .default_value = 1000, + .default_value = 900, }, { .gpio = GPIO_TIM4_CH3OUT, .timer_index = 1, .timer_channel = 3, - .default_value = 1000, + .default_value = 900, } }; diff --git a/src/drivers/device/spi.h b/src/drivers/device/spi.h index 9c3bf36f25..986a322432 100644 --- a/src/drivers/device/spi.h +++ b/src/drivers/device/spi.h @@ -77,6 +77,15 @@ protected: int irq = 0); virtual ~SPI(); + /** + * Locking modes supported by the driver. + */ + enum LockMode { + LOCK_PREEMPTION, /**< the default; lock against all forms of preemption. */ + LOCK_THREADS, /**< lock only against other threads, using SPI_LOCK */ + LOCK_NONE /**< perform no locking, only safe if the bus is entirely private */ + }; + virtual int init(); /** @@ -117,13 +126,14 @@ protected: void set_frequency(uint32_t frequency); /** - * Locking modes supported by the driver. + * Set the SPI bus locking mode + * + * This set the SPI locking mode. For devices competing with NuttX SPI + * drivers on a bus the right lock mode is LOCK_THREADS. + * + * @param mode Locking mode */ - enum LockMode { - LOCK_PREEMPTION, /**< the default; lock against all forms of preemption. */ - LOCK_THREADS, /**< lock only against other threads, using SPI_LOCK */ - LOCK_NONE /**< perform no locking, only safe if the bus is entirely private */ - }; + void set_lockmode(enum LockMode mode) { locking_mode = mode; } LockMode locking_mode; /**< selected locking mode */ diff --git a/src/drivers/frsky_telemetry/CMakeLists.txt b/src/drivers/frsky_telemetry/CMakeLists.txt index 7d6c17f463..a35310aaf7 100644 --- a/src/drivers/frsky_telemetry/CMakeLists.txt +++ b/src/drivers/frsky_telemetry/CMakeLists.txt @@ -38,6 +38,7 @@ px4_add_module( -Os SRCS frsky_data.c + sPort_data.c frsky_telemetry.c DEPENDS platforms__common diff --git a/src/drivers/frsky_telemetry/frsky_telemetry.c b/src/drivers/frsky_telemetry/frsky_telemetry.c index 6100f5f1d0..d527696501 100644 --- a/src/drivers/frsky_telemetry/frsky_telemetry.c +++ b/src/drivers/frsky_telemetry/frsky_telemetry.c @@ -35,11 +35,13 @@ /** * @file frsky_telemetry.c * @author Stefan Rado + * @author Mark Whitehorn * - * FrSky telemetry implementation. + * FrSky D8 mode and SmartPort (D16 mode) telemetry implementation. * - * This daemon emulates an FrSky sensor hub by periodically sending data - * packets to an attached FrSky receiver. + * This daemon emulates the FrSky Sensor Hub for D8 mode. + * For X series receivers (D16 mode) it emulates SmartPort sensors by responding to polling + * packets received from an attached FrSky X series receiver. * */ @@ -48,12 +50,15 @@ #include #include #include +#include #include #include #include #include #include +#include +#include "sPort_data.h" #include "frsky_data.h" @@ -63,7 +68,8 @@ static volatile bool thread_running = false; static int frsky_task; /* functions */ -static int frsky_open_uart(const char *uart_name, struct termios *uart_config_original); +static int sPort_open_uart(const char *uart_name, struct termios *uart_config, struct termios *uart_config_original); +static int set_uart_speed(int uart, struct termios *uart_config, speed_t speed); static void usage(void); static int frsky_telemetry_thread_main(int argc, char *argv[]); __EXPORT int frsky_telemetry_main(int argc, char *argv[]); @@ -72,10 +78,10 @@ __EXPORT int frsky_telemetry_main(int argc, char *argv[]); /** * Opens the UART device and sets all required serial parameters. */ -static int frsky_open_uart(const char *uart_name, struct termios *uart_config_original) +static int sPort_open_uart(const char *uart_name, struct termios *uart_config, struct termios *uart_config_original) { /* Open UART */ - const int uart = open(uart_name, O_WRONLY | O_NOCTTY); + const int uart = open(uart_name, O_RDWR | O_NOCTTY | O_NONBLOCK); if (uart < 0) { err(1, "Error opening port: %s", uart_name); @@ -91,22 +97,21 @@ static int frsky_open_uart(const char *uart_name, struct termios *uart_config_or } /* Fill the struct for the new configuration */ - struct termios uart_config; - tcgetattr(uart, &uart_config); + tcgetattr(uart, uart_config); /* Disable output post-processing */ - uart_config.c_oflag &= ~OPOST; + uart_config->c_oflag &= ~OPOST; /* Set baud rate */ - static const speed_t speed = B9600; + static const speed_t speed = B57600; - if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { + if (cfsetispeed(uart_config, speed) < 0 || cfsetospeed(uart_config, speed) < 0) { warnx("ERR: %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state); close(uart); return -1; } - if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) { + if ((termios_state = tcsetattr(uart, TCSANOW, uart_config)) < 0) { warnx("ERR: %s (tcsetattr)\n", uart_name); close(uart); return -1; @@ -115,6 +120,20 @@ static int frsky_open_uart(const char *uart_name, struct termios *uart_config_or return uart; } +static int set_uart_speed(int uart, struct termios *uart_config, speed_t speed) +{ + + if (cfsetispeed(uart_config, speed) < 0) { + return -1; + } + + if (tcsetattr(uart, TCSANOW, uart_config) < 0) { + return -1; + } + + return uart; +} + /** * Print command usage information */ @@ -133,7 +152,7 @@ static void usage() static int frsky_telemetry_thread_main(int argc, char *argv[]) { /* Default values for arguments */ - char *device_name = "/dev/ttyS1"; /* USART2 */ + char *device_name = "/dev/ttyS6"; /* USART8 */ /* Work around some stupidity in task_create's argv handling */ argc -= 2; @@ -154,42 +173,181 @@ static int frsky_telemetry_thread_main(int argc, char *argv[]) } /* Open UART */ + warnx("opening uart"); struct termios uart_config_original; - const int uart = frsky_open_uart(device_name, &uart_config_original); + struct termios uart_config; + const int uart = sPort_open_uart(device_name, &uart_config, &uart_config_original); if (uart < 0) { + warnx("could not open %s", device_name); err(1, "could not open %s", device_name); } - /* Subscribe to topics */ - frsky_init(); + /* poll descriptor */ + struct pollfd fds[1]; + fds[0].fd = uart; + fds[0].events = POLLIN; thread_running = true; /* Main thread loop */ - unsigned int iteration = 0; + char sbuf[20]; - while (!thread_should_exit) { + /* look for polling bytes indicating SmartPort telemetry + * if not found, send D type telemetry frames instead + */ + int status = poll(fds, sizeof(fds) / sizeof(fds[0]), 3000); - /* Sleep 200 ms */ - usleep(200000); + if (status > 0) { + /* traffic on the port, assume it's a SmartPort master */ + /* Subscribe to topics */ + sPort_init(); - /* Send frame 1 (every 200ms): acceleration values, altitude (vario), temperatures, current & voltages, RPM */ - frsky_send_frame1(uart); + /* send S.port telemetry */ + while (!thread_should_exit) { - /* Send frame 2 (every 1000ms): course, latitude, longitude, speed, altitude (GPS), fuel level */ - if (iteration % 5 == 0) { - frsky_send_frame2(uart); + /* wait for poll frame starting with value 0x7E + * note that only the bus master is supposed to put a 0x7E on the bus. + * slaves use byte stuffing to send 0x7E and 0x7D. + * we expect a poll frame every 12msec + */ + int status = poll(fds, sizeof(fds) / sizeof(fds[0]), 50); + + if (status < 1) { continue; } + + // read 1 byte + int newBytes = read(uart, &sbuf[0], 1); + + if (newBytes < 1 || sbuf[0] != 0x7E) { continue; } + + /* wait for ID byte */ + status = poll(fds, sizeof(fds) / sizeof(fds[0]), 50); + + if (status < 1) { continue; } + + hrt_abstime now = hrt_absolute_time(); + + newBytes = read(uart, &sbuf[1], 1); + + // allow a minimum of 500usec before reply + usleep(500); + + static hrt_abstime lastBATV = 0; + static hrt_abstime lastCUR = 0; + static hrt_abstime lastALT = 0; + static hrt_abstime lastSPD = 0; + static hrt_abstime lastFUEL = 0; + + switch (sbuf[1]) { + + case SMARTPORT_POLL_1: + + /* report BATV at 1Hz */ + if (now - lastBATV > 1000 * 1000) { + lastBATV = now; + /* send battery voltage */ + sPort_send_BATV(uart); + } + + break; + + + case SMARTPORT_POLL_2: + + /* report battery current at 5Hz */ + if (now - lastCUR > 200 * 1000) { + lastCUR = now; + /* send battery current */ + sPort_send_CUR(uart); + } + + break; + + + case SMARTPORT_POLL_3: + + /* report altitude at 5Hz */ + if (now - lastALT > 200 * 1000) { + lastALT = now; + /* send altitude */ + sPort_send_ALT(uart); + } + + break; + + + case SMARTPORT_POLL_4: + + /* report speed at 5Hz */ + if (now - lastSPD > 200 * 1000) { + lastSPD = now; + /* send speed */ + sPort_send_SPD(uart); + } + + break; + + case SMARTPORT_POLL_5: + + /* report fuel at 1Hz */ + if (now - lastFUEL > 1000 * 1000) { + lastFUEL = now; + /* send fuel */ + sPort_send_FUEL(uart); + } + + break; + } } - /* Send frame 3 (every 5000ms): date, time */ - if (iteration % 25 == 0) { - frsky_send_frame3(uart); + } else { - iteration = 0; + /* timed out: reconfigure UART and send D type telemetry */ + warnx("SmartPort receiver not detected, sending FrSky D type telemetry"); + + status = set_uart_speed(uart, &uart_config, B9600); + + if (status < 0) { + warnx("error setting speed for %s, quitting", device_name); + /* Reset the UART flags to original state */ + tcsetattr(uart, TCSANOW, &uart_config_original); + close(uart); + + thread_running = false; + return 0; } - iteration++; + int iteration = 0; + + /* Subscribe to topics */ + frsky_init(); + + /* send D8 mode telemetry */ + while (!thread_should_exit) { + + /* Sleep 200 ms */ + usleep(200000); + + /* Send frame 1 (every 200ms): acceleration values, altitude (vario), temperatures, current & voltages, RPM */ + frsky_send_frame1(uart); + + /* Send frame 2 (every 1000ms): course, latitude, longitude, speed, altitude (GPS), fuel level */ + if (iteration % 5 == 0) { + frsky_send_frame2(uart); + } + + /* Send frame 3 (every 5000ms): date, time */ + if (iteration % 25 == 0) { + frsky_send_frame3(uart); + + iteration = 0; + } + + iteration++; + } + + /* TODO: flush the input buffer if in full duplex mode */ + read(uart, &sbuf[0], sizeof(sbuf)); } /* Reset the UART flags to original state */ @@ -221,7 +379,7 @@ int frsky_telemetry_main(int argc, char *argv[]) thread_should_exit = false; frsky_task = px4_task_spawn_cmd("frsky_telemetry", SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, + 200, 2000, frsky_telemetry_thread_main, (char *const *)argv); diff --git a/src/drivers/frsky_telemetry/sPort_data.c b/src/drivers/frsky_telemetry/sPort_data.c new file mode 100644 index 0000000000..6af5ecc09e --- /dev/null +++ b/src/drivers/frsky_telemetry/sPort_data.c @@ -0,0 +1,217 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * Author: Stefan Rado + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file sPort_data.c + * @author Stefan Rado + * @author Mark Whitehorn + * + * FrSky SmartPort telemetry implementation. + * + */ + +#include "sPort_data.h" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#define frac(f) (f - (int)f) + +static int battery_sub = -1; +static int sensor_sub = -1; +static int global_position_sub = -1; +static int vehicle_status_sub = -1; + +/** + * Initializes the uORB subscriptions. + */ +void sPort_init() +{ + battery_sub = orb_subscribe(ORB_ID(battery_status)); + global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); +} + +/** + * Sends a 0x10 start byte. + */ +//static void sPort_send_start(int uart) +//{ +// static const uint8_t c = 0x10; +// write(uart, &c, 1); +//} + +static void update_crc(uint16_t *crc, unsigned char b) +{ + *crc += b; + *crc += *crc >> 8; + *crc &= 0xFF; +} + +/** + * Sends one byte, performing byte-stuffing if necessary. + */ +static void sPort_send_byte(int uart, uint8_t value) +{ + const uint8_t x7E[] = { 0x7D, 0x5E }; + const uint8_t x7D[] = { 0x7D, 0x5D }; + + switch (value) { + case 0x7E: + write(uart, x7E, sizeof(x7E)); + break; + + case 0x7D: + write(uart, x7D, sizeof(x7D)); + break; + + default: + write(uart, &value, sizeof(value)); + break; + } +} + +/** + * Sends one data id/value pair. + */ +void sPort_send_data(int uart, uint16_t id, uint32_t data) +{ + union { + uint32_t word; + uint8_t byte[4]; + } buf; + + /* send start byte */ + static const uint8_t c = 0x10; + write(uart, &c, 1); + + /* init crc */ + uint16_t crc = c; + + buf.word = id; + + for (int i = 0; i < 2; i++) { + update_crc(&crc, buf.byte[i]); + sPort_send_byte(uart, buf.byte[i]); /* LSB first */ + } + + buf.word = data; + + for (int i = 0; i < 4; i++) { + update_crc(&crc, buf.byte[i]); + sPort_send_byte(uart, buf.byte[i]); /* LSB first */ + } + + sPort_send_byte(uart, 0xFF - crc); +} + + +// scaling correct with OpenTX 2.1.7 +void sPort_send_BATV(int uart) +{ + /* get a local copy of the vehicle status data */ + struct vehicle_status_s vehicle_status; + memset(&vehicle_status, 0, sizeof(vehicle_status)); + orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status); + + /* send battery voltage as VFAS */ + uint32_t voltage = (int)(100 * vehicle_status.battery_voltage); + sPort_send_data(uart, SMARTPORT_ID_VFAS, voltage); +} + +// verified scaling +void sPort_send_CUR(int uart) +{ + /* get a local copy of the vehicle status data */ + struct vehicle_status_s vehicle_status; + memset(&vehicle_status, 0, sizeof(vehicle_status)); + orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status); + + /* send data */ + uint32_t current = (int)(10 * vehicle_status.battery_current); + sPort_send_data(uart, SMARTPORT_ID_CURR, current); +} + +// verified scaling for "custom" altitude option +// OpenTX uses the initial reading as field elevation and displays +// the difference (altitude - field) +void sPort_send_ALT(int uart) +{ + /* get a local copy of the current sensor values */ + struct sensor_combined_s raw; + memset(&raw, 0, sizeof(raw)); + orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); + + /* send data */ + uint32_t alt = (int)(100 * raw.baro_alt_meter[0]); + sPort_send_data(uart, SMARTPORT_ID_ALT, alt); +} + +// verified scaling for "calculated" option +void sPort_send_SPD(int uart) +{ + /* get a local copy of the global position data */ + struct vehicle_global_position_s global_pos; + memset(&global_pos, 0, sizeof(global_pos)); + orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos); + + /* send data for A2 */ + float speed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e); + uint32_t ispeed = (int)(10 * speed); + sPort_send_data(uart, SMARTPORT_ID_GPS_SPD, ispeed); +} + +// verified scaling +void sPort_send_FUEL(int uart) +{ + /* get a local copy of the vehicle status data */ + struct vehicle_status_s vehicle_status; + memset(&vehicle_status, 0, sizeof(vehicle_status)); + orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status); + + /* send data */ + uint32_t fuel = (int)(100 * vehicle_status.battery_remaining); + sPort_send_data(uart, SMARTPORT_ID_FUEL, fuel); +} diff --git a/src/drivers/frsky_telemetry/sPort_data.h b/src/drivers/frsky_telemetry/sPort_data.h new file mode 100644 index 0000000000..8173780b88 --- /dev/null +++ b/src/drivers/frsky_telemetry/sPort_data.h @@ -0,0 +1,87 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * Author: Stefan Rado + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file sPort_data.h + * @author Mark Whitehorn + * + * FrSky SmartPort telemetry implementation. + * + */ +#ifndef _SPORT_DATA_H +#define _SPORT_DATA_H + +#include + +/* FrSky SmartPort polling IDs captured from X4R */ +#define SMARTPORT_POLL_1 0x1B +#define SMARTPORT_POLL_2 0x34 +#define SMARTPORT_POLL_3 0x95 +#define SMARTPORT_POLL_4 0x16 +#define SMARTPORT_POLL_5 0xB7 + +/* FrSky SmartPort sensor IDs */ +#define SMARTPORT_ID_RSSI 0xf101 +#define SMARTPORT_ID_RXA1 0xf102 // supplied by RX +#define SMARTPORT_ID_RXA2 0xf103 // supplied by RX +#define SMARTPORT_ID_BATV 0xf104 +#define SMARTPORT_ID_SWR 0xf105 +#define SMARTPORT_ID_T1 0x0400 +#define SMARTPORT_ID_T2 0x0410 +#define SMARTPORT_ID_RPM 0x0500 +#define SMARTPORT_ID_FUEL 0x0600 +#define SMARTPORT_ID_ALT 0x0100 +#define SMARTPORT_ID_VARIO 0x0110 +#define SMARTPORT_ID_ACCX 0x0700 +#define SMARTPORT_ID_ACCY 0x0710 +#define SMARTPORT_ID_ACCZ 0x0720 +#define SMARTPORT_ID_CURR 0x0200 +#define SMARTPORT_ID_VFAS 0x0210 +#define SMARTPORT_ID_CELLS 0x0300 +#define SMARTPORT_ID_GPS_LON_LAT 0x0800 +#define SMARTPORT_ID_GPS_ALT 0x0820 +#define SMARTPORT_ID_GPS_SPD 0x0830 +#define SMARTPORT_ID_GPS_CRS 0x0840 +#define SMARTPORT_ID_GPS_TIME 0x0850 + +// Public functions +void sPort_init(void); +void sPort_send_data(int uart, uint16_t id, uint32_t data); +void sPort_send_BATV(int uart); +void sPort_send_CUR(int uart); +void sPort_send_ALT(int uart); +void sPort_send_SPD(int uart); +void sPort_send_FUEL(int uart); + +#endif /* _SPORT_TELEMETRY_H */ diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 05ff3e5b91..758cb3f320 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -402,7 +402,12 @@ GPS::task_main() if (!(_pub_blocked)) { if (helper_ret & 1) { - orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); + if (_report_gps_pos_pub != nullptr) { + orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); + + } else { + _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos); + } } if (_p_report_sat_info && (helper_ret & 2)) { diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index 78192c3a98..5fe0fc501d 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -131,7 +131,7 @@ MS5611_spi_interface(ms5611::prom_u &prom_buf, uint8_t busnum) } MS5611_SPI::MS5611_SPI(uint8_t bus, spi_dev_e device, ms5611::prom_u &prom_buf) : - SPI("MS5611_SPI", nullptr, bus, device, SPIDEV_MODE3, 11 * 1000 * 1000 /* will be rounded to 10.4 MHz */), + SPI("MS5611_SPI", nullptr, bus, device, SPIDEV_MODE3, 20 * 1000 * 1000 /* will be rounded to 10.4 MHz */), _prom(prom_buf) { } @@ -152,6 +152,11 @@ MS5611_SPI::init() goto out; } + /* sharing a bus with NuttX drivers */ +#if defined (CONFIG_ARCH_BOARD_PX4FMU_V4) + //set_lockmode(SPI::LOCK_THREADS); +#endif + /* send reset command */ ret = _reset(); diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index e017a2388d..67f9bec9ad 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -161,6 +161,8 @@ private: volatile bool _initialized; bool _throttle_armed; bool _pwm_on; + bool _pwm_init; + uint8_t _pwm_channel_mask; MixerGroup *_mixers; @@ -313,6 +315,8 @@ PX4FMU::PX4FMU() : _initialized(false), _throttle_armed(false), _pwm_on(false), + _pwm_init(false), + _pwm_channel_mask(0), _mixers(nullptr), _groups_required(0), _groups_subscribed(0), @@ -418,10 +422,7 @@ PX4FMU::set_mode(Mode mode) _pwm_default_rate = 50; _pwm_alt_rate = 50; _pwm_alt_rate_channels = 0; - - /* XXX magic numbers */ - up_pwm_servo_init(0x3); - set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate); + _pwm_channel_mask = 0x3; break; @@ -432,10 +433,7 @@ PX4FMU::set_mode(Mode mode) _pwm_default_rate = 50; _pwm_alt_rate = 50; _pwm_alt_rate_channels = 0; - - /* XXX magic numbers */ - up_pwm_servo_init(0xf); - set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate); + _pwm_channel_mask = 0xf; break; @@ -446,10 +444,7 @@ PX4FMU::set_mode(Mode mode) _pwm_default_rate = 50; _pwm_alt_rate = 50; _pwm_alt_rate_channels = 0; - - /* XXX magic numbers */ - up_pwm_servo_init(0x3f); - set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate); + _pwm_channel_mask = 0x3f; break; @@ -461,10 +456,7 @@ PX4FMU::set_mode(Mode mode) _pwm_default_rate = 50; _pwm_alt_rate = 50; _pwm_alt_rate_channels = 0; - - /* XXX magic numbers */ - up_pwm_servo_init(0xff); - set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate); + _pwm_channel_mask = 0xff; break; #endif @@ -477,6 +469,7 @@ PX4FMU::set_mode(Mode mode) /* disable servo outputs - no need to set rates */ up_pwm_servo_deinit(); + _pwm_init = false; break; @@ -873,6 +866,13 @@ PX4FMU::cycle() if (_pwm_on != pwm_on) { _pwm_on = pwm_on; + + if (!_pwm_init && _pwm_on) { + up_pwm_servo_init(_pwm_channel_mask); + set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate); + _pwm_init = true; + } + up_pwm_servo_arm(pwm_on); } } @@ -1717,7 +1717,7 @@ ssize_t PX4FMU::write(file *filp, const char *buffer, size_t len) { unsigned count = len / 2; - uint16_t values[6]; + uint16_t values[8]; #ifdef CONFIG_ARCH_BOARD_AEROCORE diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 9db91d28b8..1ff147d190 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -273,6 +273,7 @@ private: /* cached IO state */ uint16_t _status; ///< Various IO status flags uint16_t _alarms; ///< Various IO alarms + uint16_t _last_written_arming; ///< the last written arming state reg /* subscribed topics */ int _t_actuator_controls_0; ///< actuator controls group 0 topic @@ -509,6 +510,7 @@ PX4IO::PX4IO(device::Device *interface) : _perf_sample_latency(perf_alloc(PC_ELAPSED, "io latency")), _status(0), _alarms(0), + _last_written_arming(0), _t_actuator_controls_0(-1), _t_actuator_controls_1(-1), _t_actuator_controls_2(-1), @@ -977,7 +979,7 @@ PX4IO::task_main() } if (now >= poll_last + IO_POLL_INTERVAL) { - /* run at 50Hz */ + /* run at 50-250Hz */ poll_last = now; /* pull status and alarms from IO */ @@ -988,16 +990,6 @@ PX4IO::task_main() /* fetch PWM outputs from IO */ io_publish_pwm_outputs(); - } - - if (now >= orb_check_last + ORB_CHECK_INTERVAL) { - /* run at 5Hz */ - orb_check_last = now; - - /* try to claim the MAVLink log FD */ - if (_mavlink_fd < 0) { - _mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0); - } /* check updates on uORB topics and handle it */ bool updated = false; @@ -1012,6 +1004,19 @@ PX4IO::task_main() if (updated) { io_set_arming_state(); } + } + + if (now >= orb_check_last + ORB_CHECK_INTERVAL) { + /* run at 5Hz */ + orb_check_last = now; + + /* try to claim the MAVLink log FD */ + if (_mavlink_fd < 0) { + _mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0); + } + + /* check updates on uORB topics and handle it */ + bool updated = false; /* vehicle command */ orb_check(_t_vehicle_command, &updated); @@ -1350,7 +1355,15 @@ PX4IO::io_set_arming_state() } } - return io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, clear, set); + uint16_t new_arming_state = set; + new_arming_state &= ~(clear); + + if (_last_written_arming != new_arming_state) { + _last_written_arming = new_arming_state; + return io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, clear, set); + } + + return 0; } int diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp index 9c6e60947a..2b34f360d4 100644 --- a/src/drivers/stm32/adc/adc.cpp +++ b/src/drivers/stm32/adc/adc.cpp @@ -315,13 +315,13 @@ void ADC::update_system_power(void) { #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 - system_power_s system_power; + system_power_s system_power = {}; system_power.timestamp = hrt_absolute_time(); system_power.voltage5V_v = 0; for (unsigned i = 0; i < _channel_count; i++) { - if (_samples[i].am_channel == 4) { + if (_samples[i].am_channel == ADC_5V_RAIL_SENSE) { // it is 2:1 scaled system_power.voltage5V_v = _samples[i].am_data * (6.6f / 4096); } @@ -349,13 +349,13 @@ ADC::update_system_power(void) #endif // CONFIG_ARCH_BOARD_PX4FMU_V2 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V4 - system_power_s system_power; + system_power_s system_power = {}; system_power.timestamp = hrt_absolute_time(); system_power.voltage5V_v = 0; for (unsigned i = 0; i < _channel_count; i++) { - if (_samples[i].am_channel == 4) { + if (_samples[i].am_channel == ADC_5V_RAIL_SENSE) { // it is 2:1 scaled system_power.voltage5V_v = _samples[i].am_data * (6.6f / 4096); } diff --git a/src/lib/ecl b/src/lib/ecl index a11709956f..8d0022ab1e 160000 --- a/src/lib/ecl +++ b/src/lib/ecl @@ -1 +1 @@ -Subproject commit a11709956fd0d082d4fde4da0ad8c79117a38990 +Subproject commit 8d0022ab1e65543124efd2f55b4e72b9e672cd61 diff --git a/src/lib/matrix b/src/lib/matrix index 1d9d8e6f9b..ce30542dba 160000 --- a/src/lib/matrix +++ b/src/lib/matrix @@ -1 +1 @@ -Subproject commit 1d9d8e6f9b0c2ed930ea1c8996f5fb398b3cf322 +Subproject commit ce30542dbaa3e6f5e68b742a34cda993a351e15d diff --git a/src/lib/version/version.h b/src/lib/version/version.h index 9492cec90f..9c1caf8e83 100644 --- a/src/lib/version/version.h +++ b/src/lib/version/version.h @@ -66,7 +66,12 @@ #ifdef CONFIG_ARCH_BOARD_SITL #define HW_ARCH "LINUXTEST" #endif + #ifdef CONFIG_ARCH_BOARD_EAGLE #define HW_ARCH "LINUXTEST" #endif + +#ifdef CONFIG_ARCH_BOARD_RPI2 +#define HW_ARCH "LINUXTEST" +#endif #endif /* VERSION_H_ */ diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index dd6d5e3a9d..488f662e11 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -595,7 +595,8 @@ int do_level_calibration(int mavlink_fd) { param_get(pitch_offset_handle, &pitch_offset_current); param_get(board_rot_handle, &board_rot_current); - if (board_rot_current == 0) { + // give attitude some time to settle if there have been changes to the board rotation parameters + if (board_rot_current == 0 && fabsf(roll_offset_current) < FLT_EPSILON && fabsf(pitch_offset_current) < FLT_EPSILON ) { settle_time = 0; } diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 6b5c29ab93..767b13367e 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1509,12 +1509,12 @@ int commander_thread_main(int argc, char *argv[]) /* EPH / EPV */ param_get(_param_eph, &eph_threshold); param_get(_param_epv, &epv_threshold); - } - /* Set flag to autosave parameters if necessary */ - if (updated && autosave_params != 0) { - /* trigger an autosave */ - need_param_autosave = true; + /* Set flag to autosave parameters if necessary */ + if (updated && autosave_params != 0 && param_changed.saved == false) { + /* trigger an autosave */ + need_param_autosave = true; + } } orb_check(sp_man_sub, &updated); @@ -1570,11 +1570,12 @@ int commander_thread_main(int argc, char *argv[]) /* and the system is not already armed (and potentially flying) */ !armed.armed) { - bool chAirspeed = false; - bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT; - + bool chAirspeed = false; + bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT; + /* Perform airspeed check only if circuit breaker is not - * engaged and it's not a rotary wing */ + * engaged and it's not a rotary wing + */ if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) { chAirspeed = true; } @@ -2284,6 +2285,15 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "main state transition denied"); } + /* check throttle kill switch */ + if (sp_man.kill_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + /* set lockdown flag */ + armed.lockdown = true; + } else if (sp_man.kill_switch == manual_control_setpoint_s::SWITCH_POS_OFF) { + armed.lockdown = false; + } + /* no else case: do not change lockdown flag in unconfigured case */ + } else { if (!status.rc_input_blocked && !status.rc_signal_lost) { mavlink_log_critical(mavlink_fd, "MANUAL CONTROL LOST (at t=%llums)", hrt_absolute_time() / 1000); @@ -2714,7 +2724,7 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu } } -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 +#if defined (CONFIG_ARCH_BOARD_PX4FMU_V1) || defined (CONFIG_ARCH_BOARD_PX4FMU_V4) /* this runs at around 20Hz, full cycle is 16 ticks = 10/16Hz */ if (actuator_armed->armed) { diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 5163087830..db151fbe5c 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -97,6 +97,9 @@ static const char * const state_names[vehicle_status_s::ARMING_STATE_MAX] = { "ARMING_STATE_IN_AIR_RESTORE", }; +static hrt_abstime last_preflight_check = 0; ///< initialize so it gets checked immediately +static int last_prearm_ret = 1; ///< initialize to fail + transition_result_t arming_state_transition(struct vehicle_status_s *status, ///< current vehicle status const struct safety_s *safety, ///< current safety settings @@ -132,12 +135,18 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s } /* re-run the pre-flight check as long as sensors are failing */ if (!status->condition_system_sensors_initialized - && (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || - new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) - && status->hil_state == vehicle_status_s::HIL_STATE_OFF) { + && (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED + || new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) + && status->hil_state == vehicle_status_s::HIL_STATE_OFF) { - prearm_ret = preflight_check(status, mavlink_fd, false /* pre-flight */); - status->condition_system_sensors_initialized = !prearm_ret; + if (last_preflight_check == 0 || hrt_absolute_time() - last_preflight_check > 1000 * 1000) { + prearm_ret = preflight_check(status, mavlink_fd, false /* pre-flight */); + status->condition_system_sensors_initialized = !prearm_ret; + last_preflight_check = hrt_absolute_time(); + last_prearm_ret = prearm_ret; + } else { + prearm_ret = last_prearm_ret; + } } /* diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 22fce7ec6f..38dbaf3327 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -72,7 +72,8 @@ #include #include #include -#include +#include +#include #include @@ -84,7 +85,7 @@ class Ekf2; namespace ekf2 { -Ekf2 *instance; +Ekf2 *instance = nullptr; } @@ -116,6 +117,8 @@ public: void print_status(); + void exit() { _task_should_exit = true; } + private: static constexpr float _dt_max = 0.02; bool _task_should_exit = false; /**< if true, task should exit */ @@ -124,11 +127,15 @@ private: int _sensors_sub = -1; int _gps_sub = -1; int _airspeed_sub = -1; + int _params_sub = -1; orb_advert_t _att_pub; orb_advert_t _lpos_pub; orb_advert_t _control_state_pub; orb_advert_t _vehicle_global_position_pub; + orb_advert_t _estimator_status_pub; + orb_advert_t _estimator_innovations_pub; + /* Low pass filter for attitude rates */ math::LowPassFilter2p _lp_roll_rate; @@ -162,24 +169,23 @@ private: EstimatorBase *_ekf; - - void update_parameters(bool force); - int update_subscriptions(); }; Ekf2::Ekf2(): SuperBlock(NULL, "EKF"), + _att_pub(nullptr), + _lpos_pub(nullptr), + _control_state_pub(nullptr), + _vehicle_global_position_pub(nullptr), + _estimator_status_pub(nullptr), + _estimator_innovations_pub(nullptr), _lp_roll_rate(250.0f, 30.0f), _lp_pitch_rate(250.0f, 30.0f), - _lp_yaw_rate(250.0f, 20.0f) + _lp_yaw_rate(250.0f, 20.0f), + _ekf(new Ekf()) { - _ekf = new Ekf(); - _att_pub = nullptr; - _lpos_pub = nullptr; - _control_state_pub = nullptr; - parameters *params = _ekf->getParamHandle(); _mag_delay_ms = new control::BlockParamFloat(this, "EKF2_MAG_DELAY", false, ¶ms->mag_delay_ms); _baro_delay_ms = new control::BlockParamFloat(this, "EKF2_BARO_DELAY", false, ¶ms->baro_delay_ms); @@ -230,34 +236,49 @@ void Ekf2::task_main() _sensors_sub = orb_subscribe(ORB_ID(sensor_combined)); _gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); + _params_sub = orb_subscribe(ORB_ID(parameter_update)); - px4_pollfd_struct_t fds[1]; + px4_pollfd_struct_t fds[2] = {}; fds[0].fd = _sensors_sub; fds[0].events = POLLIN; + fds[1].fd = _params_sub; + fds[1].events = POLLIN; // initialise parameter cache updateParams(); + vehicle_gps_position_s gps = {}; + while (!_task_should_exit) { - int ret = px4_poll(fds, 1, 1000); + int ret = px4_poll(fds, sizeof(fds) / sizeof(fds[0]), 1000); if (ret < 0) { // Poll error, sleep and try again usleep(10000); continue; - } else if (ret == 0 || fds[0].revents != POLLIN) { + } else if (ret == 0) { // Poll timeout or no new data, do nothing continue; } - updateParams(); + if (fds[1].revents & POLLIN) { + // read from param to clear updated flag + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), _params_sub, &update); + updateParams(); + + // fetch sensor data in next loop + continue; + } else if (!(fds[0].revents & POLLIN)) { + // no new data + continue; + } bool gps_updated = false; bool airspeed_updated = false; sensor_combined_s sensors = {}; - vehicle_gps_position_s gps = {}; airspeed_s airspeed = {}; orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors); @@ -315,7 +336,7 @@ void Ekf2::task_main() _ekf->update(); // generate vehicle attitude data - struct vehicle_attitude_s att; + struct vehicle_attitude_s att = {}; att.timestamp = hrt_absolute_time(); _ekf->copy_quaternion(att.q); @@ -326,7 +347,7 @@ void Ekf2::task_main() att.yaw = euler(2); // generate vehicle local position data - struct vehicle_local_position_s lpos; + struct vehicle_local_position_s lpos = {}; float pos[3] = {}; float vel[3] = {}; @@ -355,8 +376,8 @@ void Ekf2::task_main() lpos.xy_global = _ekf->position_is_valid();// true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) lpos.z_global = true;// true if z is valid and has valid global reference (ref_alt) - lpos.ref_lat = _ekf->_posRef.lat_rad * (double)180.0 * M_PI; // Reference point latitude in degrees - lpos.ref_lon = _ekf->_posRef.lon_rad * (double)180.0 * M_PI; // Reference point longitude in degrees + lpos.ref_lat = _ekf->_posRef.lat_rad * 180.0 / M_PI; // Reference point latitude in degrees + lpos.ref_lon = _ekf->_posRef.lon_rad * 180.0 / M_PI; // Reference point longitude in degrees lpos.ref_alt = _ekf->_gps_alt_ref; // Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level @@ -421,7 +442,7 @@ void Ekf2::task_main() } // generate and publish global position data - struct vehicle_global_position_s global_pos; + struct vehicle_global_position_s global_pos = {}; if (_ekf->position_is_valid()) { // TODO: local origin is currenlty at GPS height origin - this is different to ekf_att_pos_estimator @@ -460,7 +481,42 @@ void Ekf2::task_main() orb_publish(ORB_ID(vehicle_global_position), _vehicle_global_position_pub, &global_pos); } } + + // publish estimator status + struct estimator_status_s status = {}; + status.timestamp = hrt_absolute_time(); + _ekf->get_state_delayed(status.states); + _ekf->get_covariances(status.covariances); + + if (_estimator_status_pub == nullptr) { + _estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &status); + + } else { + orb_publish(ORB_ID(estimator_status), _estimator_status_pub, &status); + } + + // publish estimator innovation data + struct ekf2_innovations_s innovations = {}; + innovations.timestamp = hrt_absolute_time(); + _ekf->get_vel_pos_innov(&innovations.vel_pos_innov[0]); + _ekf->get_mag_innov(&innovations.mag_innov[0]); + _ekf->get_heading_innov(&innovations.heading_innov); + + _ekf->get_vel_pos_innov_var(&innovations.vel_pos_innov_var[0]); + _ekf->get_mag_innov_var(&innovations.mag_innov_var[0]); + _ekf->get_heading_innov_var(&innovations.heading_innov_var); + + if (_estimator_innovations_pub == nullptr) { + _estimator_innovations_pub = orb_advertise(ORB_ID(ekf2_innovations), &innovations); + + } else { + orb_publish(ORB_ID(ekf2_innovations), _estimator_innovations_pub, &innovations); + } + } + + delete ekf2::instance; + ekf2::instance = nullptr; } void Ekf2::task_main_trampoline(int argc, char *argv[]) @@ -525,8 +581,13 @@ int ekf2_main(int argc, char *argv[]) return 1; } - delete ekf2::instance; - ekf2::instance = nullptr; + ekf2::instance->exit(); + + // wait for the destruction of the instance + while (ekf2::instance != nullptr) { + usleep(50000); + } + return 0; } diff --git a/src/modules/mavlink/CMakeLists.txt b/src/modules/mavlink/CMakeLists.txt index 3ae2e570ec..625c88f9aa 100644 --- a/src/modules/mavlink/CMakeLists.txt +++ b/src/modules/mavlink/CMakeLists.txt @@ -41,7 +41,7 @@ px4_add_module( ${MODULE_CFLAGS} -Wno-attributes -Wno-packed - -DMAVLINK_COMM_NUM_BUFFERS=3 + -DMAVLINK_COMM_NUM_BUFFERS=4 -Wno-packed -Wno-tautological-constant-out-of-range-compare -Os diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 4eda4edb6c..e31cf29411 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -169,6 +169,7 @@ Mavlink::Mavlink() : _flow_control_enabled(true), _last_write_success_time(0), _last_write_try_time(0), + _mavlink_start_time(0), _bytes_tx(0), _bytes_txerr(0), _bytes_rx(0), @@ -886,6 +887,10 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID _last_write_try_time = hrt_absolute_time(); + if (_mavlink_start_time == 0) { + _mavlink_start_time = _last_write_try_time; + } + if (get_protocol() == SERIAL) { /* check if there is space in the buffer, let it overflow else */ unsigned buf_free = get_free_tx_buf(); @@ -938,6 +943,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID /* resend heartbeat via broadcast */ if ((_mode != MAVLINK_MODE_ONBOARD) + && (_mavlink_start_time > 0 && (hrt_elapsed_time(&_mavlink_start_time) > 4 * 1000 * 1000)) && (((hrt_elapsed_time(&tstatus.heartbeat_time) > 3 * 1000 * 1000) || (tstatus.heartbeat_time == 0)) && msgid == MAVLINK_MSG_ID_HEARTBEAT)) { @@ -1049,10 +1055,10 @@ Mavlink::init_udp() /* set default target address, but not for onboard mode (will be set on first received packet) */ memset((char *)&_src_addr, 0, sizeof(_src_addr)); if (_mode != MAVLINK_MODE_ONBOARD) { - set_client_source_initialized(); _src_addr.sin_family = AF_INET; inet_aton("127.0.0.1", &_src_addr.sin_addr); _src_addr.sin_port = htons(DEFAULT_REMOTE_PORT_UDP); + set_client_source_initialized(); } /* default broadcast address */ @@ -1796,7 +1802,6 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("GLOBAL_POSITION_INT", 10.0f); configure_stream("HOME_POSITION", 0.5f); configure_stream("ATTITUDE_TARGET", 10.0f); - configure_stream("BATTERY_STATUS", 1.0f); configure_stream("SYSTEM_TIME", 1.0f); configure_stream("RC_CHANNELS", 5.0f); configure_stream("SERVO_OUTPUT_RAW_0", 1.0f); @@ -2141,7 +2146,7 @@ Mavlink::start(int argc, char *argv[]) px4_task_spawn_cmd(buf, SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 2400, + 2700, (px4_main_t)&Mavlink::start_helper, (char *const *)argv); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 530eb760b2..5e5cc34649 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -339,7 +339,7 @@ public: int get_socket_fd () { return _socket_fd; }; #ifdef __PX4_POSIX - struct sockaddr_in * get_client_source_address() {return &_src_addr;}; + struct sockaddr_in * get_client_source_address() {return &_src_addr;}; void set_client_source_initialized() { _src_addr_initialized = true; }; @@ -416,6 +416,7 @@ private: bool _flow_control_enabled; uint64_t _last_write_success_time; uint64_t _last_write_try_time; + uint64_t _mavlink_start_time; unsigned _bytes_tx; unsigned _bytes_txerr; diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index a196b6eb8e..34507c7c59 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -885,6 +885,9 @@ private: MavlinkOrbSubscription *_airspeed_sub; uint64_t _airspeed_time; + MavlinkOrbSubscription *_sensor_sub; + uint64_t _sensor_time; + /* do not allow top copying this class */ MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &); MavlinkStreamVFRHUD& operator = (const MavlinkStreamVFRHUD &); @@ -900,16 +903,18 @@ protected: _act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_0))), _act_time(0), _airspeed_sub(_mavlink->add_orb_subscription(ORB_ID(airspeed))), - _airspeed_time(0) + _airspeed_time(0), + _sensor_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined))), + _sensor_time(0) {} void send(const hrt_abstime t) { - struct vehicle_attitude_s att; - struct vehicle_global_position_s pos; - struct actuator_armed_s armed; - struct actuator_controls_s act; - struct airspeed_s airspeed; + struct vehicle_attitude_s att = {}; + struct vehicle_global_position_s pos = {}; + struct actuator_armed_s armed = {}; + struct actuator_controls_s act = {}; + struct airspeed_s airspeed = {}; bool updated = _att_sub->update(&_att_time, &att); updated |= _pos_sub->update(&_pos_time, &pos); @@ -924,7 +929,15 @@ protected: msg.groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e); msg.heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F; msg.throttle = armed.armed ? act.control[3] * 100.0f : 0.0f; - msg.alt = pos.alt; + if (_pos_time > 0) { + /* use global estimate */ + msg.alt = pos.alt; + } else { + /* fall back to baro altitude */ + sensor_combined_s sensor; + (void)_sensor_sub->update(&_sensor_time, &sensor); + msg.alt = sensor.baro_alt_meter[0]; + } msg.climb = -pos.vel_d; _mavlink->send_message(MAVLINK_MSG_ID_VFR_HUD, &msg); @@ -2668,6 +2681,9 @@ private: MavlinkOrbSubscription *_home_sub; uint64_t _home_time; + MavlinkOrbSubscription *_sensor_sub; + uint64_t _sensor_time; + /* do not allow top copying this class */ MavlinkStreamAltitude(MavlinkStreamAltitude &); MavlinkStreamAltitude& operator = (const MavlinkStreamAltitude &); @@ -2679,7 +2695,9 @@ protected: _local_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position))), _local_pos_time(0), _home_sub(_mavlink->add_orb_subscription(ORB_ID(home_position))), - _home_time(0) + _home_time(0), + _sensor_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined))), + _sensor_time(0) {} void send(const hrt_abstime t) @@ -2693,21 +2711,25 @@ protected: updated |= _home_sub->update(&_home_time, &home); if (updated) { + + struct sensor_combined_s sensor; + (void)_sensor_sub->update(&_sensor_time, &sensor); + mavlink_altitude_t msg; msg.time_usec = hrt_absolute_time(); - msg.altitude_monotonic = global_pos.pressure_alt; - msg.altitude_amsl = global_pos.alt; - msg.altitude_local = -local_pos.z; - msg.altitude_relative = home.alt; + msg.altitude_monotonic = (_sensor_time > 0) ? sensor.baro_alt_meter[0] : 0.0f / 0.0f; + msg.altitude_amsl = (_global_pos_time > 0) ? global_pos.alt : 0.0f / 0.0f; + msg.altitude_local = (_local_pos_time > 0) ? -local_pos.z : 0.0f / 0.0f; + msg.altitude_relative = (_home_time > 0) ? home.alt : 0.0f / 0.0f; if (global_pos.terrain_alt_valid) { msg.altitude_terrain = global_pos.terrain_alt; msg.bottom_clearance = global_pos.alt - global_pos.terrain_alt; } else { - msg.altitude_terrain = -1001; - msg.bottom_clearance = -1; + msg.altitude_terrain = 0.0f / 0.0f; + msg.bottom_clearance = 0.0f / 0.0f; } _mavlink->send_message(MAVLINK_MSG_ID_ALTITUDE, &msg); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 231ea21c1d..06f49cc3ec 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1771,7 +1771,7 @@ MavlinkReceiver::receive_thread(void *arg) fds[0].events = POLLIN; } #ifdef __PX4_POSIX - struct sockaddr_in srcaddr; + struct sockaddr_in srcaddr = {}; socklen_t addrlen = sizeof(srcaddr); if (_mavlink->get_protocol() == UDP || _mavlink->get_protocol() == TCP) { @@ -1817,8 +1817,12 @@ MavlinkReceiver::receive_thread(void *arg) if ((srcaddr_last->sin_addr.s_addr == htonl(localhost) && srcaddr.sin_addr.s_addr != htonl(localhost)) || (_mavlink->get_mode() == Mavlink::MAVLINK_MODE_ONBOARD && !_mavlink->get_client_source_initialized())) { // if we were sending to localhost before but have a new host then accept him - memcpy(srcaddr_last, &srcaddr, sizeof(srcaddr)); - PX4_WARN("UDP source addr changed: %s", inet_ntoa(srcaddr.sin_addr)); +// This is causing issues on Linux, so use default port for now +// this will kill tablet testing on Linux and VMs +#ifndef __PX4_LINUX + srcaddr_last->sin_addr.s_addr = srcaddr.sin_addr.s_addr; + srcaddr_last->sin_port = srcaddr.sin_port; +#endif _mavlink->set_client_source_initialized(); } #endif diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 5461f0ec97..27901dcdff 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -199,6 +199,7 @@ private: param_t vtol_type; param_t roll_tc; param_t pitch_tc; + param_t vtol_opt_recovery_enabled; } _params_handles; /**< handles for interesting parameters */ @@ -218,6 +219,7 @@ private: math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */ float rattitude_thres; int vtol_type; /**< 0 = Tailsitter, 1 = Tiltrotor, 2 = Standard airframe */ + bool vtol_opt_recovery_enabled; } _params; TailsitterRecovery *_ts_opt_recovery; /**< Computes optimal rates for tailsitter recovery */ @@ -347,6 +349,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params.mc_rate_max.zero(); _params.acro_rate_max.zero(); _params.rattitude_thres = 1.0f; + _params.vtol_opt_recovery_enabled = false; _rates_prev.zero(); _rates_sp.zero(); @@ -383,11 +386,12 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params_handles.vtol_type = param_find("VT_TYPE"); _params_handles.roll_tc = param_find("MC_ROLL_TC"); _params_handles.pitch_tc = param_find("MC_PITCH_TC"); + _params_handles.vtol_opt_recovery_enabled = param_find("VT_OPT_RECOV_EN"); /* fetch initial parameter values */ parameters_update(); - if (_params.vtol_type == 0) { + if (_params.vtol_type == 0 && _params.vtol_opt_recovery_enabled) { // the vehicle is a tailsitter, use optimal recovery control strategy _ts_opt_recovery = new TailsitterRecovery(); } @@ -415,8 +419,10 @@ MulticopterAttitudeControl::~MulticopterAttitudeControl() } } while (_control_task != -1); } + if (_ts_opt_recovery != nullptr) { + delete _ts_opt_recovery; + } - delete _ts_opt_recovery; mc_att_control::g_control = nullptr; } @@ -489,6 +495,10 @@ MulticopterAttitudeControl::parameters_update() param_get(_params_handles.vtol_type, &_params.vtol_type); + int tmp; + param_get(_params_handles.vtol_opt_recovery_enabled, &tmp); + _params.vtol_opt_recovery_enabled = (bool)tmp; + _actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY); return OK; diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index 011a6755ad..a6e64d49a2 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -254,13 +254,14 @@ PARAM_DEFINE_FLOAT(MC_PITCHRATE_MAX, 220.0f); * Limit for yaw rate, has effect for large rotations in autonomous mode, * to avoid large control output and mixer saturation. A value of significantly * over 60 degrees per second can already lead to mixer saturation. + * A value of 30 degrees / second is recommended to avoid very audible twitches. * * @unit deg/s * @min 0.0 * @max 360.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX, 60.0f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX, 45.0f); /** * Max acro roll rate diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 3bc8f9797e..1b8503a790 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -180,12 +180,12 @@ private: param_t xy_ff; param_t tilt_max_air; param_t land_speed; - param_t tko_jmpspd; param_t tko_speed; param_t tilt_max_land; param_t man_roll_max; param_t man_pitch_max; param_t man_yaw_max; + param_t global_yaw_max; param_t mc_att_yaw_p; param_t hold_xy_dz; param_t hold_z_dz; @@ -199,12 +199,12 @@ private: float thr_max; float tilt_max_air; float land_speed; - float tko_jmpspd; float tko_speed; float tilt_max_land; float man_roll_max; float man_pitch_max; float man_yaw_max; + float global_yaw_max; float mc_att_yaw_p; float hold_xy_dz; float hold_z_dz; @@ -245,11 +245,11 @@ private: math::Matrix<3, 3> _R; /**< rotation matrix from attitude quaternions */ float _yaw; /**< yaw angle (euler) */ - float _landing_thrust; - hrt_abstime _landing_started; + bool _in_landing; bool _takeoff_jumped; float _vel_z_lp; float _acc_z_lp; + float _takeoff_thrust_sp; /** * Update our local parameter cache. @@ -374,11 +374,11 @@ MulticopterPositionControl::MulticopterPositionControl() : _run_pos_control(true), _run_alt_control(true), _yaw(0.0f), - _landing_thrust(1.0f), - _landing_started(0), + _in_landing(false), _takeoff_jumped(false), _vel_z_lp(0), - _acc_z_lp(0) + _acc_z_lp(0), + _takeoff_thrust_sp(0.0f) { memset(&_vehicle_status, 0, sizeof(_vehicle_status)); memset(&_ctrl_state, 0, sizeof(_ctrl_state)); @@ -429,12 +429,12 @@ MulticopterPositionControl::MulticopterPositionControl() : _params_handles.xy_ff = param_find("MPC_XY_FF"); _params_handles.tilt_max_air = param_find("MPC_TILTMAX_AIR"); _params_handles.land_speed = param_find("MPC_LAND_SPEED"); - _params_handles.tko_jmpspd = param_find("MPC_TKO_JMPSPD"); _params_handles.tko_speed = param_find("MPC_TKO_SPEED"); _params_handles.tilt_max_land = param_find("MPC_TILTMAX_LND"); _params_handles.man_roll_max = param_find("MPC_MAN_R_MAX"); _params_handles.man_pitch_max = param_find("MPC_MAN_P_MAX"); _params_handles.man_yaw_max = param_find("MPC_MAN_Y_MAX"); + _params_handles.global_yaw_max = param_find("MC_YAWRATE_MAX"); _params_handles.mc_att_yaw_p = param_find("MC_YAW_P"); _params_handles.hold_xy_dz = param_find("MPC_HOLD_XY_DZ"); _params_handles.hold_z_dz = param_find("MPC_HOLD_Z_DZ"); @@ -493,7 +493,6 @@ MulticopterPositionControl::parameters_update(bool force) param_get(_params_handles.tilt_max_air, &_params.tilt_max_air); _params.tilt_max_air = math::radians(_params.tilt_max_air); param_get(_params_handles.land_speed, &_params.land_speed); - param_get(_params_handles.tko_jmpspd, &_params.tko_jmpspd); param_get(_params_handles.tko_speed, &_params.tko_speed); param_get(_params_handles.tilt_max_land, &_params.tilt_max_land); _params.tilt_max_land = math::radians(_params.tilt_max_land); @@ -551,9 +550,12 @@ MulticopterPositionControl::parameters_update(bool force) param_get(_params_handles.man_roll_max, &_params.man_roll_max); param_get(_params_handles.man_pitch_max, &_params.man_pitch_max); param_get(_params_handles.man_yaw_max, &_params.man_yaw_max); + param_get(_params_handles.global_yaw_max, &_params.global_yaw_max); _params.man_roll_max = math::radians(_params.man_roll_max); _params.man_pitch_max = math::radians(_params.man_pitch_max); _params.man_yaw_max = math::radians(_params.man_yaw_max); + _params.global_yaw_max = math::radians(_params.global_yaw_max); + param_get(_params_handles.mc_att_yaw_p, &v); _params.mc_att_yaw_p = v; } @@ -1358,25 +1360,36 @@ MulticopterPositionControl::task_main() if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { + // check if we are not already in air. + // if yes then we don't need a jumped takeoff anymore + if (!_takeoff_jumped && !_vehicle_status.condition_landed && fabsf(_takeoff_thrust_sp) < FLT_EPSILON ) { + _takeoff_jumped = true; + } + if (!_takeoff_jumped) { - /* do a quick jump (ramp vel sp up in 1/2sec) until we shoot over takeoff speed */ - float accel = _vel_sp_prev(2) - _params.tko_jmpspd; - float tmp = _vel_sp_prev(2) + accel * dt * 2.0f; - _vel_sp(2) = math::max(tmp, -_params.tko_jmpspd); - - if (_vel(2) < -_params.tko_speed) { + // ramp thrust setpoint up + if (_vel(2) > -(_params.tko_speed / 2.0f)) { + _takeoff_thrust_sp += 0.5f * dt; + _vel_sp.zero(); + _vel_prev.zero(); + } else { + // copter has reached our takeoff speed. split the thrust setpoint up + // into an integral part and into a P part + thrust_int(2) = _takeoff_thrust_sp - _params.vel_p(2) * fabsf(_vel(2)); + thrust_int(2) = -math::constrain(thrust_int(2), _params.thr_min, _params.thr_max); + _vel_sp_prev(2) = -_params.tko_speed; _takeoff_jumped = true; + reset_int_z = false; } + } - } else { - /* slowly reduce forced takeoff speed to set climb rate (in 3 sec) */ - float decel = _params.tko_jmpspd - _params.tko_speed; - float tmp = decel / 3.0f * dt + _vel_sp_prev(2); - _vel_sp(2) = math::min(tmp, -_params.tko_speed); + if (_takeoff_jumped) { + _vel_sp(2) = -_params.tko_speed; } } else { _takeoff_jumped = false; + _takeoff_thrust_sp = 0.0f; } // limit total horizontal acceleration @@ -1393,6 +1406,14 @@ MulticopterPositionControl::task_main() _vel_sp(1) = vel_sp_hor(1); } + // limit vertical acceleration + float acc_v = (_vel_sp(2) - _vel_sp_prev(2)) / dt; + + if (fabsf(acc_v) > 2 * _params.acc_hor_max) { + acc_v /= fabsf(acc_v); + _vel_sp(2) = acc_v * 2 * _params.acc_hor_max * dt + _vel_sp_prev(2); + } + _vel_sp_prev = _vel_sp; _global_vel_sp.vx = _vel_sp(0); @@ -1450,6 +1471,13 @@ MulticopterPositionControl::task_main() // TODO?: + _vel_sp.emult(_params.vel_ff) math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + _vel_err_d.emult(_params.vel_d) + thrust_int; + if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF + && !_takeoff_jumped && !_control_mode.flag_control_manual_enabled) { + // for jumped takeoffs use special thrust setpoint calculated above + thrust_sp.zero(); + thrust_sp(2) = -_takeoff_thrust_sp; + } + if (!_control_mode.flag_control_velocity_enabled) { thrust_sp(0) = 0.0f; thrust_sp(1) = 0.0f; @@ -1490,48 +1518,32 @@ MulticopterPositionControl::task_main() thr_min = 0.0f; } - if (_landing_started == 0) { - _landing_started = hrt_absolute_time(); - } - - #if 0 - // TODO quick fix: remove this since in combination with the non-working fall detection - // it can lead to the copter falling out of the sky - /* don't let it throttle up again during landing */ - if (thrust_sp(2) < 0.0f && thrust_abs < _landing_thrust - /* fix landing thrust after a certain time when velocity change is minimal */ + /* descent stabilized, we're in landing */ + if (!_in_landing && (float)fabs(_acc_z_lp) < 0.1f - && _vel_z_lp > 0.5f * _params.land_speed - && hrt_elapsed_time(&_landing_started) > 15e5) { - _landing_thrust = thrust_abs; + && _vel_z_lp > 0.5f * _params.land_speed) { + _in_landing = true; } - #endif /* assume ground, reduce thrust */ - if (hrt_elapsed_time(&_landing_started) > 15e5 - && _landing_thrust > FLT_EPSILON + if (_in_landing && _vel_z_lp < 0.1f ) { - _landing_thrust = 0.0f; + thr_max = 0.0f; } /* if we suddenly fall, reset landing logic and remove thrust limit */ - if (hrt_elapsed_time(&_landing_started) > 15e5 - && _landing_thrust < FLT_EPSILON + if (_in_landing /* XXX: magic value, assuming free fall above 4m/s2 acceleration */ && (_acc_z_lp > 4.0f - || _vel_z_lp > 2.0f * _params.land_speed) + || _vel_z_lp > 2.0f * _params.land_speed) ) { - _landing_thrust = _params.thr_max; - _landing_started = 0; + thr_max = _params.thr_max; + _in_landing = false; } - /* add 5% to give it some margin to compensate external influences */ - thr_max = _landing_thrust + 0.05f * _landing_thrust; - } else { - _landing_started = 0; - _landing_thrust = _params.thr_max; + _in_landing = false; } /* limit min lift */ @@ -1766,14 +1778,19 @@ MulticopterPositionControl::task_main() /* do not move yaw while sitting on the ground */ else if (!_vehicle_status.condition_landed && !(!_control_mode.flag_control_altitude_enabled && _manual.z < 0.1f)) { - const float yaw_offset_max = _params.man_yaw_max / _params.mc_att_yaw_p; - _att_sp.yaw_sp_move_rate = _manual.r * _params.man_yaw_max; + /* we want to know the real constraint, and global overrides manual */ + const float yaw_rate_max = (_params.man_yaw_max < _params.global_yaw_max) ? _params.man_yaw_max : _params.global_yaw_max; + const float yaw_offset_max = yaw_rate_max / _params.mc_att_yaw_p; + + _att_sp.yaw_sp_move_rate = _manual.r * yaw_rate_max; float yaw_target = _wrap_pi(_att_sp.yaw_body + _att_sp.yaw_sp_move_rate * dt); float yaw_offs = _wrap_pi(yaw_target - _yaw); // If the yaw offset became too big for the system to track stop // shifting it + + // XXX this needs inspection - probably requires a clamp, not an if if (fabsf(yaw_offs) < yaw_offset_max) { _att_sp.yaw_body = yaw_target; } diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index 80792304d9..1ddf9114e0 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -234,15 +234,6 @@ PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND, 12.0f); */ PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 0.5f); -/** - * Takeoff initial speed to jump off the ground - * - * @unit m/s - * @min 0.0 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_TKO_JMPSPD, 2.0f); - /** * Takeoff climb rate * diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 992a426bfe..3dd2ce36d1 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -188,11 +188,11 @@ Mission::on_active() } } - /* see if we need to update the current yaw heading for rotary wing types */ - if (_navigator->get_vstatus()->is_rotary_wing - && _param_yawmode.get() != MISSION_YAWMODE_NONE + /* see if we need to update the current yaw heading */ + if ((_param_yawmode.get() != MISSION_YAWMODE_NONE && _param_yawmode.get() < MISSION_YAWMODE_MAX - && _mission_type != MISSION_TYPE_NONE) { + && _mission_type != MISSION_TYPE_NONE) + || _navigator->get_vstatus()->is_vtol) { heading_sp_update(); } @@ -259,7 +259,8 @@ Mission::update_offboard_mission() dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(), _navigator->get_home_position()->alt, _navigator->home_position_valid(), _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, - _param_dist_1wp.get(), _navigator->get_mission_result()->warning); + _param_dist_1wp.get(), _navigator->get_mission_result()->warning, _navigator->get_acceptance_radius(), + _navigator->get_vstatus()->condition_landed); _navigator->get_mission_result()->valid = !failed; _navigator->increment_mission_instance_count(); @@ -561,7 +562,8 @@ Mission::heading_sp_update() } /* always keep the front of the rotary wing pointing to the next waypoint */ - if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_WAYPOINT) { + if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_WAYPOINT + || _navigator->get_vstatus()->is_vtol) { _mission_item.yaw = get_bearing_to_next_waypoint( point_from_latlon[0], point_from_latlon[1], @@ -843,7 +845,8 @@ Mission::check_mission_valid() dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(), _navigator->get_home_position()->alt, _navigator->home_position_valid(), _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, - _param_dist_1wp.get(), _navigator->get_mission_result()->warning); + _param_dist_1wp.get(), _navigator->get_mission_result()->warning, _navigator->get_acceptance_radius(), + _navigator->get_vstatus()->condition_landed); _navigator->increment_mission_instance_count(); _navigator->set_mission_result_updated(); diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index fd9a206c86..d8ee9c8628 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -64,7 +64,9 @@ MissionFeasibilityChecker::MissionFeasibilityChecker() : bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, - float home_alt, bool home_valid, double curr_lat, double curr_lon, float max_waypoint_distance, bool &warning_issued) + float home_alt, bool home_valid, double curr_lat, double curr_lon, float max_waypoint_distance, bool &warning_issued, + float default_acceptance_rad, + bool condition_landed) { bool failed = false; bool warned = false; @@ -83,12 +85,12 @@ bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRota } // check if all mission item commands are supported - failed = failed || !checkMissionItemValidity(dm_current, nMissionItems); + failed = failed || !checkMissionItemValidity(dm_current, nMissionItems, condition_landed); failed = failed || !checkGeofence(dm_current, nMissionItems, geofence); failed = failed || !checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid, warned); if (isRotarywing) { - failed = failed || !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt, home_valid); + failed = failed || !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt, home_valid, default_acceptance_rad); } else { failed = failed || !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt, home_valid); } @@ -96,9 +98,41 @@ bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRota return !failed; } -bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid) +bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, + Geofence &geofence, float home_alt, bool home_valid, float default_acceptance_rad) { - /* no custom rotary wing checks yet */ + /* Check if all all waypoints are above the home altitude, only return false if bool throw_error = true */ + for (size_t i = 0; i < nMissionItems; i++) { + struct mission_item_s missionitem; + const ssize_t len = sizeof(struct mission_item_s); + + if (dm_read(dm_current, i, &missionitem, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + return false; + } + + // look for a takeoff waypoint + if (missionitem.nav_cmd == NAV_CMD_TAKEOFF) { + // make sure that the altitude of the waypoint is at least one meter larger than the acceptance radius + // this makes sure that the takeoff waypoint is not reached before we are at least one meter in the air + float takeoff_alt = missionitem.altitude_is_relative + ? missionitem.altitude + : missionitem.altitude - home_alt; + // check if we should use default acceptance radius + float acceptance_radius = default_acceptance_rad; + + if (missionitem.acceptance_radius > NAV_EPSILON_POSITION) { + acceptance_radius = missionitem.acceptance_radius; + } + + if (takeoff_alt - 1.0f < acceptance_radius) { + mavlink_log_critical(_mavlink_fd, "Mission rejected: Takeoff altitude too low!"); + return false; + } + } + } + + // all checks have passed return true; } @@ -178,7 +212,7 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, return true; } -bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems) { +bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems, bool condition_landed) { // do not allow mission if we find unsupported item for (size_t i = 0; i < nMissionItems; i++) { struct mission_item_s missionitem; @@ -190,7 +224,7 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s return false; } - // check if we find unsupported item and reject mission if so + // check if we find unsupported items and reject mission if so if (missionitem.nav_cmd != NAV_CMD_IDLE && missionitem.nav_cmd != NAV_CMD_WAYPOINT && missionitem.nav_cmd != NAV_CMD_LOITER_UNLIMITED && @@ -207,6 +241,17 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s mavlink_log_critical(_mavlink_fd, "Rejecting mission item %i: unsupported action.", (int)(i+1)); return false; } + + // check if the mission starts with a land command while the vehicle is landed + if (missionitem.nav_cmd == NAV_CMD_LAND && + i == 0 && + condition_landed) { + + mavlink_log_critical(_mavlink_fd, "Rejecting mission that starts with LAND command while vehicle is landed."); + return false; + } + + } return true; } diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index 4586f75a47..7b44c3246d 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -63,7 +63,7 @@ private: /* Checks for all airframes */ bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence); bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid, bool &warning_issued, bool throw_error = false); - bool checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems); + bool checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems, bool condition_landed); bool check_dist_1wp(dm_item_t dm_current, size_t nMissionItems, double curr_lat, double curr_lon, float dist_first_wp, bool &warning_issued); /* Checks specific to fixedwing airframes */ @@ -72,7 +72,7 @@ private: void updateNavigationCapabilities(); /* Checks specific to rotarywing airframes */ - bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid); + bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid, float default_acceptance_rad); public: MissionFeasibilityChecker(); @@ -83,7 +83,8 @@ public: */ bool checkMissionFeasible(int mavlink_fd, bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid, - double curr_lat, double curr_lon, float max_waypoint_distance, bool &warning_issued); + double curr_lat, double curr_lon, float max_waypoint_distance, bool &warning_issued, float default_acceptance_rad, + bool condition_landed); }; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index b16f9b9b91..2bba051573 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -616,7 +616,11 @@ Navigator::get_acceptance_radius(float mission_item_radius) { float radius = mission_item_radius; - if (hrt_elapsed_time(&_nav_caps.timestamp) < 5000000) { + // XXX only use navigation capabilities for now + // when in fixed wing mode + // this might need locking against a commanded transition + // so that a stale _vstatus doesn't trigger an accepted mission item. + if (!_vstatus.is_rotary_wing && !_vstatus.in_transition_mode && hrt_elapsed_time(&_nav_caps.timestamp) < 5000000) { if (_nav_caps.turn_distance > radius) { radius = _nav_caps.turn_distance; } diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 4ff95bd5af..bbcd39c69f 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -418,7 +418,7 @@ mixer_callback(uintptr_t handle, * not loaded faithfully. */ -static char mixer_text[256]; /* large enough for one mixer */ +static char mixer_text[200]; /* large enough for one mixer */ static unsigned mixer_text_length = 0; int diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index 1ee50d5a7f..4715cfd063 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -82,7 +82,7 @@ static volatile uint8_t msg_next_out, msg_next_in; * for mixer handling. Do not allocate more than 80 bytes for * output. */ -#define NUM_MSG 2 +#define NUM_MSG 1 static char msg[NUM_MSG][40]; static void heartbeat_blink(void); diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 57aec762e6..e5f1b7480f 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -109,6 +109,7 @@ #include #include #include +#include #include #include @@ -1095,6 +1096,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct time_offset_s time_offset; struct mc_att_ctrl_status_s mc_att_ctrl_status; struct control_state_s ctrl_state; + struct ekf2_innovations_s innovations; } buf; memset(&buf, 0, sizeof(buf)); @@ -1116,7 +1118,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_STAT_s log_STAT; struct log_VTOL_s log_VTOL; struct log_RC_s log_RC; - struct log_OUT0_s log_OUT0; + struct log_OUT_s log_OUT; struct log_AIRS_s log_AIRS; struct log_ARSP_s log_ARSP; struct log_FLOW_s log_FLOW; @@ -1144,6 +1146,8 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_TSYN_s log_TSYN; struct log_MACS_s log_MACS; struct log_CTS_s log_CTS; + struct log_EST4_s log_INO1; + struct log_EST5_s log_INO2; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -1160,6 +1164,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int att_sp_sub; int rates_sp_sub; int act_outputs_sub; + int act_outputs_1_sub; int act_controls_sub; int act_controls_1_sub; int local_pos_sub; @@ -1187,6 +1192,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int tsync_sub; int mc_att_ctrl_status_sub; int ctrl_state_sub; + int innov_sub; } subs; subs.cmd_sub = -1; @@ -1198,6 +1204,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.att_sp_sub = -1; subs.rates_sp_sub = -1; subs.act_outputs_sub = -1; + subs.act_outputs_1_sub = -1; subs.act_controls_sub = -1; subs.act_controls_1_sub = -1; subs.local_pos_sub = -1; @@ -1222,6 +1229,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.mc_att_ctrl_status_sub = -1; subs.ctrl_state_sub = -1; subs.encoders_sub = -1; + subs.innov_sub = -1; /* add new topics HERE */ @@ -1526,8 +1534,14 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- ACTUATOR OUTPUTS --- */ if (copy_if_updated(ORB_ID(actuator_outputs), &subs.act_outputs_sub, &buf.act_outputs)) { log_msg.msg_type = LOG_OUT0_MSG; - memcpy(log_msg.body.log_OUT0.output, buf.act_outputs.output, sizeof(log_msg.body.log_OUT0.output)); - LOGBUFFER_WRITE_AND_COUNT(OUT0); + memcpy(log_msg.body.log_OUT.output, buf.act_outputs.output, sizeof(log_msg.body.log_OUT.output)); + LOGBUFFER_WRITE_AND_COUNT(OUT); + } + + if (copy_if_updated(ORB_ID(actuator_outputs), &subs.act_outputs_1_sub, &buf.act_outputs)) { + log_msg.msg_type = LOG_OUT1_MSG; + memcpy(log_msg.body.log_OUT.output, buf.act_outputs.output, sizeof(log_msg.body.log_OUT.output)); + LOGBUFFER_WRITE_AND_COUNT(OUT); } /* --- ACTUATOR CONTROL --- */ @@ -1810,6 +1824,29 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(EST3); } + /* --- EKF2 INNOVATIONS --- */ + if (copy_if_updated(ORB_ID(ekf2_innovations), &subs.innov_sub, &buf.innovations)) { + log_msg.msg_type = LOG_EST4_MSG; + memset(&(log_msg.body.log_INO1.s), 0, sizeof(log_msg.body.log_INO1.s)); + for (unsigned i = 0; i < 6; i++) { + log_msg.body.log_INO1.s[i] = buf.innovations.vel_pos_innov[i]; + log_msg.body.log_INO1.s[i + 6] = buf.innovations.vel_pos_innov_var[i]; + } + LOGBUFFER_WRITE_AND_COUNT(EST4); + + log_msg.msg_type = LOG_EST5_MSG; + memset(&(log_msg.body.log_INO2.s), 0, sizeof(log_msg.body.log_INO2.s)); + for (unsigned i = 0; i < 3; i++) { + log_msg.body.log_INO2.s[i] = buf.innovations.mag_innov[i]; + log_msg.body.log_INO2.s[i + 3] = buf.innovations.mag_innov_var[i]; + } + + log_msg.body.log_INO2.s[6] = buf.innovations.heading_innov; + log_msg.body.log_INO2.s[7] = buf.innovations.heading_innov_var; + LOGBUFFER_WRITE_AND_COUNT(EST5); + + } + /* --- TECS STATUS --- */ if (copy_if_updated(ORB_ID(tecs_status), &subs.tecs_status_sub, &buf.tecs_status)) { log_msg.msg_type = LOG_TECS_MSG; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 86c2c58868..a7e2960def 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -195,9 +195,9 @@ struct log_RC_s { uint32_t frame_drop; }; -/* --- OUT0 - ACTUATOR_0 OUTPUT --- */ +/* --- OUT - ACTUATOR OUTPUT --- */ #define LOG_OUT0_MSG 12 -struct log_OUT0_s { +struct log_OUT_s { float output[8]; }; @@ -416,6 +416,18 @@ struct log_EST3_s { float cov[16]; }; +/* --- EST4 - ESTIMATOR INNOVATIONS --- */ +#define LOG_EST4_MSG 48 +struct log_EST4_s { + float s[12]; +}; + +/* --- EST4 - ESTIMATOR INNOVATIONS --- */ +#define LOG_EST5_MSG 49 +struct log_EST5_s { + float s[8]; +}; + /* --- TEL0..3 - TELEMETRY STATUS --- */ #define LOG_TEL0_MSG 36 #define LOG_TEL1_MSG 37 @@ -479,6 +491,8 @@ struct log_MACS_s { float yaw_rate_integ; }; +/* WARNING: ID 46 is already in use for ATTC1 */ + /* --- CONTROL STATE --- */ #define LOG_CTS_MSG 47 struct log_CTS_s { @@ -491,7 +505,7 @@ struct log_CTS_s { float yaw_rate; }; -/* WARNING: ID 46 is already in use for ATTC1 */ +#define LOG_OUT1_MSG 50 /********** SYSTEM MESSAGES, ID > 0x80 **********/ @@ -535,7 +549,8 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(VTOL, "f", "Arsp"), LOG_FORMAT(CTS, "fffffff", "Vx_b,Vy_b,Vz_b,Vinf,P,Q,R"), LOG_FORMAT(RC, "ffffffffffffBBBL", "C0,C1,C2,C3,C4,C5,C6,C7,C8,C9,C10,C11,RSSI,CNT,Lost,Drop"), - LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), + LOG_FORMAT_S(OUT0, OUT, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), + LOG_FORMAT_S(OUT1, OUT, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), LOG_FORMAT(AIRS, "fff", "IndSpeed,TrueSpeed,AirTemp"), LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), LOG_FORMAT(FLOW, "BffffffLLHhB", "ID,RawX,RawY,RX,RY,RZ,Dist,TSpan,DtSonar,FrmCnt,GT,Qlty"), @@ -553,6 +568,8 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(EST1, "ffffffffffffffff", "s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27"), LOG_FORMAT(EST2, "ffffffffffff", "P0,P1,P2,P3,P4,P5,P6,P7,P8,P9,P10,P11"), LOG_FORMAT(EST3, "ffffffffffffffff", "P12,P13,P14,P15,P16,P17,P18,P19,P20,P21,P22,P23,P24,P25,P26,P27"), + LOG_FORMAT(EST4, "ffffffffffff", "VxI,VyI,VzI,PxI,PyI,PzI,VxIV,VyIV,VzIV,PxIV,PyIV,PzIV"), + LOG_FORMAT(EST5, "ffffffff", "MAGxI,MAGyI,MAGzI,MAGxIV,MAGyIV,MAGzIV,HeadI,HeadIV"), LOG_FORMAT(PWR, "fffBBBBB", "Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"), LOG_FORMAT(MOCP, "fffffff", "QuatW,QuatX,QuatY,QuatZ,X,Y,Z"), LOG_FORMAT(VISN, "ffffffffff", "X,Y,Z,VX,VY,VZ,QuatW,QuatX,QuatY,QuatZ"), diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index aa91fcf92a..d2faa1a4f5 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -1842,7 +1842,7 @@ PARAM_DEFINE_FLOAT(BAT_V_SCALING, -1.0f); * * @group Battery Calibration */ -PARAM_DEFINE_FLOAT(BAT_C_SCALING, 0.0124); /* scaling for 3DR power brick */ +PARAM_DEFINE_FLOAT(BAT_C_SCALING, -1.0); /** @@ -2009,6 +2009,15 @@ PARAM_DEFINE_INT32(RC_MAP_ACRO_SW, 0); */ PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); +/** + * Kill switch channel mapping. + * + * @min 0 + * @max 18 + * @group Radio Switches + */ +PARAM_DEFINE_INT32(RC_MAP_KILL_SW, 0); + /** * Flaps channel mapping. * @@ -2243,6 +2252,25 @@ PARAM_DEFINE_FLOAT(RC_ACRO_TH, 0.5f); */ PARAM_DEFINE_FLOAT(RC_OFFB_TH, 0.5f); + +/** + * Threshold for the kill switch + * + * 0-1 indicate where in the full channel range the threshold sits + * 0 : min + * 1 : max + * sign indicates polarity of comparison + * positive : true when channel>th + * negative : true when channel #include #include +#include +#include #include #include #include @@ -235,9 +237,9 @@ param_find_changed(param_t param) } static void -param_notify_changes(void) +param_notify_changes(bool is_saved) { - struct parameter_update_s pup = { .timestamp = hrt_absolute_time() }; + struct parameter_update_s pup = { .timestamp = hrt_absolute_time(), .saved = is_saved}; /* * If we don't have a handle to our topic, create one now; otherwise @@ -497,7 +499,7 @@ param_get(param_t param, void *val) } static int -param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_changes) +param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_changes, bool is_saved) { int result = -1; bool params_changed = false; @@ -575,7 +577,7 @@ out: * a thing has been set. */ if (params_changed && notify_changes) { - param_notify_changes(); + param_notify_changes(is_saved); } return result; @@ -584,13 +586,19 @@ out: int param_set(param_t param, const void *val) { - return param_set_internal(param, val, false, true); + return param_set_internal(param, val, false, true, false); +} + +int +param_set_no_autosave(param_t param, const void *val) +{ + return param_set_internal(param, val, false, true, true); } int param_set_no_notification(param_t param, const void *val) { - return param_set_internal(param, val, false, false); + return param_set_internal(param, val, false, false, false); } bool @@ -643,7 +651,7 @@ param_reset(param_t param) param_unlock(); if (s != NULL) { - param_notify_changes(); + param_notify_changes(false); } return (!param_found); @@ -663,7 +671,7 @@ param_reset_all(void) param_unlock(); - param_notify_changes(); + param_notify_changes(false); } void @@ -695,7 +703,7 @@ param_reset_excludes(const char *excludes[], int num_excludes) param_unlock(); - param_notify_changes(); + param_notify_changes(false); } static const char *param_default_file = PX4_ROOTFSDIR"/eeprom/parameters"; @@ -738,7 +746,13 @@ param_save_default(void) return ERROR; } - res = param_export(fd, false); + res = 1; + int attempts = 5; + + while (res != OK && attempts > 0) { + res = param_export(fd, false); + attempts--; + } if (res != OK) { warnx("failed to write parameters to file: %s", filename); @@ -779,6 +793,38 @@ param_load_default(void) return 0; } +#if defined (CONFIG_ARCH_BOARD_PX4FMU_V4) +//struct spi_dev_s *dev = nullptr; +irqstate_t state; +#endif + +static void +param_bus_lock(bool lock) +{ + +#if defined (CONFIG_ARCH_BOARD_PX4FMU_V4) + // FMUv4 has baro and FRAM on the same bus, + // as this offers on average a 100% silent + // bus for the baro operation + + // XXX this would be the preferred locking method + // if (dev == nullptr) { + // dev = up_spiinitialize(PX4_SPI_BUS_BARO); + // } + + // SPI_LOCK(dev, lock); + + // we lock like this for Pixracer for now + if (lock) { + state = irqsave(); + + } else { + irqrestore(state); + } + +#endif +} + int param_export(int fd, bool only_unsaved) { @@ -788,7 +834,9 @@ param_export(int fd, bool only_unsaved) param_lock(); + param_bus_lock(true); bson_encoder_init_file(&encoder, fd); + param_bus_lock(false); /* no modified parameters -> we are done */ if (param_values == NULL) { @@ -813,6 +861,9 @@ param_export(int fd, bool only_unsaved) /* append the appropriate BSON type object */ + /* lock as short as possible */ + param_bus_lock(true); + switch (param_type(s->param)) { case PARAM_TYPE_INT32: @@ -820,6 +871,7 @@ param_export(int fd, bool only_unsaved) if (bson_encoder_append_int(&encoder, param_name(s->param), i)) { debug("BSON append failed for '%s'", param_name(s->param)); + param_bus_lock(false); goto out; } @@ -830,6 +882,7 @@ param_export(int fd, bool only_unsaved) if (bson_encoder_append_double(&encoder, param_name(s->param), f)) { debug("BSON append failed for '%s'", param_name(s->param)); + param_bus_lock(false); goto out; } @@ -842,6 +895,7 @@ param_export(int fd, bool only_unsaved) param_size(s->param), param_get_value_ptr(s->param))) { debug("BSON append failed for '%s'", param_name(s->param)); + param_bus_lock(false); goto out; } @@ -849,8 +903,14 @@ param_export(int fd, bool only_unsaved) default: debug("unrecognized parameter type"); + param_bus_lock(false); goto out; } + + param_bus_lock(false); + + /* allow this process to be interrupted by another process / thread */ + usleep(5); } result = 0; @@ -955,7 +1015,7 @@ param_import_callback(bson_decoder_t decoder, void *private, bson_node_t node) goto out; } - if (param_set_internal(param, v, state->mark_saved, true)) { + if (param_set_internal(param, v, state->mark_saved, true, false)) { debug("error setting value for '%s'", node->name); goto out; } @@ -984,15 +1044,23 @@ param_import_internal(int fd, bool mark_saved) int result = -1; struct param_import_state state; + param_bus_lock(true); + if (bson_decoder_init_file(&decoder, fd, param_import_callback, &state)) { debug("decoder init failed"); + param_bus_lock(false); goto out; } + param_bus_lock(false); + state.mark_saved = mark_saved; do { + param_bus_lock(true); result = bson_decoder_next(&decoder); + usleep(1); + param_bus_lock(false); } while (result > 0); diff --git a/src/modules/systemlib/param/param.h b/src/modules/systemlib/param/param.h index f8334361f1..8c49a0b01c 100644 --- a/src/modules/systemlib/param/param.h +++ b/src/modules/systemlib/param/param.h @@ -211,6 +211,16 @@ __EXPORT int param_get(param_t param, void *val); */ __EXPORT int param_set(param_t param, const void *val); +/** + * Set the value of a parameter, but do not trigger an auto-save + * + * @param param A handle returned by param_find or passed by param_foreach. + * @param val The value to set; assumed to point to a variable of the parameter type. + * For structures, the pointer is assumed to point to a structure to be copied. + * @return Zero if the parameter's value could be set from a scalar, nonzero otherwise. + */ +__EXPORT int param_set_no_autosave(param_t param, const void *val); + /** * Set the value of a parameter, but do not notify the system about the change. * diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 15dab60df4..0ff426e3b5 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -273,3 +273,6 @@ ORB_DEFINE(camera_trigger, struct camera_trigger_s); #include "topics/vehicle_command_ack.h" ORB_DEFINE(vehicle_command_ack, struct vehicle_command_ack_s); + +#include "topics/ekf2_innovations.h" +ORB_DEFINE(ekf2_innovations, struct ekf2_innovations_s); diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index fe7d8ebd82..49643ee422 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -203,3 +203,12 @@ PARAM_DEFINE_FLOAT(VT_ARSP_BLEND, 8.0f); * @group VTOL Attitude Control */ PARAM_DEFINE_FLOAT(VT_ARSP_TRANS, 10.0f); + +/** + * Enable optimal recovery strategy for pitch-weak tailsitters + * + * @min 0 + * @max 1 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_INT32(VT_OPT_RECOV_EN, 0); diff --git a/src/platforms/posix/main.cpp b/src/platforms/posix/main.cpp index 749feaa6a3..91bb6b3074 100644 --- a/src/platforms/posix/main.cpp +++ b/src/platforms/posix/main.cpp @@ -112,10 +112,16 @@ static void run_cmd(const vector &appargs, bool exit_on_fail) int retval = apps[command](i, (char **)arg); - if (exit_on_fail && retval) { - exit(retval); + if (retval) { + cout << "Command failed, returned " << retval << endl; + + if (exit_on_fail && retval) { + exit(retval); + } } + + } else if (command.compare("help") == 0) { list_builtins(); @@ -126,17 +132,13 @@ static void run_cmd(const vector &appargs, bool exit_on_fail) cout << endl << "Invalid command: " << command << "\ntype 'help' for a list of commands" << endl; } - - if (exit_on_fail) { - print_prompt(); - } } static void usage() { cout << "./mainapp [-d] [startup_config] -h" << std::endl; - cout << " -d - Optional flag to run the app in daemon mode and does not take listen for user input." << + cout << " -d - Optional flag to run the app in daemon mode and does not listen for user input." << std::endl; cout << " This is needed if mainapp is intended to be run as a upstart job on linux" << std::endl; cout << " - config file for starting/stopping px4 modules" << std::endl; @@ -215,6 +217,7 @@ int main(int argc, char **argv) if (infile.is_open()) { for (string line; getline(infile, line, '\n');) { + // TODO: this should be true but for that we have to check all startup files process_line(line, false); } @@ -308,9 +311,11 @@ int main(int argc, char **argv) } } - process_line(mystr, !daemon_mode); + process_line(mystr, false); mystr = ""; buf_ptr_read = buf_ptr_write; + + print_prompt(); break; case '\033': { // arrow keys diff --git a/src/systemcmds/param/CMakeLists.txt b/src/systemcmds/param/CMakeLists.txt index cd35b0da04..8158bfb09e 100644 --- a/src/systemcmds/param/CMakeLists.txt +++ b/src/systemcmds/param/CMakeLists.txt @@ -33,7 +33,7 @@ px4_add_module( MODULE systemcmds__param MAIN param - STACK 1800 + STACK 2500 COMPILE_FLAGS -Os SRCS diff --git a/src/systemcmds/param/module.mk b/src/systemcmds/param/module.mk deleted file mode 100644 index f716eb71e9..0000000000 --- a/src/systemcmds/param/module.mk +++ /dev/null @@ -1,45 +0,0 @@ -############################################################################ -# -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# Build the parameters tool. -# - -MODULE_COMMAND = param -SRCS = param.c - -# Note: measurements yielded a max of 900 bytes used. -MODULE_STACKSIZE = 1800 - -MAXOPTIMIZATION = -Os - diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index 88f5197d27..19be8fa0d6 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -448,7 +448,7 @@ do_set(const char *name, const char *val, bool fail_on_not_found) param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'), param_name(param)); printf("curr: %ld", (long)i); - param_set(param, &newval); + param_set_no_autosave(param, &newval); printf(" -> new: %ld\n", (long)newval); } } @@ -470,7 +470,7 @@ do_set(const char *name, const char *val, bool fail_on_not_found) param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'), param_name(param)); printf("curr: %4.4f", (double)f); - param_set(param, &newval); + param_set_no_autosave(param, &newval); printf(" -> new: %4.4f\n", (double)newval); } @@ -483,7 +483,13 @@ do_set(const char *name, const char *val, bool fail_on_not_found) return 1; } - return 0; + if (param_save_default()) { + warnx("Param export failed."); + return 1; + + } else { + return 0; + } } static int diff --git a/src/systemcmds/tests/CMakeLists.txt b/src/systemcmds/tests/CMakeLists.txt index e9cb903477..8fc1ea5451 100644 --- a/src/systemcmds/tests/CMakeLists.txt +++ b/src/systemcmds/tests/CMakeLists.txt @@ -69,7 +69,7 @@ endif() px4_add_module( MODULE systemcmds__tests MAIN tests - STACK 60000 + STACK 8000 COMPILE_FLAGS -Wframe-larger-than=6000 -Wno-float-equal