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

Merged master

This commit is contained in:
Lorenz Meier 2015-10-14 22:07:49 +02:00
parent 7dacb835b9 dbcb226de6
commit b126f00052
39 changed files with 530 additions and 318 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
.gitmodules vendored
View File

@ -4,8 +4,8 @@
[submodule "NuttX"]
path = NuttX
url = git://github.com/PX4/NuttX.git
[submodule "src/lib/uavcan"]
path = src/lib/uavcan
[submodule "src/modules/uavcan/libuavcan"]
path = src/modules/uavcan/libuavcan
url = git://github.com/UAVCAN/libuavcan.git
[submodule "Tools/genmsg"]
path = Tools/genmsg

2
CMakeLists.txt
View File

@ -224,7 +224,7 @@ px4_add_git_submodule(TARGET git_genmsg PATH "Tools/genmsg")
px4_add_git_submodule(TARGET git_gencpp PATH "Tools/gencpp")
px4_add_git_submodule(TARGET git_mavlink PATH "mavlink/include/mavlink/v1.0")
px4_add_git_submodule(TARGET git_gtest PATH "unittets/gtest")
px4_add_git_submodule(TARGET git_uavcan PATH "src/lib/uavcan")
px4_add_git_submodule(TARGET git_uavcan PATH "src/modules/uavcan/libuavcan")
px4_add_git_submodule(TARGET git_eigen PATH "src/lib/eigen")
px4_add_git_submodule(TARGET git_nuttx PATH "NuttX")
px4_add_git_submodule(TARGET git_dspal PATH "src/lib/dspal")

20
Makefile
View File

@ -74,7 +74,13 @@ ifdef NINJA_BUILD
PX4_MAKE = ninja
PX4_MAKE_ARGS =
else
PX4_CMAKE_GENERATOR ?= "Unix Makefiles"
ifdef SYSTEMROOT
# Windows
PX4_CMAKE_GENERATOR ?= "MSYS Makefiles"
else
PX4_CMAKE_GENERATOR ?= "Unix Makefiles"
endif
PX4_MAKE = make
PX4_MAKE_ARGS = -j$(j) --no-print-directory
endif
@ -83,7 +89,7 @@ endif
# --------------------------------------------------------------------
# describe how to build a cmake config
define cmake-build
+@if [ ! -e $(PWD)/build_$@/CMakeCache.txt ]; then mkdir -p $(PWD)/build_$@ && cd $(PWD)/build_$@ && cmake .. -G$(PX4_CMAKE_GENERATOR) -DCONFIG=$(1); fi
+@if [ ! -e $(PWD)/build_$@/CMakeCache.txt ]; then git submodule update --init --recursive --force && mkdir -p $(PWD)/build_$@ && cd $(PWD)/build_$@ && cmake .. -G$(PX4_CMAKE_GENERATOR) -DCONFIG=$(1); fi
+$(PX4_MAKE) -C $(PWD)/build_$@ $(PX4_MAKE_ARGS) $(ARGS)
endef
@ -166,14 +172,8 @@ check_format:
clean:
@rm -rf build_*/
distclean: clean
@cd NuttX
@git clean -d -f -x
@cd ..
@cd src/lib/uavcan
@git clean -d -f -x
@cd ../../..
@(cd NuttX && git clean -d -f -x)
@(cd src/modules/uavcan/libuavcan && git clean -d -f -x)
# targets handled by cmake
cmake_targets = test upload package package_source debug debug_tui debug_ddd debug_io debug_io_tui debug_io_ddd check_weak libuavcan

2
Tools/check_code_style.sh
View File

@ -25,7 +25,7 @@ for fn in $(find src/examples \
-path './NuttX' -prune -o \
-path './src/lib/eigen' -prune -o \
-path './src/lib/mathlib/CMSIS' -prune -o \
-path './src/lib/uavcan' -prune -o \
-path './src/modules/uavcan/libuavcan' -prune -o \
-path './src/modules/attitude_estimator_ekf/codegen' -prune -o \
-path './src/modules/ekf_att_pos_estimator' -prune -o \
-path './src/modules/sdlog2' -prune -o \

144
Tools/check_submodules.sh
View File

@ -1,144 +0,0 @@
#!/bin/sh
[ -n "$GIT_SUBMODULES_ARE_EVIL" ] && {
# GIT_SUBMODULES_ARE_EVIL is set, meaning user doesn't want submodules
echo "Skipping submodules. NUTTX_SRC is set to $NUTTX_SRC"
exit 0
}
if [ -d NuttX/nuttx ];
then
STATUSRETVAL=$(git submodule summary | grep -A20 -i "NuttX" | grep "<")
if [ -z "$STATUSRETVAL" ]; then
echo "Checked NuttX submodule, correct version found"
else
echo ""
echo ""
echo "New commits required:"
echo "$(git submodule summary)"
echo ""
echo ""
echo " NuttX sub repo not at correct version. Try 'git submodule update'"
echo " or follow instructions on http://pixhawk.org/dev/git/submodules"
echo ""
echo " DO NOT FORGET TO RUN 'make distclean && make archives' AFTER EACH NUTTX UPDATE!"
exit 1
fi
else
git submodule update --init --recursive
fi
if [ -d mavlink/include/mavlink/v1.0 ];
then
STATUSRETVAL=$(git submodule summary | grep -A20 -i "mavlink/include/mavlink/v1.0" | grep "<")
if [ -z "$STATUSRETVAL" ]; then
echo "Checked mavlink submodule, correct version found"
else
echo ""
echo ""
echo "New commits required:"
echo "$(git submodule summary)"
echo ""
echo ""
echo "mavlink sub repo not at correct version. Try 'git submodule update'"
echo "or follow instructions on http://pixhawk.org/dev/git/submodules"
exit 1
fi
else
git submodule update --init --recursive
fi
if [ -d uavcan ]
then
STATUSRETVAL=$(git submodule summary | grep -A20 -i uavcan | grep "<")
if [ -z "$STATUSRETVAL" ]
then
echo "Checked uavcan submodule, correct version found"
else
echo ""
echo ""
echo "New commits required:"
echo "$(git submodule summary)"
echo ""
echo ""
echo "uavcan sub repo not at correct version. Try 'git submodule update'"
echo "or follow instructions on http://pixhawk.org/dev/git/submodules"
exit 1
fi
else
git submodule update --init --recursive
fi
if [ -d src/lib/eigen ]
then
echo "ARG = $1"
if [ $1 = "qurt" ]
then
# QuRT needs to use Eigen 3.2 because the toolchain doews not support C++11
STATUSRETVAL=$(true)
else
STATUSRETVAL=$(git submodule summary | grep -A20 -i eigen | grep "<")
if [ -z "$STATUSRETVAL" ]
then
echo "Checked Eigen submodule, correct version found"
else
echo ""
echo ""
echo "New commits required:"
echo "$(git submodule summary)"
echo ""
echo ""
echo "eigen sub repo not at correct version. Try 'git submodule update'"
echo "or follow instructions on http://pixhawk.org/dev/git/submodules"
exit 1
fi
fi
else
git submodule update --init --recursive
fi
if [ -d Tools/gencpp ]
then
STATUSRETVAL=$(git submodule summary | grep -A20 -i gencpp | grep "<")
if [ -z "$STATUSRETVAL" ]
then
echo "Checked gencpp submodule, correct version found"
else
echo ""
echo ""
echo "New commits required:"
echo "$(git submodule summary)"
echo ""
echo ""
echo "gencpp sub repo not at correct version. Try 'git submodule update'"
echo "or follow instructions on http://pixhawk.org/dev/git/submodules"
exit 1
fi
else
git submodule update --init --recursive
fi
if [ -d Tools/genmsg ]
then
STATUSRETVAL=$(git submodule summary | grep -A20 -i genmsg | grep "<")
if [ -z "$STATUSRETVAL" ]
then
echo "Checked genmsg submodule, correct version found"
else
echo ""
echo ""
echo "New commits required:"
echo "$(git submodule summary)"
echo ""
echo ""
echo "genmsg sub repo not at correct version. Try 'git submodule update'"
echo "or follow instructions on http://pixhawk.org/dev/git/submodules"
exit 1
fi
else
git submodule update --init --recursive
fi
exit 0

5
Tools/generate_listener.py
View File

@ -1,6 +1,7 @@
#!/usr/bin/python
import glob
import os
import sys
# This script is run from Build/<target>_default.build/$(PX4_BASE)/Firmware/src/systemcmds/topic_listener
@ -48,7 +49,9 @@ for index,m in enumerate(raw_messages):
temp_list.append(("int8",line.split(' ')[1].split('\t')[0].split('\n')[0]))
f.close()
messages.append(m.split('/')[-1].split('.')[0])
(m_head, m_tail) = os.path.split(m)
messages.append(m_tail.split('.')[0])
#messages.append(m.split('/')[-1].split('.')[0])
message_elements.append(temp_list)
num_messages = len(messages);

5
Tools/px_generate_uorb_topic_headers.py
View File

@ -43,6 +43,11 @@ import shutil
import filecmp
import argparse
import sys
px4_tools_dir = os.path.dirname(os.path.abspath(__file__))
sys.path.append(px4_tools_dir + "/genmsg/src")
sys.path.append(px4_tools_dir + "/gencpp/src")
try:
import genmsg.template_tools
except ImportError as e:

23
cmake/common/px4_base.cmake
View File

@ -136,15 +136,17 @@ function(px4_add_git_submodule)
REQUIRED TARGET PATH
ARGN ${ARGN})
string(REPLACE "/" "_" NAME ${PATH})
add_custom_command(OUTPUT ${CMAKE_BINARY_DIR}/git_${NAME}.stamp
add_custom_command(OUTPUT ${CMAKE_BINARY_DIR}/git_init_${NAME}.stamp
WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
COMMAND git submodule init ${PATH}
COMMAND git submodule update -f ${PATH}
COMMAND touch ${CMAKE_BINARY_DIR}/git_${NAME}.stamp
COMMAND touch ${CMAKE_BINARY_DIR}/git_init_${NAME}.stamp
DEPENDS ${CMAKE_SOURCE_DIR}/.gitmodules
)
add_custom_target(${TARGET}
DEPENDS git_${NAME}.stamp
)
WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
COMMAND git submodule update --recursive ${PATH}
DEPENDS ${CMAKE_BINARY_DIR}/git_init_${NAME}.stamp
)
endfunction()
#=============================================================================
@ -330,7 +332,6 @@ function(px4_generate_messages)
if(NOT VERBOSE)
set(QUIET "-q")
endif()
set(PYTHONPATH "${CMAKE_SOURCE_DIR}/Tools/genmsg/src:${CMAKE_SOURCE_DIR}/Tools/gencpp/src:$ENV{PYTHONPATH}")
set(msg_out_path ${CMAKE_BINARY_DIR}/src/modules/uORB/topics)
set(msg_list)
foreach(msg_file ${MSG_FILES})
@ -342,7 +343,7 @@ function(px4_generate_messages)
list(APPEND msg_files_out ${msg_out_path}/${msg}.h)
endforeach()
add_custom_command(OUTPUT ${msg_files_out}
COMMAND PYTHONPATH=${PYTHONPATH} ${PYTHON_EXECUTABLE}
COMMAND ${PYTHON_EXECUTABLE}
Tools/px_generate_uorb_topic_headers.py
${QUIET}
-d msg
@ -363,7 +364,7 @@ function(px4_generate_messages)
list(APPEND msg_multi_files_out ${msg_multi_out_path}/px4_${msg}.h)
endforeach()
add_custom_command(OUTPUT ${msg_multi_files_out}
COMMAND PYTHONPATH=${PYTHONPATH} ${PYTHON_EXECUTABLE}
COMMAND ${PYTHON_EXECUTABLE}
Tools/px_generate_uorb_topic_headers.py
${QUIET}
-d msg
@ -425,7 +426,7 @@ function(px4_add_upload)
endif()
px4_join(OUT serial_ports LIST "${serial_ports}" GLUE ",")
add_custom_target(${OUT}
COMMAND PYTHONPATH=${PYTHONPATH} ${PYTHON_EXECUTABLE}
COMMAND ${PYTHON_EXECUTABLE}
${CMAKE_SOURCE_DIR}/Tools/px_uploader.py --port ${serial_ports} ${BUNDLE}
DEPENDS ${BUNDLE}
WORKING_DIRECTORY ${CMAKE_BINARY_DIR}
@ -536,6 +537,7 @@ function(px4_add_common_flags)
-ffunction-sections
-fdata-sections
)
if (NOT ${CMAKE_C_COMPILER_ID} STREQUAL "Clang")
list(APPEND optimization_flags
-fno-strength-reduce
@ -566,6 +568,7 @@ function(px4_add_common_flags)
set(cxx_warnings
-Wno-missing-field-initializers
)
set(cxx_compile_flags
-g
-fno-exceptions
@ -578,7 +581,7 @@ function(px4_add_common_flags)
set(visibility_flags
-fvisibility=hidden
"-include ${CMAKE_SOURCE_DIR}/src/include/visibility.h"
-include visibility.h
)
set(added_c_flags

4
cmake/configs/nuttx_px4fmu-v2_default.cmake
View File

@ -168,8 +168,8 @@ set(config_io_board
set(config_extra_libs
${CMAKE_SOURCE_DIR}/src/lib/mathlib/CMSIS/libarm_cortexM4lf_math.a
libuavcan.a
libuavcan_stm32_driver.a
uavcan
uavcan_stm32_driver
)
set(config_io_extra_libs

9
cmake/nuttx/bin_to_obj.py
View File

@ -12,6 +12,7 @@ from subprocess import PIPE
parser = argparse.ArgumentParser(description='Convert bin to obj.')
parser.add_argument('--c_flags', required=True)
parser.add_argument('--c_compiler', required=True)
parser.add_argument('--include_path', required=True)
parser.add_argument('--nm', required=True)
parser.add_argument('--ld', required=True)
parser.add_argument('--objcopy', required=True)
@ -23,6 +24,7 @@ args = parser.parse_args()
in_bin = args.bin
c_flags = args.c_flags
c_compiler = args.c_compiler
include_path = args.include_path
nm = args.nm
ld = args.ld
obj = args.obj
@ -46,7 +48,7 @@ def run_cmd(cmd, d):
return stdout
# do compile
run_cmd("{c_compiler:s} {c_flags:s} -c {obj:s}.c -o {obj:s}.c.o",
run_cmd("{c_compiler:s} -I{include_path:s} {c_flags:s} -c {obj:s}.c -o {obj:s}.c.o",
locals())
# link
@ -55,9 +57,10 @@ run_cmd("{ld:s} -r -o {obj:s}.bin.o {obj:s}.c.o -b binary {in_bin:s}",
# get size of image
stdout = run_cmd("{nm:s} -p --radix=x {obj:s}.bin.o", locals())
re_string = r"^([0-9A-F-a-f]+) .*{sym:s}_size\n".format(**locals())
re_string = r"^([0-9A-Fa-f]+) .*{sym:s}_size".format(**locals())
re_size = re.compile(re_string, re.MULTILINE)
size_match = re.search(re_size, stdout.decode())
try:
size = size_match.group(1)
except AttributeError as e:
@ -76,7 +79,7 @@ with open('{obj:s}.c'.format(**locals()), 'w') as f:
**locals()))
# do compile
run_cmd("{c_compiler:s} {c_flags:s} -c {obj:s}.c -o {obj:s}.c.o",
run_cmd("{c_compiler:s} -I{include_path:s} {c_flags:s} -c {obj:s}.c -o {obj:s}.c.o",
locals())
# link

7
cmake/nuttx/px4_impl_nuttx.cmake
View File

@ -213,7 +213,9 @@ function(px4_nuttx_add_export)
# copy
add_custom_command(OUTPUT nuttx_copy_${CONFIG}.stamp
COMMAND ${MKDIR} -p ${CMAKE_BINARY_DIR}/${CONFIG}
COMMAND rsync -a --exclude=.git ${CMAKE_SOURCE_DIR}/NuttX/ ${nuttx_src}/
COMMAND ${MKDIR} -p ${nuttx_src}
COMMAND ${CP} -a ${CMAKE_SOURCE_DIR}/NuttX/. ${nuttx_src}/
COMMAND ${RM} -rf ${nuttx_src}/.git
COMMAND ${TOUCH} nuttx_copy_${CONFIG}.stamp
DEPENDS ${DEPENDS})
add_custom_target(__nuttx_copy_${CONFIG}
@ -227,7 +229,7 @@ function(px4_nuttx_add_export)
COMMAND ${CP} -r ${CMAKE_SOURCE_DIR}/nuttx-configs/${CONFIG} ${nuttx_src}/nuttx/configs
COMMAND cd ${nuttx_src}/nuttx/tools && ./configure.sh ${CONFIG}/nsh
COMMAND ${ECHO} Exporting NuttX for ${CONFIG}
COMMAND ${MAKE} --no-print-directory --quiet -C ${nuttx_src}/nuttx -j${THREADS} -r CONFIG_ARCH_BOARD=${CONFIG} export
COMMAND ${MAKE} --no-print-directory --quiet -C ${nuttx_src}/nuttx -j${THREADS} -r CONFIG_ARCH_BOARD=${CONFIG} export > /dev/null
COMMAND ${CP} -r ${nuttx_src}/nuttx/nuttx-export.zip ${CMAKE_BINARY_DIR}/${CONFIG}.export
DEPENDS ${config_files} ${DEPENDS} __nuttx_copy_${CONFIG})
@ -347,6 +349,7 @@ function(px4_nuttx_add_romfs)
#COMMAND cmake -E remove_directory ${romfs_temp_dir}
COMMAND ${PYTHON_EXECUTABLE} ${bin_to_obj}
--ld ${LD} --c_flags ${CMAKE_C_FLAGS}
--include_path "${CMAKE_SOURCE_DIR}/src/include"
--c_compiler ${CMAKE_C_COMPILER}
--nm ${NM} --objcopy ${OBJCOPY}
--obj romfs.o

6
nuttx-configs/px4fmu-v2/nsh/defconfig
View File

@ -801,11 +801,11 @@ CONFIG_SCHED_WORKQUEUE=y
CONFIG_SCHED_HPWORK=y
CONFIG_SCHED_WORKPRIORITY=192
CONFIG_SCHED_WORKPERIOD=5000
CONFIG_SCHED_WORKSTACKSIZE=1800
CONFIG_SCHED_WORKSTACKSIZE=1600
CONFIG_SCHED_LPWORK=y
CONFIG_SCHED_LPWORKPRIORITY=50
CONFIG_SCHED_LPWORKPERIOD=50000
CONFIG_SCHED_LPWORKSTACKSIZE=1800
CONFIG_SCHED_LPWORKSTACKSIZE=1600
# CONFIG_LIB_KBDCODEC is not set
# CONFIG_LIB_SLCDCODEC is not set
@ -1064,3 +1064,5 @@ CONFIG_SYSTEM_SYSINFO=y
#
# USB Monitor
#
CONFIG_NSOCKET_DESCRIPTORS=0

5
src/drivers/drv_sensor.h
View File

@ -121,5 +121,10 @@
*/
#define SENSORIOCGROTATION _SENSORIOC(6)
/**
* Test the sensor calibration
*/
#define SENSORIOCCALTEST _SENSORIOC(7)
#endif /* _DRV_SENSOR_H */

1
src/lib/uavcan

@ -1 +0,0 @@
Subproject commit 8effe93d6ee055d9169c1ba460064c90a50eddf1

161
src/modules/commander/PreflightCheck.cpp
View File

@ -71,7 +71,50 @@
namespace Commander
{
static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional)
static int check_calibration(int fd, const char* param_template, int &devid);
int check_calibration(int fd, const char* param_template, int &devid)
{
bool calibration_found;
/* new style: ask device for calibration state */
int ret = px4_ioctl(fd, SENSORIOCCALTEST, 0);
calibration_found = (ret == OK);
devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
char s[20];
int instance = 0;
/* old style transition: check param values */
while (!calibration_found) {
sprintf(s, param_template, instance);
param_t parm = param_find(s);
/* if the calibration param is not present, abort */
if (parm == PARAM_INVALID) {
break;
}
/* if param get succeeds */
int calibration_devid;
if (!param_get(parm, &(calibration_devid))) {
/* if the devid matches, exit early */
if (devid == calibration_devid) {
calibration_found = true;
break;
}
}
instance++;
}
return !calibration_found;
}
static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id)
{
bool success = true;
@ -88,13 +131,9 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional)
return false;
}
int calibration_devid;
int ret;
int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
sprintf(s, "CAL_MAG%u_ID", instance);
param_get(param_find(s), &(calibration_devid));
int ret = check_calibration(fd, "CAL_MAG%u_ID", device_id);
if (devid != calibration_devid) {
if (ret) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: MAG #%u UNCALIBRATED", instance);
success = false;
@ -115,7 +154,7 @@ out:
return success;
}
static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, bool dynamic)
static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, bool dynamic, int &device_id)
{
bool success = true;
@ -132,13 +171,9 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
return false;
}
int calibration_devid;
int ret;
int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
sprintf(s, "CAL_ACC%u_ID", instance);
param_get(param_find(s), &(calibration_devid));
int ret = check_calibration(fd, "CAL_ACC%u_ID", device_id);
if (devid != calibration_devid) {
if (ret) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED", instance);
success = false;
@ -184,7 +219,7 @@ out:
return success;
}
static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional)
static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id)
{
bool success = true;
@ -201,13 +236,9 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional)
return false;
}
int calibration_devid;
int ret;
int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
sprintf(s, "CAL_GYRO%u_ID", instance);
param_get(param_find(s), &(calibration_devid));
int ret = check_calibration(fd, "CAL_GYRO%u_ID", device_id);
if (devid != calibration_devid) {
if (ret) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: GYRO #%u UNCALIBRATED", instance);
success = false;
@ -228,7 +259,7 @@ out:
return success;
}
static bool baroCheck(int mavlink_fd, unsigned instance, bool optional)
static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id)
{
bool success = true;
@ -245,6 +276,20 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional)
return false;
}
device_id = -1000;
// TODO: There is no baro calibration yet, since no external baros exist
// int ret = check_calibration(fd, "CAL_BARO%u_ID");
// if (ret) {
// mavlink_and_console_log_critical(mavlink_fd,
// "PREFLIGHT FAIL: BARO #%u UNCALIBRATED", instance);
// success = false;
// goto out;
// }
//out:
px4_close(fd);
return success;
}
@ -311,49 +356,110 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
/* ---- MAG ---- */
if (checkMag) {
bool prime_found = false;
int32_t prime_id = 0;
param_get(param_find("CAL_MAG_PRIME"), &prime_id);
/* check all sensors, but fail only for mandatory ones */
for (unsigned i = 0; i < max_optional_mag_count; i++) {
bool required = (i < max_mandatory_mag_count);
int device_id = -1;
if (!magnometerCheck(mavlink_fd, i, !required) && required) {
if (!magnometerCheck(mavlink_fd, i, !required, device_id) && required) {
failed = true;
}
if (device_id == prime_id) {
prime_found = true;
}
}
/* check if the primary device is present */
if (!prime_found && prime_id != 0) {
mavlink_log_critical(mavlink_fd, "warning: primary compass not operational");
failed = true;
}
}
/* ---- ACCEL ---- */
if (checkAcc) {
bool prime_found = false;
int32_t prime_id = 0;
param_get(param_find("CAL_ACC_PRIME"), &prime_id);
/* check all sensors, but fail only for mandatory ones */
for (unsigned i = 0; i < max_optional_accel_count; i++) {
bool required = (i < max_mandatory_accel_count);
int device_id = -1;
if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic) && required) {
if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic, device_id) && required) {
failed = true;
}
if (device_id == prime_id) {
prime_found = true;
}
}
/* check if the primary device is present */
if (!prime_found && prime_id != 0) {
mavlink_log_critical(mavlink_fd, "warning: primary accelerometer not operational");
failed = true;
}
}
/* ---- GYRO ---- */
if (checkGyro) {
bool prime_found = false;
int32_t prime_id = 0;
param_get(param_find("CAL_GYRO_PRIME"), &prime_id);
/* check all sensors, but fail only for mandatory ones */
for (unsigned i = 0; i < max_optional_gyro_count; i++) {
bool required = (i < max_mandatory_gyro_count);
int device_id = -1;
if (!gyroCheck(mavlink_fd, i, !required) && required) {
if (!gyroCheck(mavlink_fd, i, !required, device_id) && required) {
failed = true;
}
if (device_id == prime_id) {
prime_found = true;
}
}
/* check if the primary device is present */
if (!prime_found && prime_id != 0) {
mavlink_log_critical(mavlink_fd, "warning: primary gyro not operational");
failed = true;
}
}
/* ---- BARO ---- */
if (checkBaro) {
bool prime_found = false;
int32_t prime_id = 0;
param_get(param_find("CAL_BARO_PRIME"), &prime_id);
/* check all sensors, but fail only for mandatory ones */
for (unsigned i = 0; i < max_optional_baro_count; i++) {
bool required = (i < max_mandatory_baro_count);
int device_id = -1;
if (!baroCheck(mavlink_fd, i, !required) && required) {
if (!baroCheck(mavlink_fd, i, !required, device_id) && required) {
failed = true;
}
if (device_id == prime_id) {
prime_found = true;
}
}
// TODO there is no logic in place to calibrate the primary baro yet
// // check if the primary device is present
if (!prime_found && prime_id != 0) {
mavlink_log_critical(mavlink_fd, "warning: primary barometer not operational");
failed = true;
}
}
@ -367,12 +473,13 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
/* ---- RC CALIBRATION ---- */
if (checkRC) {
if (rc_calibration_check(mavlink_fd) != OK) {
mavlink_log_critical(mavlink_fd, "RC calibration check failed");
failed = true;
}
}
/* ---- Global Navigation Satellite System receiver ---- */
if(checkGNSS) {
if (checkGNSS) {
if(!gnssCheck(mavlink_fd)) {
failed = true;
}

16
src/modules/commander/accelerometer_calibration.cpp
View File

@ -156,6 +156,10 @@ static const int ERROR = -1;
static const char *sensor_name = "accel";
static int32_t device_id[max_accel_sens];
static int device_prio_max = 0;
static int32_t device_id_primary = 0;
calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors);
calibrate_return read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num);
int mat_invert3(float src[3][3], float dst[3][3]);
@ -172,7 +176,6 @@ typedef struct {
int do_accel_calibration(int mavlink_fd)
{
int fd;
int32_t device_id[max_accel_sens];
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
@ -259,6 +262,8 @@ int do_accel_calibration(int mavlink_fd)
bool failed = false;
failed = failed || (OK != param_set_no_notification(param_find("CAL_ACC_PRIME"), &(device_id_primary)));
/* set parameters */
(void)sprintf(str, "CAL_ACC%u_XOFF", i);
failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.x_offset)));
@ -370,6 +375,15 @@ calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel
struct accel_report arp = {};
(void)orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &arp);
timestamps[i] = arp.timestamp;
// Get priority
int32_t prio;
orb_priority(worker_data.subs[i], &prio);
if (prio > device_prio_max) {
device_prio_max = prio;
device_id_primary = device_id[i];
}
}
if (result == calibrate_return_ok) {

15
src/modules/commander/gyro_calibration.cpp
View File

@ -168,6 +168,9 @@ int do_gyro_calibration(int mavlink_fd)
1.0f, // z scale
};
int device_prio_max = 0;
int32_t device_id_primary = 0;
for (unsigned s = 0; s < max_gyros; s++) {
char str[30];
@ -199,6 +202,15 @@ int do_gyro_calibration(int mavlink_fd)
for (unsigned s = 0; s < max_gyros; s++) {
worker_data.gyro_sensor_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s);
// Get priority
int32_t prio;
orb_priority(worker_data.gyro_sensor_sub[s], &prio);
if (prio > device_prio_max) {
device_prio_max = prio;
device_id_primary = worker_data.device_id[s];
}
}
int cancel_sub = calibrate_cancel_subscribe();
@ -258,9 +270,12 @@ int do_gyro_calibration(int mavlink_fd)
}
if (res == OK) {
/* set offset parameters to new values */
bool failed = false;
failed = failed || (OK != param_set_no_notification(param_find("CAL_GYRO_PRIME"), &(device_id_primary)));
for (unsigned s = 0; s < max_gyros; s++) {
if (worker_data.device_id[s] != 0) {
char str[30];

17
src/modules/commander/mag_calibration.cpp
View File

@ -73,6 +73,10 @@ static constexpr unsigned int calibration_sides = 6; ///< The total number of
static constexpr unsigned int calibration_total_points = 240; ///< The total points per magnetometer
static constexpr unsigned int calibraton_duration_seconds = 42; ///< The total duration the routine is allowed to take
int32_t device_ids[max_mags];
int device_prio_max = 0;
int32_t device_id_primary = 0;
calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]);
/// Data passed to calibration worker routine
@ -108,7 +112,6 @@ int do_mag_calibration(int mavlink_fd)
// Determine which mags are available and reset each
int32_t device_ids[max_mags];
char str[30];
for (size_t i=0; i<max_mags; i++) {
@ -434,6 +437,15 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
result = calibrate_return_error;
break;
}
// Get priority
int32_t prio;
orb_priority(worker_data.sub_mag[cur_mag], &prio);
if (prio > device_prio_max) {
device_prio_max = prio;
device_id_primary = device_ids[cur_mag];
}
}
}
}
@ -550,6 +562,9 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
}
if (result == calibrate_return_ok) {
(void)param_set_no_notification(param_find("CAL_MAG_PRIME"), &(device_id_primary));
for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
if (device_ids[cur_mag] != 0) {
int fd_mag = -1;

43
src/modules/controllib/blocks.cpp
View File

@ -39,6 +39,7 @@
#include <math.h>
#include <stdio.h>
#include <float.h>
#include "blocks.hpp"
@ -51,6 +52,7 @@ int basicBlocksTest()
blockLimitSymTest();
blockLowPassTest();
blockHighPassTest();
blockLowPass2Test();
blockIntegralTest();
blockIntegralTrapTest();
blockDerivativeTest();
@ -198,6 +200,47 @@ int blockHighPassTest()
return 0;
}
float BlockLowPass2::update(float input)
{
if (!isfinite(getState())) {
setState(input);
}
if (fabsf(_lp.get_cutoff_freq() - getFCutParam()) > FLT_EPSILON) {
_lp.set_cutoff_frequency(_fs, getFCutParam());
}
_state = _lp.apply(input);
return _state;
}
int blockLowPass2Test()
{
printf("Test BlockLowPass2\t\t: ");
BlockLowPass2 lowPass(NULL, "TEST_LP", 100);
// test initial state
ASSERT(equal(10.0f, lowPass.getFCutParam()));
ASSERT(equal(0.0f, lowPass.getState()));
ASSERT(equal(0.0f, lowPass.getDt()));
// set dt
lowPass.setDt(0.1f);
ASSERT(equal(0.1f, lowPass.getDt()));
// set state
lowPass.setState(1.0f);
ASSERT(equal(1.0f, lowPass.getState()));
// test update
ASSERT(equal(1.06745527f, lowPass.update(2.0f)));
// test end condition
for (int i = 0; i < 100; i++) {
lowPass.update(2.0f);
}
ASSERT(equal(2.0f, lowPass.getState()));
ASSERT(equal(2.0f, lowPass.update(2.0f)));
printf("PASS\n");
return 0;
};
float BlockIntegral::update(float input)
{
// trapezoidal integration

32
src/modules/controllib/blocks.hpp
View File

@ -45,6 +45,7 @@
#include <stdlib.h>
#include <math.h>
#include <mathlib/math/test/test.hpp>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include "block/Block.hpp"
#include "block/BlockParam.hpp"
@ -163,6 +164,36 @@ protected:
int __EXPORT blockHighPassTest();
/**
* A 2nd order low pass filter block which uses the default px4 2nd order low pass filter
*/
class __EXPORT BlockLowPass2 : public Block
{
public:
// methods
BlockLowPass2(SuperBlock *parent, const char *name, float sample_freq) :
Block(parent, name),
_state(0.0 / 0.0 /* initialize to invalid val, force into is_finite() check on first call */),
_fCut(this, ""), // only one parameter, no need to name
_fs(sample_freq),
_lp(_fs, _fCut.get())
{};
virtual ~BlockLowPass2() {};
float update(float input);
// accessors
float getState() { return _state; }
float getFCutParam() { return _fCut.get(); }
void setState(float state) { _state = _lp.reset(state); }
protected:
// attributes
float _state;
control::BlockParamFloat _fCut;
float _fs;
math::LowPassFilter2p _lp;
};
int __EXPORT blockLowPass2Test();
/**
* A rectangular integrator.
* A limiter is built into the class to bound the
@ -263,6 +294,7 @@ public:
void setU(float u) {_u = u;}
float getU() {return _u;}
float getLP() {return _lowPass.getFCut();}
float getO() { return _lowPass.getState(); }
protected:
// attributes
float _u; /**< previous input */

1
src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
View File

@ -177,6 +177,7 @@ private:
hrt_abstime _last_accel;
hrt_abstime _last_mag;
unsigned _prediction_steps;
uint64_t _prediction_last;
struct sensor_combined_s _sensor_combined;

1
src/modules/ekf_att_pos_estimator/CMakeLists.txt
View File

@ -39,6 +39,7 @@ endif()
px4_add_module(
MODULE modules__ekf_att_pos_estimator
MAIN ekf_att_pos_estimator
STACK 3000
COMPILE_FLAGS ${MODULE_CFLAGS}
SRCS
ekf_att_pos_estimator_main.cpp

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

@ -157,6 +157,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
_last_accel(0),
_last_mag(0),
_prediction_steps(0),
_prediction_last(0),
_sensor_combined{},
@ -997,11 +998,12 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const
_covariancePredictionDt += _ekf->dtIMU;
// only fuse every few steps
if (_prediction_steps < MAX_PREDICITION_STEPS) {
if (_prediction_steps < MAX_PREDICITION_STEPS && ((hrt_absolute_time() - _prediction_last) < 20 * 1000)) {
_prediction_steps++;
return;
} else {
_prediction_steps = 0;
_prediction_last = hrt_absolute_time();
}
// perform a covariance prediction if the total delta angle has exceeded the limit
@ -1120,7 +1122,7 @@ int AttitudePositionEstimatorEKF::start()
_estimator_task = px4_task_spawn_cmd("ekf_att_pos_estimator",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 20,
4200,
3900,
(px4_main_t)&AttitudePositionEstimatorEKF::task_main_trampoline,
nullptr);
@ -1161,6 +1163,7 @@ void AttitudePositionEstimatorEKF::print_status()
PX4_INFO("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y,
(double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y,
(double)_ekf->correctedDelAng.z);
PX4_INFO("states (quat) [0-3]: %8.4f, %8.4f, %8.4f, %8.4f", (double)_ekf->states[0], (double)_ekf->states[1],
(double)_ekf->states[2], (double)_ekf->states[3]);
PX4_INFO("states (vel m/s) [4-6]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[4], (double)_ekf->states[5],
@ -1180,11 +1183,12 @@ void AttitudePositionEstimatorEKF::print_status()
PX4_INFO("states (terrain) [22]: %8.4f", (double)_ekf->states[22]);
} else {
PX4_INFO("states (wind) [13-14]: %8.4f, %8.4f", (double)_ekf->states[13], (double)_ekf->states[14]);
PX4_INFO("states (earth mag) [15-17]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[15], (double)_ekf->states[16],
(double)_ekf->states[17]);
PX4_INFO("states (body mag) [18-20]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[18], (double)_ekf->states[19],
(double)_ekf->states[20]);
PX4_INFO("states (accel offs) [13]: %8.4f", (double)_ekf->states[13]);
PX4_INFO("states (wind) [14-15]: %8.4f, %8.4f", (double)_ekf->states[14], (double)_ekf->states[15]);
PX4_INFO("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[16], (double)_ekf->states[17],
(double)_ekf->states[18]);
PX4_INFO("states (mag bias) [19-21]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[19], (double)_ekf->states[20],
(double)_ekf->states[21]);
}
PX4_INFO("states: %s %s %s %s %s %s %s %s %s %s",
@ -1263,7 +1267,7 @@ void AttitudePositionEstimatorEKF::pollData()
} else {
_ekf->dVelIMU.x = 0.5f * (_ekf->accel.x + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0]);
_ekf->dVelIMU.y = 0.5f * (_ekf->accel.y + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 1]);
_ekf->dVelIMU.z = 0.5f * (_ekf->accel.z + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 2]);
_ekf->dVelIMU.z = 0.5f * (_ekf->accel.z + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 2]) + 9.80665f;
}
_ekf->accel.x = _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0];
@ -1331,25 +1335,33 @@ void AttitudePositionEstimatorEKF::pollData()
// leave this in as long as larger improvements are still being made.
#if 0
float deltaTIntegral = (_sensor_combined.gyro_integral_dt) / 1e6f;
float deltaTIntAcc = (_sensor_combined.accelerometer_integral_dt) / 1e6f;
float deltaTIntegral = (_sensor_combined.gyro_integral_dt[0]) / 1e6f;
float deltaTIntAcc = (_sensor_combined.accelerometer_integral_dt[0]) / 1e6f;
static unsigned dtoverflow5 = 0;
static unsigned dtoverflow10 = 0;
static hrt_abstime lastprint = 0;
if (hrt_elapsed_time(&lastprint) > 1000000) {
warnx("dt: %8.6f, dtg: %8.6f, dta: %8.6f, dt > 5 ms: %u, dt > 10 ms: %u",
PX4_WARN("dt: %8.6f, dtg: %8.6f, dta: %8.6f, dt > 5 ms: %u, dt > 10 ms: %u",
(double)deltaT, (double)deltaTIntegral, (double)deltaTIntAcc,
dtoverflow5, dtoverflow10);
warnx("DRV: dang: %8.4f %8.4f dvel: %8.4f %8.4f",
PX4_WARN("EKF: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f",
(double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.z,
(double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.z);
(double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z);
warnx("EKF: dang: %8.4f %8.4f dvel: %8.4f %8.4f",
PX4_WARN("INT: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f",
(double)(_sensor_combined.gyro_integral_rad[0]), (double)(_sensor_combined.gyro_integral_rad[2]),
(double)(_sensor_combined.accelerometer_integral_m_s[0]),
(double)(_sensor_combined.accelerometer_integral_m_s[1]),
(double)(_sensor_combined.accelerometer_integral_m_s[2]));
PX4_WARN("DRV: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f",
(double)(_sensor_combined.gyro_rad_s[0] * deltaT), (double)(_sensor_combined.gyro_rad_s[2] * deltaT),
(double)(_sensor_combined.accelerometer_m_s2[0] * deltaT), (double)(_sensor_combined.accelerometer_m_s2[2] * deltaT));
(double)(_sensor_combined.accelerometer_m_s2[0] * deltaT),
(double)(_sensor_combined.accelerometer_m_s2[1] * deltaT),
(double)((_sensor_combined.accelerometer_m_s2[2] + 9.8665f) * deltaT));
lastprint = hrt_absolute_time();
}
@ -1655,8 +1667,6 @@ int ekf_att_pos_estimator_main(int argc, char *argv[])
}
if (!strcmp(argv[1], "status")) {
PX4_INFO("running");
estimator::g_estimator->print_status();
return 0;

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

@ -81,6 +81,7 @@
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/tecs_status.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/pid/pid.h>

30
src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
View File

@ -52,15 +52,17 @@ mTecs::mTecs() :
_mTecsEnabled(this, "ENABLED"),
_airspeedMin(this, "FW_AIRSPD_MIN", false),
/* Publications */
#if defined(__PX4_NUTTX)
_status(ORB_ID(tecs_status), &getPublications()),
#endif // defined(__PX4_NUTTX)
/* control blocks */
_controlTotalEnergy(this, "THR"),
_controlEnergyDistribution(this, "PIT", true),
_controlAltitude(this, "FPA", true),
_controlAirSpeed(this, "ACC"),
_flightPathAngleLowpass(this, "FPA_LP"),
_altitudeLowpass(this, "ALT_LP"),
_airspeedLowpass(this, "A_LP"),
_flightPathAngleLowpass(this, "FPA_LP", 50),
_altitudeLowpass(this, "ALT_LP", 50),
_airspeedLowpass(this, "A_LP", 50),
_airspeedDerivative(this, "AD"),
_throttleSp(0.0f),
_pitchSp(0.0f),
@ -107,9 +109,11 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti
debug("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, altitude filtered %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)altitudeFiltered, (double)flightPathAngleSp);
}
#if defined(__PX4_NUTTX)
/* Write part of the status message */
_status.altitudeSp = altitudeSp;
_status.altitude_filtered = altitudeFiltered;
#endif // defined(__PX4_NUTTX)
/* use flightpath angle setpoint for total energy control */
@ -143,9 +147,11 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng
(double)airspeedFiltered, (double)accelerationLongitudinalSp);
}
#if defined(__PX4_NUTTX)
/* Write part of the status message */
_status.airspeedSp = airspeedSp;
_status.airspeed_filtered = airspeedFiltered;
#endif // defined(__PX4_NUTTX)
/* use longitudinal acceleration setpoint for total energy control */
@ -200,23 +206,23 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
}
/* Check airspeed: if below safe value switch to underspeed mode (if not in land or takeoff mode) */
if (mode != tecs_status_s::TECS_MODE_LAND && mode != tecs_status_s::TECS_MODE_TAKEOFF && airspeedFiltered < _airspeedMin.get()) {
mode = tecs_status_s::TECS_MODE_UNDERSPEED;
if (mode != MTECS_MODE_LAND && mode != MTECS_MODE_TAKEOFF && airspeedFiltered < _airspeedMin.get()) {
mode = MTECS_MODE_UNDERSPEED;
}
/* Set special output limiters if we are not in TECS_MODE_NORMAL */
/* Set special output limiters if we are not in MTECS_MODE_NORMAL */
BlockOutputLimiter *outputLimiterThrottle = &_controlTotalEnergy.getOutputLimiter();
BlockOutputLimiter *outputLimiterPitch = &_controlEnergyDistribution.getOutputLimiter();
if (mode == tecs_status_s::TECS_MODE_TAKEOFF) {
if (mode == MTECS_MODE_TAKEOFF) {
outputLimiterThrottle = &_BlockOutputLimiterTakeoffThrottle;
outputLimiterPitch = &_BlockOutputLimiterTakeoffPitch;
} else if (mode == tecs_status_s::TECS_MODE_LAND) {
} else if (mode == MTECS_MODE_LAND) {
// only limit pitch but do not limit throttle
outputLimiterPitch = &_BlockOutputLimiterLandPitch;
} else if (mode == tecs_status_s::TECS_MODE_LAND_THROTTLELIM) {
} else if (mode == MTECS_MODE_LAND_THROTTLELIM) {
outputLimiterThrottle = &_BlockOutputLimiterLandThrottle;
outputLimiterPitch = &_BlockOutputLimiterLandPitch;
} else if (mode == tecs_status_s::TECS_MODE_UNDERSPEED) {
} else if (mode == MTECS_MODE_UNDERSPEED) {
outputLimiterThrottle = &_BlockOutputLimiterUnderspeedThrottle;
outputLimiterPitch = &_BlockOutputLimiterUnderspeedPitch;
}
@ -226,6 +232,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
* is running) */
limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch);
// #if defined(__PX4_NUTTX)
// /* Write part of the status message */
// _status.flightPathAngleSp = flightPathAngleSp;
// _status.flightPathAngle = flightPathAngle;
@ -237,6 +244,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
// _status.energyDistributionRateSp = energyDistributionRateSp;
// _status.energyDistributionRate = energyDistributionRate;
// _status.mode = mode;
// #endif // defined(__PX4_NUTTX)
/** update control blocks **/
/* update total energy rate control block */
@ -253,8 +261,10 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
(double)accelerationLongitudinalSp, (double)airspeedDerivative);
}
#if defined(__PX4_NUTTX)
/* publish status message */
_status.update();
#endif // defined(__PX4_NUTTX)
/* clean up */
_firstIterationAfterReset = false;

26
src/modules/fw_pos_control_l1/mtecs/mTecs.h
View File

@ -49,11 +49,25 @@
#include <controllib/block/BlockParam.hpp>
#include <drivers/drv_hrt.h>
#include <uORB/Publication.hpp>
#if defined(__PX4_NUTTX)
#include <uORB/topics/tecs_status.h>
#endif // defined(__PX4_NUTTX)
namespace fwPosctrl
{
/* corresponds to TECS_MODE in tecs_status.msg */
enum MTECS_MODE {
MTECS_MODE_NORMAL = 0,
MTECS_MODE_UNDERSPEED = 1,
MTECS_MODE_TAKEOFF = 2,
MTECS_MODE_LAND = 3,
MTECS_MODE_LAND_THROTTLELIM = 4,
MTECS_MODE_BAD_DESCENT = 5,
MTECS_MODE_CLIMBOUT = 6
};
/* Main class of the mTecs */
class mTecs : public control::SuperBlock
{
@ -94,6 +108,10 @@ public:
float getThrottleSetpoint() { return _throttleSp; }
float getPitchSetpoint() { return _pitchSp; }
float airspeedLowpassUpdate(float input) { return _airspeedLowpass.update(input); }
float getFlightPathAngleLowpassState() { return _flightPathAngleLowpass.getState(); }
float getAltitudeLowpassState() { return _altitudeLowpass.getState(); }
float getAirspeedLowpassState() { return _airspeedLowpass.getState(); }
float getAirspeedDerivativeLowpassState() { return _airspeedDerivative.getO(); }
protected:
/* parameters */
@ -101,7 +119,9 @@ protected:
control::BlockParamFloat _airspeedMin; /**< minimal airspeed */
/* Publications */
#if defined(__PX4_NUTTX)
uORB::Publication<tecs_status_s> _status; /**< publish internal values for logging */
#endif // defined(__PX4_NUTTX)
/* control blocks */
BlockFFPILimitedCustom _controlTotalEnergy; /**< FFPI controller for total energy control: output
@ -114,9 +134,9 @@ protected:
setpoint */
/* Other calculation Blocks */
control::BlockLowPass _flightPathAngleLowpass; /**< low pass filter for the flight path angle */
control::BlockLowPass _altitudeLowpass; /**< low pass filter for altitude */
control::BlockLowPass _airspeedLowpass; /**< low pass filter for airspeed */
control::BlockLowPass2 _flightPathAngleLowpass; /**< low pass filter for the flight path angle */
control::BlockLowPass2 _altitudeLowpass; /**< low pass filter for altitude */
control::BlockLowPass2 _airspeedLowpass; /**< low pass filter for airspeed */
control::BlockDerivative _airspeedDerivative; /**< airspeed derivative calulation */
/* Output setpoints */

9
src/modules/mavlink/mavlink_mission.cpp
View File

@ -779,13 +779,12 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
mission_item->altitude_is_relative = true;
break;
case MAV_FRAME_LOCAL_NED:
case MAV_FRAME_LOCAL_ENU:
return MAV_MISSION_UNSUPPORTED_FRAME;
case MAV_FRAME_MISSION:
// This is a mission item with no coordinate
break;
default:
return MAV_MISSION_ERROR;
return MAV_MISSION_UNSUPPORTED_FRAME;
}
switch (mavlink_mission_item->command) {

29
src/modules/mavlink/mavlink_parameters.cpp
View File

@ -49,6 +49,7 @@
ORB_DEFINE(uavcan_parameter_request, struct uavcan_parameter_request_s);
ORB_DEFINE(uavcan_parameter_value, struct uavcan_parameter_value_s);
#define HASH_PARAM "_HASH_CHECK"
MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkStream(mavlink),
_send_all_index(-1),
@ -163,13 +164,27 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
/* when no index is given, loop through string ids and compare them */
if (req_read.param_index < 0) {
/* local name buffer to enforce null-terminated string */
char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
strncpy(name, req_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
/* enforce null termination */
name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
/* attempt to find parameter and send it */
send_param(param_find_no_notification(name));
if (strncmp(req_read.param_id, HASH_PARAM, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN) == 0) {
/* return hash check for cached params */
uint32_t hash = param_hash_check();
/* build the one-off response message */
mavlink_param_value_t msg;
msg.param_count = param_count_used();
msg.param_index = -1;
strncpy(msg.param_id, HASH_PARAM, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
msg.param_type = MAV_PARAM_TYPE_UINT32;
memcpy(&msg.param_value, &hash, sizeof(hash));
_mavlink->send_message(MAVLINK_MSG_ID_PARAM_VALUE, &msg);
} else {
/* local name buffer to enforce null-terminated string */
char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
strncpy(name, req_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
/* enforce null termination */
name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
/* attempt to find parameter and send it */
send_param(param_find_no_notification(name));
}
} else {
/* when index is >= 0, send this parameter again */

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

@ -336,7 +336,7 @@ MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionI
} else {
/* item is too far from home */
mavlink_log_critical(_mavlink_fd, "Waypoint too far: %d m,[MIS_DIST_1WP=%d]", (int)dist_to_1wp, (int)dist_first_wp);
mavlink_log_critical(_mavlink_fd, "First waypoint too far: %d m,refusing mission", (int)dist_to_1wp, (int)dist_first_wp);
warning_issued = true;
return false;
}

14
src/modules/sdlog2/logbuffer.c
View File

@ -50,8 +50,8 @@ int logbuffer_init(struct logbuffer_s *lb, int size)
lb->size = size;
lb->write_ptr = 0;
lb->read_ptr = 0;
lb->data = malloc(lb->size);
return (lb->data == 0) ? PX4_ERROR : PX4_OK;
lb->data = NULL;
return PX4_OK;
}
int logbuffer_count(struct logbuffer_s *lb)
@ -72,6 +72,16 @@ int logbuffer_is_empty(struct logbuffer_s *lb)
bool logbuffer_write(struct logbuffer_s *lb, void *ptr, int size)
{
// allocate buffer if not yet present
if (lb->data == NULL) {
lb->data = malloc(lb->size);
}
// allocation failed, bail out
if (lb->data == NULL) {
return false;
}
// bytes available to write
int available = lb->read_ptr - lb->write_ptr - 1;

1
src/modules/sensors/CMakeLists.txt
View File

@ -40,7 +40,6 @@ px4_add_module(
-O3
SRCS
sensors.cpp
sensor_params.c
DEPENDS
platforms__common
)

28
src/modules/sensors/sensor_params.c
View File

@ -585,6 +585,34 @@ PARAM_DEFINE_FLOAT(CAL_ACC2_YSCALE, 1.0f);
*/
PARAM_DEFINE_FLOAT(CAL_ACC2_ZSCALE, 1.0f);
/**
* Primary accel ID
*
* @group Sensor Calibration
*/
PARAM_DEFINE_INT32(CAL_ACC_PRIME, 0);
/**
* Primary gyro ID
*
* @group Sensor Calibration
*/
PARAM_DEFINE_INT32(CAL_GYRO_PRIME, 0);
/**
* Primary mag ID
*
* @group Sensor Calibration
*/
PARAM_DEFINE_INT32(CAL_MAG_PRIME, 0);
/**
* Primary baro ID
*
* @group Sensor Calibration
*/
PARAM_DEFINE_INT32(CAL_BARO_PRIME, 0);
/**
* Differential pressure sensor offset
*

16
src/modules/sensors/sensors.cpp
View File

@ -1412,6 +1412,8 @@ Sensors::parameter_update_poll(bool forced)
(void)sprintf(str, "CAL_MAG%u_ID", i);
int device_id;
failed = failed || (OK != param_get(param_find(str), &device_id));
(void)sprintf(str, "CAL_MAG%u_ROT", i);
(void)param_find(str);
if (failed) {
px4_close(fd);
@ -2020,6 +2022,11 @@ Sensors::task_main()
* do subscriptions
*/
int gcount_prev = _gyro_count;
int mcount_prev = _mag_count;
int acount_prev = _accel_count;
int bcount_prev = _baro_count;
_gyro_count = init_sensor_class(ORB_ID(sensor_gyro), &_gyro_sub[0],
&raw.gyro_priority[0], &raw.gyro_errcount[0]);
@ -2032,6 +2039,15 @@ Sensors::task_main()
_baro_count = init_sensor_class(ORB_ID(sensor_baro), &_baro_sub[0],
&raw.baro_priority[0], &raw.baro_errcount[0]);
if (gcount_prev != _gyro_count ||
mcount_prev != _mag_count ||
acount_prev != _accel_count ||
bcount_prev != _baro_count) {
/* reload calibration params */
parameter_update_poll(true);
}
_rc_sub = orb_subscribe(ORB_ID(input_rc));
_diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));

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

@ -65,6 +65,8 @@
#include "uORB/topics/parameter_update.h"
#include "px4_parameters.h"
#include <crc32.h>
#if 0
# define debug(fmt, args...) do { warnx(fmt, ##args); } while(0)
#else
@ -1035,3 +1037,26 @@ param_foreach(void (*func)(void *arg, param_t param), void *arg, bool only_chang
func(arg, param);
}
}
uint32_t param_hash_check(void)
{
uint32_t param_hash = 0;
param_lock();
/* compute the CRC32 over all string param names and 4 byte values */
for (param_t param = 0; handle_in_range(param); param++) {
if (!param_used(param)) {
continue;
}
const char *name = param_name(param);
const void *val = param_get_value_ptr(param);
param_hash = crc32part((const uint8_t *)name, strlen(name), param_hash);
param_hash = crc32part(val, sizeof(union param_value_u), param_hash);
}
param_unlock();
return param_hash;
}

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

@ -333,6 +333,13 @@ __EXPORT int param_save_default(void);
*/
__EXPORT int param_load_default(void);
/**
* Generate the hash of all parameters and their values
*
* @return CRC32 hash of all param_ids and values
*/
__EXPORT uint32_t param_hash_check(void);
/*
* Macros creating static parameter definitions.
*

78
src/modules/uavcan/CMakeLists.txt
View File

@ -31,65 +31,30 @@
#
############################################################################
# uavcan project
set(uavcan_c_flags ${c_flags})
list(REMOVE_ITEM uavcan_c_flags -std=gnu++0x -D__CUSTOM_FILE_IO__)
set(uavcan_cxx_flags ${cxx_flags})
list(REMOVE_ITEM uavcan_cxx_flags -std=gnu++0x -std=c++11 -Wundef -Werror -D__CUSTOM_FILE_IO__)
set(uavcan_deps git_uavcan)
set(uavcan_platform generic)
set(nuttx_export_dir ${CMAKE_BINARY_DIR}/${BOARD}/NuttX/nuttx-export)
set(UAVCAN_USE_CPP03 ON CACHE BOOL "uavcan cpp03")
set(UAVCAN_PLATFORM stm32 CACHE STRING "uavcan platform")
string(TOUPPER "${OS}" OS_UPPER)
set(uavcan_definitions
-DUAVCAN_NO_ASSERTIONS
-DUAVCAN_STM32_NUM_IFACES=2
-DUAVCAN_USE_EXTERNAL_SNPRINT
-DUAVCAN_MEM_POOL_BLOCK_SIZE=48
-DUAVCAN_MAX_NETWORK_SIZE_HINT=16
-DUAVCAN_STM32_TIMER_NUMBER=5
-DUAVCAN_STM32_${OS_UPPER}=1
-DUAVCAN_CPP_VERSION=UAVCAN_CPP03
)
set(uavcan_extra_flags ${uavcan_definitions})
if (${OS} STREQUAL "nuttx")
set(uavcan_platform stm32)
list(APPEND uavcan_extra_flags
-I${nuttx_export_dir}/include
-I${nuttx_export_dir}/include/cxx
-I${nuttx_export_dir}/arch/chip
-I${nuttx_export_dir}/arch/common
)
list(APPEND uavcan_deps nuttx_export_${BOARD})
endif()
list(APPEND uavcan_c_flags ${uavcan_extra_flags})
list(APPEND uavcan_cxx_flags ${uavcan_extra_flags})
px4_join(OUT uavcan_c_flags LIST "${uavcan_c_flags}" GLUE " ")
px4_join(OUT uavcan_cxx_flags LIST "${uavcan_cxx_flags}" GLUE " ")
externalproject_add(libuavcan
DEPENDS ${uavcan_deps}
DOWNLOAD_COMMAND ""
UPDATE_COMMAND git submodule update --init
LOG_INSTALL 1
SOURCE_DIR ${CMAKE_SOURCE_DIR}/src/lib/uavcan
CMAKE_ARGS -DCMAKE_TOOLCHAIN_FILE=${CMAKE_TOOLCHAIN_FILE}
add_definitions(
-DUAVCAN_CPP_VERSION=UAVCAN_CPP03
-DUAVCAN_MAX_NETWORK_SIZE_HINT=16
-DUAVCAN_MEM_POOL_BLOCK_SIZE=48
-DUAVCAN_NO_ASSERTIONS
-DUAVCAN_PLATFORM=stm32
-DUAVCAN_STM32_${OS_UPPER}=1
-DUAVCAN_STM32_NUM_IFACES=2
-DUAVCAN_STM32_TIMER_NUMBER=5
-DUAVCAN_USE_CPP03=ON
-DUAVCAN_PLATFORM=${uavcan_platform}
-DUAVCAN_OS=${OS}
-DCMAKE_CXX_FLAGS=${uavcan_cxx_flags}
-DCMAKE_C_FLAGS=${uavcan_c_flags}
-DCMAKE_INSTALL_PREFIX=${ep_base}/Install
)
-DUAVCAN_USE_EXTERNAL_SNPRINT
)
string(TOUPPER ${OS} OS_UPPER)
add_subdirectory(libuavcan EXCLUDE_FROM_ALL)
add_dependencies(uavcan platforms__nuttx)
add_definitions(${uavcan_definitions})
include_directories(libuavcan/libuavcan/include)
include_directories(libuavcan/libuavcan/include/dsdlc_generated)
include_directories(libuavcan/libuavcan_drivers/posix/include)
include_directories(libuavcan/libuavcan_drivers/stm32/driver/include)
px4_add_module(
MODULE modules__uavcan
@ -98,9 +63,8 @@ px4_add_module(
COMPILE_FLAGS
-Wframe-larger-than=1500
-Wno-deprecated-declarations
-O3
-Os
SRCS
# Main
uavcan_main.cpp
uavcan_servers.cpp
@ -117,7 +81,7 @@ px4_add_module(
DEPENDS
platforms__common
libuavcan
uavcan
)
## vim: set noet ft=cmake fenc=utf-8 ff=unix :

1
src/modules/uavcan/libuavcan Submodule

@ -0,0 +1 @@
Subproject commit 0643879922239930cf7482777356f06891c26616

2
src/modules/uavcan/uavcan_main.hpp
View File

@ -94,7 +94,7 @@ class UavcanNode : public device::CDev
*/
static constexpr unsigned RxQueueLenPerIface = FramePerMSecond * PollTimeoutMs; // At
static constexpr unsigned StackSize = 1600;
static constexpr unsigned StackSize = 1800;
public:
typedef uavcan::Node<MemPoolSize> Node;

12
src/platforms/px4_log.h
View File

@ -130,7 +130,7 @@ __EXPORT extern int __px4_log_level_current;
__END_DECLS
// __px4_log_level_current will be initialized to PX4_LOG_LEVEL_AT_RUN_TIME
#define PX4_LOG_LEVEL_AT_RUN_TIME _PX4_LOG_LEVEL_WARN
#define PX4_LOG_LEVEL_AT_RUN_TIME _PX4_LOG_LEVEL_ERROR
/****************************************************************************
* Implementation of log section formatting based on printf
@ -357,8 +357,8 @@ __END_DECLS
/****************************************************************************
* Non-verbose settings for a Release build to minimize strings in build
****************************************************************************/
#define PX4_PANIC(FMT, ...) __px4_log_file_and_line(_PX4_LOG_LEVEL_PANIC, FMT, ##__VA_ARGS__)
#define PX4_ERR(FMT, ...) __px4_log_file_and_line(_PX4_LOG_LEVEL_ERROR, FMT, ##__VA_ARGS__)
#define PX4_PANIC(FMT, ...) __px4_log(_PX4_LOG_LEVEL_PANIC, FMT, ##__VA_ARGS__)
#define PX4_ERR(FMT, ...) __px4_log(_PX4_LOG_LEVEL_ERROR, FMT, ##__VA_ARGS__)
#define PX4_WARN(FMT, ...) __px4_log_omit(_PX4_LOG_LEVEL_WARN, FMT, ##__VA_ARGS__)
#define PX4_DEBUG(FMT, ...) __px4_log_omit(_PX4_LOG_LEVEL_DEBUG, FMT, ##__VA_ARGS__)
@ -366,9 +366,9 @@ __END_DECLS
/****************************************************************************
* Medium verbose settings for a default build
****************************************************************************/
#define PX4_PANIC(FMT, ...) __px4_log_file_and_line(_PX4_LOG_LEVEL_PANIC, FMT, ##__VA_ARGS__)
#define PX4_ERR(FMT, ...) __px4_log_file_and_line(_PX4_LOG_LEVEL_ERROR, FMT, ##__VA_ARGS__)
#define PX4_WARN(FMT, ...) __px4_log_file_and_line(_PX4_LOG_LEVEL_WARN, FMT, ##__VA_ARGS__)
#define PX4_PANIC(FMT, ...) __px4_log(_PX4_LOG_LEVEL_PANIC, FMT, ##__VA_ARGS__)
#define PX4_ERR(FMT, ...) __px4_log(_PX4_LOG_LEVEL_ERROR, FMT, ##__VA_ARGS__)
#define PX4_WARN(FMT, ...) __px4_log(_PX4_LOG_LEVEL_WARN, FMT, ##__VA_ARGS__)
#define PX4_DEBUG(FMT, ...) __px4_log_omit(_PX4_LOG_LEVEL_DEBUG, FMT, ##__VA_ARGS__)
#endif