fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-20 07:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' into beta

Browse Source
This commit is contained in:
Lorenz Meier
2016-01-27 14:08:33 +01:00
parent 44792eb2fe f35e3335be
commit 1e7cfae683
69 changed files with 1388 additions and 372 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Makefile
+8 -1
View File
@@ -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
ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol
+1 -1
View File
@@ -2,7 +2,7 @@
#
# @name Duorotor Tailsitter
#
# @type VTOL Tailsitter
# @type VTOL Duo Tailsitter
#
# @maintainer Roman Bapst <roman@px4.io>
#
ROMFS/px4fmu_common/init.d/13008_QuadRanger
+4 -4
View File
@@ -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
ROMFS/px4fmu_common/init.d/rcS
+12 -1
View File
@@ -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
Tools/check_code_style.sh
-1
View File
@@ -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 \
Tools/generate_listener.py
+15 -7
View File
@@ -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}")
Tools/px4airframes/xmlout.py
+1 -1
View File
@@ -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"
Tools/px_romfs_pruner.py
+37 -33
View File
@@ -1,8 +1,7 @@
#!/usr/bin/env python
############################################################################
#
# Copyright (C) 2014 PX4 Development Team. All rights reserved.
# Author: Julian Oes <joes@student.ethz.ch>
# 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 <julian@oes.ch>
"""
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()
Tools/px_uploader.py
+2 -1
View File
@@ -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'):
Tools/sitl_gazebo
+1 -1
Submodule Tools/sitl_gazebo updated: 57a54ab737...16f7fb3761
cmake/configs/posix_rpi2_default.cmake
+47
View File
@@ -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
)
cmake/configs/posix_rpi2_release.cmake
+56
View File
@@ -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
)
cmake/configs/posix_sitl_default.cmake
+1
View File
@@ -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
msg/ekf2_innovations.msg
+7
View File
@@ -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
msg/manual_control_setpoint.msg
+3 -2
View File
@@ -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
msg/parameter_update.msg
+1
View File
@@ -1 +1,2 @@
uint64 timestamp # time at which the latest parameter was updated
bool saved # wether the change has already been saved to disk
msg/rc_channels.msg
+4 -3
View File
@@ -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
nuttx-configs/px4io-v2/nsh/defconfig
+2 -2
View File
@@ -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
posix-configs/SITL/init/rcS_jmavsim_iris
+1 -2
View File
@@ -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
posix-configs/SITL/init/rcS_lpe_jmavsim_iris
+16 -2
View File
@@ -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
posix-configs/rpi2/init/rcS_navio
View File
src/drivers/boards/px4fmu-v4/board_config.h
+1 -2
View File
@@ -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
*
src/drivers/boards/px4fmu-v4/px4fmu_init.c
+1 -5
View File
@@ -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);
src/drivers/boards/px4fmu-v4/px4fmu_pwm_servo.c
+6 -6
View File
@@ -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,
}
};
src/drivers/device/spi.h
+16 -6
View File
@@ -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 */
src/drivers/frsky_telemetry/CMakeLists.txt
+1
View File
@@ -38,6 +38,7 @@ px4_add_module(
-Os
SRCS
frsky_data.c
sPort_data.c
frsky_telemetry.c
DEPENDS
platforms__common
src/drivers/frsky_telemetry/frsky_telemetry.c
+189 -31
View File
@@ -35,11 +35,13 @@
/**
* @file frsky_telemetry.c
* @author Stefan Rado <px4@sradonia.net>
* @author Mark Whitehorn <kd0aij@github.com>
*
* 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 <stdbool.h>
#include <string.h>
#include <sys/types.h>
#include <poll.h>
#include <fcntl.h>
#include <unistd.h>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
#include <termios.h>
#include <drivers/drv_hrt.h>
#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);
src/drivers/frsky_telemetry/sPort_data.c
+217
View File
@@ -0,0 +1,217 @@
/****************************************************************************
*
* Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
* Author: Stefan Rado <px4@sradonia.net>
*
* 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 <px4@sradonia.net>
* @author Mark Whitehorn <kd0aij@github.com>
*
* FrSky SmartPort telemetry implementation.
*
*/
#include "sPort_data.h"
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <arch/math.h>
#include <geo/geo.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_status.h>
#include <drivers/drv_hrt.h>
#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);
}
src/drivers/frsky_telemetry/sPort_data.h
+87
View File
@@ -0,0 +1,87 @@
/****************************************************************************
*
* Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
* Author: Stefan Rado <px4@sradonia.net>
*
* 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 <kd0aij@github.com>
*
* FrSky SmartPort telemetry implementation.
*
*/
#ifndef _SPORT_DATA_H
#define _SPORT_DATA_H
#include <sys/types.h>
/* 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 */
src/drivers/gps/gps.cpp
+6 -1
View File
@@ -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)) {
src/drivers/ms5611/ms5611_spi.cpp
+6 -1
View File
@@ -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();
src/drivers/px4fmu/fmu.cpp
+17 -17
View File
@@ -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
src/drivers/px4io/px4io.cpp
+25 -12
View File
@@ -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
src/drivers/stm32/adc/adc.cpp
+4 -4
View File
@@ -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);
}
src/lib/ecl
+1 -1
Submodule src/lib/ecl updated: a11709956f...8d0022ab1e
src/lib/matrix
+1 -1
Submodule src/lib/matrix updated: 1d9d8e6f9b...ce30542dba
src/lib/version/version.h
+5
View File
@@ -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_ */
src/modules/commander/accelerometer_calibration.cpp
+2 -1
View File
@@ -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;
}
src/modules/commander/commander.cpp
+20 -10
View File
@@ -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) {
src/modules/commander/state_machine_helper.cpp
+14 -5
View File
@@ -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;
}
}
/*
src/modules/ekf2/ekf2_main.cpp
+84 -23
View File
@@ -72,7 +72,8 @@
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/ekf2_innovations.h>
#include <ecl/EKF/ekf.h>
@@ -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, &params->mag_delay_ms);
_baro_delay_ms = new control::BlockParamFloat(this, "EKF2_BARO_DELAY", false, &params->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;
}
src/modules/mavlink/CMakeLists.txt
+1 -1
View File
@@ -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
src/modules/mavlink/mavlink_main.cpp
+8 -3
View File
@@ -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);
src/modules/mavlink/mavlink_main.h
+2 -1
View File
@@ -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;
src/modules/mavlink/mavlink_messages.cpp
+36 -14
View File
@@ -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);
src/modules/mavlink/mavlink_receiver.cpp
+7 -3
View File
@@ -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
src/modules/mc_att_control/mc_att_control_main.cpp
+12 -2
View File
@@ -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;
src/modules/mc_att_control/mc_att_control_params.c
+2 -1
View File
@@ -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
src/modules/mc_pos_control/mc_pos_control_main.cpp
+66 -49
View File
@@ -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;
}
src/modules/mc_pos_control/mc_pos_control_params.c
-9
View File
@@ -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
*
src/modules/navigator/mission.cpp
+10 -7
View File
@@ -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();
src/modules/navigator/mission_feasibility_checker.cpp
+52 -7
View File
@@ -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;
}
src/modules/navigator/mission_feasibility_checker.h
+4 -3
View File
@@ -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);
};
src/modules/navigator/navigator_main.cpp
+5 -1
View File
@@ -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;
}
src/modules/px4iofirmware/mixer.cpp
+1 -1
View File
@@ -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
src/modules/px4iofirmware/px4io.c
+1 -1
View File
@@ -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);
src/modules/sdlog2/sdlog2.c
+40 -3
View File
@@ -109,6 +109,7 @@
#include <uORB/topics/vtol_vehicle_status.h>
#include <uORB/topics/time_offset.h>
#include <uORB/topics/mc_att_ctrl_status.h>
#include <uORB/topics/ekf2_innovations.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
@@ -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;
src/modules/sdlog2/sdlog2_messages.h
+21 -4
View File
@@ -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"),
src/modules/sensors/sensor_params.c
+29 -1
View File
@@ -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<th
*
* @min -1
* @max 1
* @group Radio Switches
*
*
*/
PARAM_DEFINE_FLOAT(RC_KILLSWITCH_TH, 0.25f);
/**
* PWM input channel that provides RSSI.
*
src/modules/sensors/sensors.cpp
+44 -4
View File
@@ -277,6 +277,7 @@ private:
int rc_map_loiter_sw;
int rc_map_acro_sw;
int rc_map_offboard_sw;
int rc_map_kill_sw;
int rc_map_flaps;
@@ -297,6 +298,7 @@ private:
float rc_loiter_th;
float rc_acro_th;
float rc_offboard_th;
float rc_killswitch_th;
bool rc_assist_inv;
bool rc_auto_inv;
bool rc_rattitude_inv;
@@ -305,6 +307,7 @@ private:
bool rc_loiter_inv;
bool rc_acro_inv;
bool rc_offboard_inv;
bool rc_killswitch_inv;
float battery_voltage_scaling;
float battery_current_scaling;
@@ -336,6 +339,7 @@ private:
param_t rc_map_loiter_sw;
param_t rc_map_acro_sw;
param_t rc_map_offboard_sw;
param_t rc_map_kill_sw;
param_t rc_map_flaps;
@@ -360,6 +364,7 @@ private:
param_t rc_loiter_th;
param_t rc_acro_th;
param_t rc_offboard_th;
param_t rc_killswitch_th;
param_t battery_voltage_scaling;
param_t battery_current_scaling;
@@ -589,6 +594,7 @@ Sensors::Sensors() :
_parameter_handles.rc_map_loiter_sw = param_find("RC_MAP_LOITER_SW");
_parameter_handles.rc_map_acro_sw = param_find("RC_MAP_ACRO_SW");
_parameter_handles.rc_map_offboard_sw = param_find("RC_MAP_OFFB_SW");
_parameter_handles.rc_map_kill_sw = param_find("RC_MAP_KILL_SW");
_parameter_handles.rc_map_aux1 = param_find("RC_MAP_AUX1");
_parameter_handles.rc_map_aux2 = param_find("RC_MAP_AUX2");
@@ -614,6 +620,7 @@ Sensors::Sensors() :
_parameter_handles.rc_loiter_th = param_find("RC_LOITER_TH");
_parameter_handles.rc_acro_th = param_find("RC_ACRO_TH");
_parameter_handles.rc_offboard_th = param_find("RC_OFFB_TH");
_parameter_handles.rc_killswitch_th = param_find("RC_KILLSWITCH_TH");
/* Differential pressure offset */
_parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF");
@@ -655,6 +662,7 @@ Sensors::Sensors() :
(void)param_find("PWM_AUX_DISARMED");
(void)param_find("TRIG_MODE");
(void)param_find("UAVCAN_ENABLE");
(void)param_find("LPE_ENABLED");
/* fetch initial parameter values */
parameters_update();
@@ -774,6 +782,10 @@ Sensors::parameters_update()
warnx("%s", paramerr);
}
if (param_get(_parameter_handles.rc_map_kill_sw, &(_parameters.rc_map_kill_sw)) != OK) {
warnx("%s", paramerr);
}
if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) {
warnx("%s", paramerr);
}
@@ -813,6 +825,9 @@ Sensors::parameters_update()
param_get(_parameter_handles.rc_offboard_th, &(_parameters.rc_offboard_th));
_parameters.rc_offboard_inv = (_parameters.rc_offboard_th < 0);
_parameters.rc_offboard_th = fabs(_parameters.rc_offboard_th);
param_get(_parameter_handles.rc_killswitch_th, &(_parameters.rc_killswitch_th));
_parameters.rc_killswitch_inv = (_parameters.rc_killswitch_th < 0);
_parameters.rc_killswitch_th = fabs(_parameters.rc_killswitch_th);
/* update RC function mappings */
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE] = _parameters.rc_map_throttle - 1;
@@ -827,6 +842,7 @@ Sensors::parameters_update()
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_LOITER] = _parameters.rc_map_loiter_sw - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_ACRO] = _parameters.rc_map_acro_sw - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD] = _parameters.rc_map_offboard_sw - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_KILLSWITCH] = _parameters.rc_map_kill_sw - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS] = _parameters.rc_map_flaps - 1;
@@ -850,12 +866,13 @@ Sensors::parameters_update()
} else if (_parameters.battery_voltage_scaling < 0.0f) {
/* apply scaling according to defaults if set to default */
#if defined (CONFIG_ARCH_BOARD_PX4FMU_V2) || defined (CONFIG_ARCH_BOARD_PX4FMU_V4)
#if defined (CONFIG_ARCH_BOARD_PX4FMU_V4)
_parameters.battery_voltage_scaling = 0.011f;
#elif defined (CONFIG_ARCH_BOARD_PX4FMU_V2)
_parameters.battery_voltage_scaling = 0.0082f;
#elif CONFIG_ARCH_BOARD_AEROCORE
#elif defined (CONFIG_ARCH_BOARD_AEROCORE)
_parameters.battery_voltage_scaling = 0.0063f;
#elif CONFIG_ARCH_BOARD_PX4FMU_V1
#elif defined (CONFIG_ARCH_BOARD_PX4FMU_V1)
_parameters.battery_voltage_scaling = 0.00459340659f;
#else
/* ensure a missing default trips a low voltage lockdown */
@@ -866,6 +883,23 @@ Sensors::parameters_update()
/* scaling of ADC ticks to battery current */
if (param_get(_parameter_handles.battery_current_scaling, &(_parameters.battery_current_scaling)) != OK) {
warnx("%s", paramerr);
} else if (_parameters.battery_current_scaling < 0.0f) {
/* apply scaling according to defaults if set to default */
#if defined (CONFIG_ARCH_BOARD_PX4FMU_V4)
/* current scaling for ACSP4 */
_parameters.battery_current_scaling = 0.029296875f;
#elif defined (CONFIG_ARCH_BOARD_PX4FMU_V2)
/* current scaling for 3DR power brick */
_parameters.battery_current_scaling = 0.0124f;
#elif defined (CONFIG_ARCH_BOARD_AEROCORE)
_parameters.battery_current_scaling = 0.0124f;
#elif defined (CONFIG_ARCH_BOARD_PX4FMU_V1)
_parameters.battery_current_scaling = 0.0124f;
#else
/* ensure a missing default leads to an unrealistic current value */
_parameters.battery_voltage_scaling = 0.00001f;
#endif
}
param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation));
@@ -1676,6 +1710,8 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
_battery_current_timestamp = t;
#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
} else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) {
/* calculate airspeed, raw is the difference from */
@@ -1704,6 +1740,8 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
_diff_pres_pub = orb_advertise(ORB_ID(differential_pressure), &_diff_pres);
}
}
#endif
}
}
@@ -1954,6 +1992,8 @@ Sensors::rc_poll()
_parameters.rc_acro_inv);
manual.offboard_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD,
_parameters.rc_offboard_th, _parameters.rc_offboard_inv);
manual.kill_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_KILLSWITCH,
_parameters.rc_killswitch_th, _parameters.rc_killswitch_inv);
/* publish manual_control_setpoint topic */
if (_manual_control_pub != nullptr) {
src/modules/systemlib/param/param.c
+79 -11
View File
@@ -44,6 +44,8 @@
//#include <debug.h>
#include <px4_defines.h>
#include <px4_posix.h>
#include <px4_config.h>
#include <px4_spi.h>
#include <string.h>
#include <stdbool.h>
#include <stdlib.h>
@@ -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);
src/modules/systemlib/param/param.h
+10
View File
@@ -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.
*
src/modules/uORB/objects_common.cpp
+3
View File
@@ -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);
src/modules/vtol_att_control/vtol_att_control_params.c
+9
View File
@@ -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);
src/platforms/posix/main.cpp
+13 -8
View File
@@ -112,10 +112,16 @@ static void run_cmd(const vector<string> &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<string> &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 << "<startup_config> - 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
src/systemcmds/param/CMakeLists.txt
+1 -1
View File
@@ -33,7 +33,7 @@
px4_add_module(
MODULE systemcmds__param
MAIN param
STACK 1800
STACK 2500
COMPILE_FLAGS
-Os
SRCS
src/systemcmds/param/module.mk
-45
View File
@@ -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
src/systemcmds/param/param.c
+9 -3
View File
@@ -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
src/systemcmds/tests/CMakeLists.txt
+1 -1
View File
@@ -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