Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 19:07:45 +08:00
Code Issues Actions 5 Packages Projects Releases Wiki Activity

Merge branch 'master' of github.com:wingtra/Firmware into control_state

Browse Source
This commit is contained in:
Youssef Demitri
2015-10-16 15:50:29 +02:00
parent 36bfa58419 d6a2c8eaa1
commit 069c6a82cb
48 changed files with 1394 additions and 384 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Makefile
+6 -17
View File
@@ -89,7 +89,7 @@ endif
# -------------------------------------------------------------------- # --------------------------------------------------------------------
# describe how to build a cmake config # describe how to build a cmake config
define cmake-build 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) +$(PX4_MAKE) -C $(PWD)/build_$@ $(PX4_MAKE_ARGS) $(ARGS)
endef endef
@@ -104,13 +104,13 @@ endef
# -------------------------------------------------------------------- # --------------------------------------------------------------------
# Do not put any spaces between function arguments. # Do not put any spaces between function arguments.
px4fmu-v1_default: git-init px4fmu-v1_default:
$(call cmake-build,nuttx_px4fmu-v1_default) $(call cmake-build,nuttx_px4fmu-v1_default)
px4fmu-v2_default: git-init px4fmu-v2_default:
$(call cmake-build,nuttx_px4fmu-v2_default) $(call cmake-build,nuttx_px4fmu-v2_default)
px4fmu-v2_simple: git-init px4fmu-v2_simple:
$(call cmake-build,nuttx_px4fmu-v2_simple) $(call cmake-build,nuttx_px4fmu-v2_simple)
nuttx_sim_simple: nuttx_sim_simple:
@@ -172,19 +172,8 @@ check_format:
clean: clean:
@rm -rf build_*/ @rm -rf build_*/
@(cd NuttX && git clean -d -f -x)
distclean: clean @(cd src/modules/uavcan/libuavcan && git clean -d -f -x)
@cd NuttX
@git clean -d -f -x
@cd ..
@cd src/modules/uavcan/libuavcan
@git clean -d -f -x
@cd ../../../..
# XXX this is not the right way to fix it, but we need a temporary solution
# for average joe
git-init:
@git submodule update --init --recursive
# targets handled by cmake # 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 cmake_targets = test upload package package_source debug debug_tui debug_ddd debug_io debug_io_tui debug_io_ddd check_weak libuavcan
Tools/check_submodules.sh
-144
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
Tools/generate_listener.py
+20 -3
View File
@@ -24,11 +24,16 @@ for index,m in enumerate(raw_messages):
if ('float32[' in line.split(' ')[0]): if ('float32[' in line.split(' ')[0]):
num_floats = int(line.split(" ")[0].split("[")[1].split("]")[0]) num_floats = int(line.split(" ")[0].split("[")[1].split("]")[0])
temp_list.append(("float_array",line.split(' ')[1].split('\t')[0].split('\n')[0],num_floats)) temp_list.append(("float_array",line.split(' ')[1].split('\t')[0].split('\n')[0],num_floats))
if ('uint64[' in line.split(' ')[0]): elif ('float64[' in line.split(' ')[0]):
num_floats = int(line.split(" ")[0].split("[")[1].split("]")[0])
temp_list.append(("double_array",line.split(' ')[1].split('\t')[0].split('\n')[0],num_floats))
elif ('uint64[' in line.split(' ')[0]):
num_floats = int(line.split(" ")[0].split("[")[1].split("]")[0]) num_floats = int(line.split(" ")[0].split("[")[1].split("]")[0])
temp_list.append(("uint64_array",line.split(' ')[1].split('\t')[0].split('\n')[0],num_floats)) temp_list.append(("uint64_array",line.split(' ')[1].split('\t')[0].split('\n')[0],num_floats))
elif(line.split(' ')[0] == "float32"): elif(line.split(' ')[0] == "float32"):
temp_list.append(("float",line.split(' ')[1].split('\t')[0].split('\n')[0])) temp_list.append(("float",line.split(' ')[1].split('\t')[0].split('\n')[0]))
elif(line.split(' ')[0] == "float64"):
temp_list.append(("double",line.split(' ')[1].split('\t')[0].split('\n')[0]))
elif(line.split(' ')[0] == "uint64") and len(line.split('=')) == 1: elif(line.split(' ')[0] == "uint64") and len(line.split('=')) == 1:
temp_list.append(("uint64",line.split(' ')[1].split('\t')[0].split('\n')[0])) temp_list.append(("uint64",line.split(' ')[1].split('\t')[0].split('\n')[0]))
elif(line.split(' ')[0] == "uint32") and len(line.split('=')) == 1: elif(line.split(' ')[0] == "uint32") and len(line.split('=')) == 1:
@@ -114,6 +119,10 @@ print("""
#define PRIu64 "llu" #define PRIu64 "llu"
#endif #endif
#ifndef PRI64
#define PRI64 "lld"
#endif
""") """)
for m in messages: for m in messages:
print("#include <uORB/topics/%s.h>" % m) print("#include <uORB/topics/%s.h>" % m)
@@ -151,11 +160,19 @@ for index,m in enumerate(messages[1:]):
print("\t\t\torb_copy(ID,sub,&container);") print("\t\t\torb_copy(ID,sub,&container);")
for item in message_elements[index+1]: for item in message_elements[index+1]:
if item[0] == "float": if item[0] == "float":
print("\t\t\tprintf(\"%s: %%f\\n\",(double)container.%s);" % (item[1], item[1])) print("\t\t\tprintf(\"%s: %%8.4f\\n\",(double)container.%s);" % (item[1], item[1]))
elif item[0] == "float_array": elif item[0] == "float_array":
print("\t\t\tprintf(\"%s: \");" % item[1]) print("\t\t\tprintf(\"%s: \");" % item[1])
print("\t\t\tfor (int j = 0; j < %d; j++) {" % item[2]) print("\t\t\tfor (int j = 0; j < %d; j++) {" % item[2])
print("\t\t\t\tprintf(\"%%f \",(double)container.%s[j]);" % item[1]) print("\t\t\t\tprintf(\"%%8.4f \",(double)container.%s[j]);" % item[1])
print("\t\t\t}")
print("\t\t\tprintf(\"\\n\");")
elif item[0] == "double":
print("\t\t\tprintf(\"%s: %%8.4f\\n\",(double)container.%s);" % (item[1], item[1]))
elif item[0] == "double_array":
print("\t\t\tprintf(\"%s: \");" % item[1])
print("\t\t\tfor (int j = 0; j < %d; j++) {" % item[2])
print("\t\t\t\tprintf(\"%%8.4f \",(double)container.%s[j]);" % item[1])
print("\t\t\t}") print("\t\t\t}")
print("\t\t\tprintf(\"\\n\");") print("\t\t\tprintf(\"\\n\");")
elif item[0] == "uint64": elif item[0] == "uint64":
Tools/px4airframes/rcout.py
+2 -1
View File
@@ -1,5 +1,6 @@
from xml.sax.saxutils import escape from xml.sax.saxutils import escape
import codecs import codecs
import os
class RCOutput(): class RCOutput():
def __init__(self, groups, board): def __init__(self, groups, board):
@@ -30,7 +31,7 @@ class RCOutput():
for group in groups: for group in groups:
result += "# GROUP: %s\n\n" % group.GetName() result += "# GROUP: %s\n\n" % group.GetName()
for param in group.GetParams(): for param in group.GetParams():
path = param.GetPath().rsplit('/', 1)[1] path = os.path.split(param.GetPath())[1]
id_val = param.GetId() id_val = param.GetId()
name = param.GetFieldValue("short_desc") name = param.GetFieldValue("short_desc")
long_desc = param.GetFieldValue("long_desc") long_desc = param.GetFieldValue("long_desc")
Tools/px4airframes/srcparser.py
+2 -1
View File
@@ -1,5 +1,6 @@
import sys import sys
import re import re
import os
class ParameterGroup(object): class ParameterGroup(object):
""" """
@@ -160,7 +161,7 @@ class SourceParser(object):
""" """
airframe_id = None airframe_id = None
airframe_id = path.rsplit('/',1)[1].split('_',1)[0] airframe_id = os.path.split(path)[1].split('_',1)[0]
# Skip if not numeric # Skip if not numeric
if (not self.IsNumber(airframe_id)): if (not self.IsNumber(airframe_id)):
cmake/common/px4_base.cmake
+11 -6
View File
@@ -136,14 +136,17 @@ function(px4_add_git_submodule)
REQUIRED TARGET PATH REQUIRED TARGET PATH
ARGN ${ARGN}) ARGN ${ARGN})
string(REPLACE "/" "_" NAME ${PATH}) 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} WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
COMMAND git submodule update --init --recursive -f ${PATH} COMMAND git submodule init ${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} 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() endfunction()
#============================================================================= #=============================================================================
@@ -418,7 +421,8 @@ function(px4_add_upload)
) )
elseif(${CMAKE_HOST_SYSTEM_NAME} STREQUAL "Windows") elseif(${CMAKE_HOST_SYSTEM_NAME} STREQUAL "Windows")
foreach(port RANGE 32 0) foreach(port RANGE 32 0)
list(APPEND "COM${port}") list(APPEND serial_ports
"COM${port}")
endforeach() endforeach()
endif() endif()
px4_join(OUT serial_ports LIST "${serial_ports}" GLUE ",") px4_join(OUT serial_ports LIST "${serial_ports}" GLUE ",")
@@ -543,6 +547,7 @@ function(px4_add_common_flags)
endif() endif()
set(c_warnings set(c_warnings
-Wbad-function-cast
-Wstrict-prototypes -Wstrict-prototypes
-Wmissing-prototypes -Wmissing-prototypes
-Wnested-externs -Wnested-externs
cmake/nuttx/px4_impl_nuttx.cmake
+3 -3
View File
@@ -213,8 +213,8 @@ function(px4_nuttx_add_export)
# copy # copy
add_custom_command(OUTPUT nuttx_copy_${CONFIG}.stamp add_custom_command(OUTPUT nuttx_copy_${CONFIG}.stamp
COMMAND ${MKDIR} -p ${CMAKE_BINARY_DIR}/${CONFIG} COMMAND ${MKDIR} -p ${CMAKE_BINARY_DIR}/${CONFIG}
COMMAND ${RM} -rf ${nuttx_src} COMMAND ${MKDIR} -p ${nuttx_src}
COMMAND ${CP} -r ${CMAKE_SOURCE_DIR}/NuttX ${nuttx_src} COMMAND ${CP} -a ${CMAKE_SOURCE_DIR}/NuttX/. ${nuttx_src}/
COMMAND ${RM} -rf ${nuttx_src}/.git COMMAND ${RM} -rf ${nuttx_src}/.git
COMMAND ${TOUCH} nuttx_copy_${CONFIG}.stamp COMMAND ${TOUCH} nuttx_copy_${CONFIG}.stamp
DEPENDS ${DEPENDS}) DEPENDS ${DEPENDS})
@@ -229,7 +229,7 @@ function(px4_nuttx_add_export)
COMMAND ${CP} -r ${CMAKE_SOURCE_DIR}/nuttx-configs/${CONFIG} ${nuttx_src}/nuttx/configs COMMAND ${CP} -r ${CMAKE_SOURCE_DIR}/nuttx-configs/${CONFIG} ${nuttx_src}/nuttx/configs
COMMAND cd ${nuttx_src}/nuttx/tools && ./configure.sh ${CONFIG}/nsh COMMAND cd ${nuttx_src}/nuttx/tools && ./configure.sh ${CONFIG}/nsh
COMMAND ${ECHO} Exporting NuttX for ${CONFIG} 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 COMMAND ${CP} -r ${nuttx_src}/nuttx/nuttx-export.zip ${CMAKE_BINARY_DIR}/${CONFIG}.export
DEPENDS ${config_files} ${DEPENDS} __nuttx_copy_${CONFIG}) DEPENDS ${config_files} ${DEPENDS} __nuttx_copy_${CONFIG})
msg/uavcan_parameter_request.msg
+8
View File
@@ -0,0 +1,8 @@
# UAVCAN-MAVLink parameter bridge request type
uint8 message_type # MAVLink message type: PARAM_REQUEST_READ, PARAM_REQUEST_LIST, PARAM_SET
uint8 node_id # UAVCAN node ID mapped from MAVLink component ID
char[17] param_id # MAVLink/UAVCAN parameter name
int16 param_index # -1 if the param_id field should be used as identifier
uint8 param_type # MAVLink parameter type
int64 int_value # current value if param_type is int-like
float32 real_value # current value if param_type is float-like
msg/uavcan_parameter_value.msg
+8
View File
@@ -0,0 +1,8 @@
# UAVCAN-MAVLink parameter bridge response type
uint8 node_id # UAVCAN node ID mapped from MAVLink component ID
char[17] param_id # MAVLink/UAVCAN parameter name
int16 param_index # parameter index, if known
uint16 param_count # number of parameters exposed by the node
uint8 param_type # MAVLink parameter type
int64 int_value # current value if param_type is int-like
float32 real_value # current value if param_type is float-like
msg/vehicle_command.msg
+1
View File
@@ -53,6 +53,7 @@ uint32 VEHICLE_CMD_DO_TRIGGER_CONTROL = 2003 # Enable or disable on-b
uint32 VEHICLE_CMD_DO_VTOL_TRANSITION = 3000 # Command VTOL transition uint32 VEHICLE_CMD_DO_VTOL_TRANSITION = 3000 # Command VTOL transition
uint32 VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY = 30001 # Prepare a payload deployment in the flight plan uint32 VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY = 30001 # Prepare a payload deployment in the flight plan
uint32 VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY = 30002 # Control a pre-programmed payload deployment uint32 VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY = 30002 # Control a pre-programmed payload deployment
uint32 VEHICLE_CMD_PREFLIGHT_UAVCAN = 243 # UAVCAN configuration. If param 1 == 1 actuator mapping and direction assignment should be started
uint32 VEHICLE_CMD_RESULT_ACCEPTED = 0 # Command ACCEPTED and EXECUTED | uint32 VEHICLE_CMD_RESULT_ACCEPTED = 0 # Command ACCEPTED and EXECUTED |
uint32 VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED = 1 # Command TEMPORARY REJECTED/DENIED | uint32 VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED = 1 # Command TEMPORARY REJECTED/DENIED |
nuttx-configs/px4fmu-v2/nsh/defconfig
+2 -2
View File
@@ -801,11 +801,11 @@ CONFIG_SCHED_WORKQUEUE=y
CONFIG_SCHED_HPWORK=y CONFIG_SCHED_HPWORK=y
CONFIG_SCHED_WORKPRIORITY=192 CONFIG_SCHED_WORKPRIORITY=192
CONFIG_SCHED_WORKPERIOD=5000 CONFIG_SCHED_WORKPERIOD=5000
CONFIG_SCHED_WORKSTACKSIZE=1800 CONFIG_SCHED_WORKSTACKSIZE=1600
CONFIG_SCHED_LPWORK=y CONFIG_SCHED_LPWORK=y
CONFIG_SCHED_LPWORKPRIORITY=50 CONFIG_SCHED_LPWORKPRIORITY=50
CONFIG_SCHED_LPWORKPERIOD=50000 CONFIG_SCHED_LPWORKPERIOD=50000
CONFIG_SCHED_LPWORKSTACKSIZE=1800 CONFIG_SCHED_LPWORKSTACKSIZE=1600
# CONFIG_LIB_KBDCODEC is not set # CONFIG_LIB_KBDCODEC is not set
# CONFIG_LIB_SLCDCODEC is not set # CONFIG_LIB_SLCDCODEC is not set
src/drivers/drv_sensor.h
+5
View File
@@ -121,5 +121,10 @@
*/ */
#define SENSORIOCGROTATION _SENSORIOC(6) #define SENSORIOCGROTATION _SENSORIOC(6)
/**
* Test the sensor calibration
*/
#define SENSORIOCCALTEST _SENSORIOC(7)
#endif /* _DRV_SENSOR_H */ #endif /* _DRV_SENSOR_H */
src/examples/hwtest/hwtest.c
+4 -2
View File
@@ -57,8 +57,10 @@ __EXPORT int ex_hwtest_main(int argc, char *argv[]);
int ex_hwtest_main(int argc, char *argv[]) int ex_hwtest_main(int argc, char *argv[])
{ {
warnx("DO NOT FORGET TO STOP THE COMMANDER APP!"); warnx("DO NOT FORGET TO STOP THE DEFAULT CONTROL APPS!");
warnx("(run <commander stop> to do so)"); warnx("(run <commander stop>,)");
warnx("( <mc_att_control stop> and)");
warnx("( <fw_att_control stop> to do so)");
warnx("usage: http://px4.io/dev/examples/write_output"); warnx("usage: http://px4.io/dev/examples/write_output");
struct actuator_controls_s actuators; struct actuator_controls_s actuators;
src/modules/commander/PreflightCheck.cpp
+134 -27
View File
@@ -71,7 +71,50 @@
namespace Commander 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; bool success = true;
@@ -88,13 +131,9 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional)
return false; return false;
} }
int calibration_devid; int ret = check_calibration(fd, "CAL_MAG%u_ID", device_id);
int ret;
int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
sprintf(s, "CAL_MAG%u_ID", instance);
param_get(param_find(s), &(calibration_devid));
if (devid != calibration_devid) { if (ret) {
mavlink_and_console_log_critical(mavlink_fd, mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: MAG #%u UNCALIBRATED", instance); "PREFLIGHT FAIL: MAG #%u UNCALIBRATED", instance);
success = false; success = false;
@@ -115,7 +154,7 @@ out:
return success; 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; bool success = true;
@@ -132,13 +171,9 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
return false; return false;
} }
int calibration_devid; int ret = check_calibration(fd, "CAL_ACC%u_ID", device_id);
int ret;
int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
sprintf(s, "CAL_ACC%u_ID", instance);
param_get(param_find(s), &(calibration_devid));
if (devid != calibration_devid) { if (ret) {
mavlink_and_console_log_critical(mavlink_fd, mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED", instance); "PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED", instance);
success = false; success = false;
@@ -184,7 +219,7 @@ out:
return success; 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; bool success = true;
@@ -201,13 +236,9 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional)
return false; return false;
} }
int calibration_devid; int ret = check_calibration(fd, "CAL_GYRO%u_ID", device_id);
int ret;
int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
sprintf(s, "CAL_GYRO%u_ID", instance);
param_get(param_find(s), &(calibration_devid));
if (devid != calibration_devid) { if (ret) {
mavlink_and_console_log_critical(mavlink_fd, mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: GYRO #%u UNCALIBRATED", instance); "PREFLIGHT FAIL: GYRO #%u UNCALIBRATED", instance);
success = false; success = false;
@@ -228,7 +259,7 @@ out:
return success; 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; bool success = true;
@@ -245,6 +276,20 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional)
return false; 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); px4_close(fd);
return success; return success;
} }
@@ -311,49 +356,110 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
/* ---- MAG ---- */ /* ---- MAG ---- */
if (checkMag) { 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 */ /* check all sensors, but fail only for mandatory ones */
for (unsigned i = 0; i < max_optional_mag_count; i++) { for (unsigned i = 0; i < max_optional_mag_count; i++) {
bool required = (i < max_mandatory_mag_count); 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; 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 ---- */ /* ---- ACCEL ---- */
if (checkAcc) { 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 */ /* check all sensors, but fail only for mandatory ones */
for (unsigned i = 0; i < max_optional_accel_count; i++) { for (unsigned i = 0; i < max_optional_accel_count; i++) {
bool required = (i < max_mandatory_accel_count); 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; 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 ---- */ /* ---- GYRO ---- */
if (checkGyro) { 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 */ /* check all sensors, but fail only for mandatory ones */
for (unsigned i = 0; i < max_optional_gyro_count; i++) { for (unsigned i = 0; i < max_optional_gyro_count; i++) {
bool required = (i < max_mandatory_gyro_count); 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; 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 ---- */ /* ---- BARO ---- */
if (checkBaro) { 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 */ /* check all sensors, but fail only for mandatory ones */
for (unsigned i = 0; i < max_optional_baro_count; i++) { for (unsigned i = 0; i < max_optional_baro_count; i++) {
bool required = (i < max_mandatory_baro_count); 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; 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 ---- */ /* ---- RC CALIBRATION ---- */
if (checkRC) { if (checkRC) {
if (rc_calibration_check(mavlink_fd) != OK) { if (rc_calibration_check(mavlink_fd) != OK) {
mavlink_log_critical(mavlink_fd, "RC calibration check failed");
failed = true; failed = true;
} }
} }
/* ---- Global Navigation Satellite System receiver ---- */ /* ---- Global Navigation Satellite System receiver ---- */
if(checkGNSS) { if (checkGNSS) {
if(!gnssCheck(mavlink_fd)) { if(!gnssCheck(mavlink_fd)) {
failed = true; failed = true;
} }
src/modules/commander/accelerometer_calibration.cpp
+15 -1
View File
@@ -156,6 +156,10 @@ static const int ERROR = -1;
static const char *sensor_name = "accel"; 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 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); 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]); 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 do_accel_calibration(int mavlink_fd)
{ {
int fd; int fd;
int32_t device_id[max_accel_sens];
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name); 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; bool failed = false;
failed = failed || (OK != param_set_no_notification(param_find("CAL_ACC_PRIME"), &(device_id_primary)));
/* set parameters */ /* set parameters */
(void)sprintf(str, "CAL_ACC%u_XOFF", i); (void)sprintf(str, "CAL_ACC%u_XOFF", i);
failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.x_offset))); 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 = {}; struct accel_report arp = {};
(void)orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &arp); (void)orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &arp);
timestamps[i] = arp.timestamp; 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) { if (result == calibrate_return_ok) {
src/modules/commander/commander.cpp
+1
View File
@@ -802,6 +802,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION: case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION:
case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS:
case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE: case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE:
case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_UAVCAN:
case vehicle_command_s::VEHICLE_CMD_CUSTOM_0: case vehicle_command_s::VEHICLE_CMD_CUSTOM_0:
case vehicle_command_s::VEHICLE_CMD_CUSTOM_1: case vehicle_command_s::VEHICLE_CMD_CUSTOM_1:
case vehicle_command_s::VEHICLE_CMD_CUSTOM_2: case vehicle_command_s::VEHICLE_CMD_CUSTOM_2:
src/modules/commander/gyro_calibration.cpp
+15
View File
@@ -168,6 +168,9 @@ int do_gyro_calibration(int mavlink_fd)
1.0f, // z scale 1.0f, // z scale
}; };
int device_prio_max = 0;
int32_t device_id_primary = 0;
for (unsigned s = 0; s < max_gyros; s++) { for (unsigned s = 0; s < max_gyros; s++) {
char str[30]; char str[30];
@@ -199,6 +202,15 @@ int do_gyro_calibration(int mavlink_fd)
for (unsigned s = 0; s < max_gyros; s++) { for (unsigned s = 0; s < max_gyros; s++) {
worker_data.gyro_sensor_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), 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(); int cancel_sub = calibrate_cancel_subscribe();
@@ -258,9 +270,12 @@ int do_gyro_calibration(int mavlink_fd)
} }
if (res == OK) { if (res == OK) {
/* set offset parameters to new values */ /* set offset parameters to new values */
bool failed = false; 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++) { for (unsigned s = 0; s < max_gyros; s++) {
if (worker_data.device_id[s] != 0) { if (worker_data.device_id[s] != 0) {
char str[30]; char str[30];
src/modules/commander/mag_calibration.cpp
+16 -1
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 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 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]); calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]);
/// Data passed to calibration worker routine /// 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 // Determine which mags are available and reset each
int32_t device_ids[max_mags];
char str[30]; char str[30];
for (size_t i=0; i<max_mags; i++) { 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; result = calibrate_return_error;
break; 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) { 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++) { for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
if (device_ids[cur_mag] != 0) { if (device_ids[cur_mag] != 0) {
int fd_mag = -1; int fd_mag = -1;
src/modules/ekf_att_pos_estimator/CMakeLists.txt
+1
View File
@@ -39,6 +39,7 @@ endif()
px4_add_module( px4_add_module(
MODULE modules__ekf_att_pos_estimator MODULE modules__ekf_att_pos_estimator
MAIN ekf_att_pos_estimator MAIN ekf_att_pos_estimator
STACK 3000
COMPILE_FLAGS ${MODULE_CFLAGS} COMPILE_FLAGS ${MODULE_CFLAGS}
SRCS SRCS
ekf_att_pos_estimator_main.cpp ekf_att_pos_estimator_main.cpp
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+7 -8
View File
@@ -1194,7 +1194,7 @@ int AttitudePositionEstimatorEKF::start()
_estimator_task = px4_task_spawn_cmd("ekf_att_pos_estimator", _estimator_task = px4_task_spawn_cmd("ekf_att_pos_estimator",
SCHED_DEFAULT, SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 20, SCHED_PRIORITY_MAX - 20,
4200, 3900,
(px4_main_t)&AttitudePositionEstimatorEKF::task_main_trampoline, (px4_main_t)&AttitudePositionEstimatorEKF::task_main_trampoline,
nullptr); nullptr);
@@ -1235,6 +1235,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, 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->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y,
(double)_ekf->correctedDelAng.z); (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], 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]); (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], PX4_INFO("states (vel m/s) [4-6]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[4], (double)_ekf->states[5],
@@ -1338,7 +1339,7 @@ void AttitudePositionEstimatorEKF::pollData()
} else { } else {
_ekf->dVelIMU.x = 0.5f * (_ekf->accel.x + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0]); _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.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]) + 9.80665f; _ekf->dVelIMU.z = 0.5f * (_ekf->accel.z + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 2]);
} }
_ekf->accel.x = _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0]; _ekf->accel.x = _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0];
@@ -1414,21 +1415,21 @@ void AttitudePositionEstimatorEKF::pollData()
static hrt_abstime lastprint = 0; static hrt_abstime lastprint = 0;
if (hrt_elapsed_time(&lastprint) > 1000000) { 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, (double)deltaT, (double)deltaTIntegral, (double)deltaTIntAcc,
dtoverflow5, dtoverflow10); dtoverflow5, dtoverflow10);
warnx("EKF: dang: %8.4f %8.4f dvel: %8.4f %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->dAngIMU.x, (double)_ekf->dAngIMU.z,
(double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z); (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z);
warnx("INT: dang: %8.4f %8.4f dvel: %8.4f %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.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[0]),
(double)(_sensor_combined.accelerometer_integral_m_s[1]), (double)(_sensor_combined.accelerometer_integral_m_s[1]),
(double)(_sensor_combined.accelerometer_integral_m_s[2])); (double)(_sensor_combined.accelerometer_integral_m_s[2]));
warnx("DRV: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f", 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.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[0] * deltaT),
(double)(_sensor_combined.accelerometer_m_s2[1] * deltaT), (double)(_sensor_combined.accelerometer_m_s2[1] * deltaT),
@@ -1738,8 +1739,6 @@ int ekf_att_pos_estimator_main(int argc, char *argv[])
} }
if (!strcmp(argv[1], "status")) { if (!strcmp(argv[1], "status")) {
PX4_INFO("running");
estimator::g_estimator->print_status(); estimator::g_estimator->print_status();
return 0; return 0;
src/modules/fw_att_control/CMakeLists.txt
-1
View File
@@ -38,7 +38,6 @@ px4_add_module(
-Os -Os
SRCS SRCS
fw_att_control_main.cpp fw_att_control_main.cpp
fw_att_control_params.c
DEPENDS DEPENDS
platforms__common platforms__common
) )
src/modules/fw_att_control/fw_att_control_params.c
-5
View File
@@ -40,11 +40,6 @@
* @author Thomas Gubler <thomas@px4.io> * @author Thomas Gubler <thomas@px4.io>
*/ */
#include <px4_config.h>
#include <systemlib/param/param.h>
/* /*
* Controller parameters, accessible via MAVLink * Controller parameters, accessible via MAVLink
* *
src/modules/fw_pos_control_l1/CMakeLists.txt
-2
View File
@@ -39,11 +39,9 @@ px4_add_module(
-Os -Os
SRCS SRCS
fw_pos_control_l1_main.cpp fw_pos_control_l1_main.cpp
fw_pos_control_l1_params.c
landingslope.cpp landingslope.cpp
mtecs/mTecs.cpp mtecs/mTecs.cpp
mtecs/limitoverride.cpp mtecs/limitoverride.cpp
mtecs/mTecs_params.c
DEPENDS DEPENDS
platforms__common platforms__common
lib__external_lgpl lib__external_lgpl
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
-3
View File
@@ -39,9 +39,6 @@
* @author Lorenz Meier <lm@inf.ethz.ch> * @author Lorenz Meier <lm@inf.ethz.ch>
*/ */
#include <px4_config.h>
#include <systemlib/param/param.h>
/* /*
* Controller parameters, accessible via MAVLink * Controller parameters, accessible via MAVLink
*/ */
src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
-3
View File
@@ -39,9 +39,6 @@
* @author Thomas Gubler <thomasgubler@gmail.com> * @author Thomas Gubler <thomasgubler@gmail.com>
*/ */
#include <px4_config.h>
#include <systemlib/param/param.h>
/* /*
* Controller parameters, accessible via MAVLink * Controller parameters, accessible via MAVLink
*/ */
src/modules/mavlink/mavlink_parameters.cpp
+116 -31
View File
@@ -41,27 +41,29 @@
#include <stdio.h> #include <stdio.h>
#include <uORB/topics/uavcan_parameter_request.h>
#include <uORB/topics/uavcan_parameter_value.h>
#include "mavlink_parameters.h" #include "mavlink_parameters.h"
#include "mavlink_main.h" #include "mavlink_main.h"
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" #define HASH_PARAM "_HASH_CHECK"
MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkStream(mavlink), MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkStream(mavlink),
_send_all_index(-1), _send_all_index(-1),
_rc_param_map_pub(nullptr), _rc_param_map_pub(nullptr),
_rc_param_map() _rc_param_map(),
_uavcan_parameter_request_pub(nullptr),
_uavcan_parameter_value_sub(-1)
{ {
} }
unsigned unsigned
MavlinkParametersManager::get_size() MavlinkParametersManager::get_size()
{ {
if (_send_all_index >= 0) { return MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
return MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
} else {
return 0;
}
} }
void void
@@ -78,36 +80,75 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
_send_all_index = 0; _send_all_index = 0;
} }
if (req_list.target_system == mavlink_system.sysid && req_list.target_component < 127 &&
(req_list.target_component != mavlink_system.compid || req_list.target_component == MAV_COMP_ID_ALL)) {
// publish list request to UAVCAN driver via uORB.
uavcan_parameter_request_s req;
req.message_type = msg->msgid;
req.node_id = req_list.target_component;
req.param_index = 0;
if (_uavcan_parameter_request_pub == nullptr) {
_uavcan_parameter_request_pub = orb_advertise(ORB_ID(uavcan_parameter_request), &req);
} else {
orb_publish(ORB_ID(uavcan_parameter_request), _uavcan_parameter_request_pub, &req);
}
}
break; break;
} }
case MAVLINK_MSG_ID_PARAM_SET: { case MAVLINK_MSG_ID_PARAM_SET: {
/* set parameter */ /* set parameter */
if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) { mavlink_param_set_t set;
mavlink_param_set_t set; mavlink_msg_param_set_decode(msg, &set);
mavlink_msg_param_set_decode(msg, &set);
if (set.target_system == mavlink_system.sysid && if (set.target_system == mavlink_system.sysid &&
(set.target_component == mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) { (set.target_component == mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) {
/* local name buffer to enforce null-terminated string */ /* local name buffer to enforce null-terminated string */
char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
strncpy(name, set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); strncpy(name, set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
/* enforce null termination */ /* enforce null termination */
name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
/* attempt to find parameter, set and send it */ /* attempt to find parameter, set and send it */
param_t param = param_find_no_notification(name); param_t param = param_find_no_notification(name);
if (param == PARAM_INVALID) { if (param == PARAM_INVALID) {
char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
sprintf(buf, "[pm] unknown param: %s", name); sprintf(buf, "[pm] unknown param: %s", name);
_mavlink->send_statustext_info(buf); _mavlink->send_statustext_info(buf);
} else { } else {
/* set and send parameter */ /* set and send parameter */
param_set(param, &(set.param_value)); param_set(param, &(set.param_value));
send_param(param); send_param(param);
} }
}
if (set.target_system == mavlink_system.sysid && set.target_component < 127 &&
(set.target_component != mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) {
// publish set request to UAVCAN driver via uORB.
uavcan_parameter_request_s req;
req.message_type = msg->msgid;
req.node_id = set.target_component;
req.param_index = -1;
strncpy(req.param_id, set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1);
req.param_id[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
if (set.param_type == MAV_PARAM_TYPE_REAL32) {
req.param_type = MAV_PARAM_TYPE_REAL32;
req.real_value = set.param_value;
} else {
int32_t val;
memcpy(&val, &set.param_value, sizeof(int32_t));
req.param_type = MAV_PARAM_TYPE_INT64;
req.int_value = val;
}
if (_uavcan_parameter_request_pub == nullptr) {
_uavcan_parameter_request_pub = orb_advertise(ORB_ID(uavcan_parameter_request), &req);
} else {
orb_publish(ORB_ID(uavcan_parameter_request), _uavcan_parameter_request_pub, &req);
} }
} }
break; break;
@@ -160,6 +201,23 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
} }
} }
} }
if (req_read.target_system == mavlink_system.sysid && req_read.target_component < 127 &&
(req_read.target_component != mavlink_system.compid || req_read.target_component == MAV_COMP_ID_ALL)) {
// publish set request to UAVCAN driver via uORB.
uavcan_parameter_request_s req;
req.message_type = msg->msgid;
req.node_id = req_read.target_component;
req.param_index = req_read.param_index;
strncpy(req.param_id, req_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1);
req.param_id[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
if (_uavcan_parameter_request_pub == nullptr) {
_uavcan_parameter_request_pub = orb_advertise(ORB_ID(uavcan_parameter_request), &req);
} else {
orb_publish(ORB_ID(uavcan_parameter_request), _uavcan_parameter_request_pub, &req);
}
}
break; break;
} }
@@ -208,11 +266,38 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
void void
MavlinkParametersManager::send(const hrt_abstime t) MavlinkParametersManager::send(const hrt_abstime t)
{ {
/* send all parameters if requested, but only after the system has booted */ bool space_available = _mavlink->get_free_tx_buf() >= get_size();
if (_send_all_index >= 0 && _mavlink->boot_complete()) {
/* Send parameter values received from the UAVCAN topic */
if (_uavcan_parameter_value_sub < 0) {
_uavcan_parameter_value_sub = orb_subscribe(ORB_ID(uavcan_parameter_value));
}
bool param_value_ready;
orb_check(_uavcan_parameter_value_sub, &param_value_ready);
if (space_available && param_value_ready) {
struct uavcan_parameter_value_s value;
orb_copy(ORB_ID(uavcan_parameter_value), _uavcan_parameter_value_sub, &value);
mavlink_param_value_t msg;
msg.param_count = value.param_count;
msg.param_index = value.param_index;
strncpy(msg.param_id, value.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
if (value.param_type == MAV_PARAM_TYPE_REAL32) {
msg.param_type = MAVLINK_TYPE_FLOAT;
msg.param_value = value.real_value;
} else {
int32_t val;
val = (int32_t)value.int_value;
memcpy(&msg.param_value, &val, sizeof(int32_t));
msg.param_type = MAVLINK_TYPE_INT32_T;
}
_mavlink->send_message(MAVLINK_MSG_ID_PARAM_VALUE, &msg, value.node_id);
} else if (_send_all_index >= 0 && _mavlink->boot_complete()) {
/* send all parameters if requested, but only after the system has booted */
/* skip if no space is available */ /* skip if no space is available */
if (_mavlink->get_free_tx_buf() < get_size()) { if (!space_available) {
return; return;
} }
src/modules/mavlink/mavlink_parameters.h
+3
View File
@@ -118,4 +118,7 @@ protected:
orb_advert_t _rc_param_map_pub; orb_advert_t _rc_param_map_pub;
struct rc_parameter_map_s _rc_param_map; struct rc_parameter_map_s _rc_param_map;
orb_advert_t _uavcan_parameter_request_pub;
int _uavcan_parameter_value_sub;
}; };
src/modules/mavlink/mavlink_receiver.cpp
+4 -4
View File
@@ -1444,12 +1444,12 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2; hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2;
hil_sensors.accelerometer_raw[1] = imu.yacc / mg2ms2; hil_sensors.accelerometer_raw[1] = imu.yacc / mg2ms2;
hil_sensors.accelerometer_raw[2] = imu.zacc / mg2ms2; hil_sensors.accelerometer_raw[2] = imu.zacc / mg2ms2;
hil_sensors.accelerometer_m_s2[0] = ((imu.xacc * dt + _hil_prev_accel[0]) / 2.0f) / dt; hil_sensors.accelerometer_m_s2[0] = imu.xacc;
hil_sensors.accelerometer_m_s2[1] = ((imu.yacc * dt + _hil_prev_accel[1]) / 2.0f) / dt; hil_sensors.accelerometer_m_s2[1] = imu.yacc;
hil_sensors.accelerometer_m_s2[2] = (((imu.zacc + 9.80665f) * dt + _hil_prev_accel[2]) / 2.0f) / dt - 9.80665f; hil_sensors.accelerometer_m_s2[2] = imu.zacc;
hil_sensors.accelerometer_integral_m_s[0] = (imu.xacc * dt + _hil_prev_accel[0]) / 2.0f; hil_sensors.accelerometer_integral_m_s[0] = (imu.xacc * dt + _hil_prev_accel[0]) / 2.0f;
hil_sensors.accelerometer_integral_m_s[1] = (imu.yacc * dt + _hil_prev_accel[1]) / 2.0f; hil_sensors.accelerometer_integral_m_s[1] = (imu.yacc * dt + _hil_prev_accel[1]) / 2.0f;
hil_sensors.accelerometer_integral_m_s[2] = ((imu.zacc + 9.80665f) * dt + _hil_prev_accel[2]) / 2.0f; hil_sensors.accelerometer_integral_m_s[2] = (imu.zacc * dt + _hil_prev_accel[2]) / 2.0f;
memcpy(&_hil_prev_accel[0], &hil_sensors.accelerometer_integral_m_s[0], sizeof(_hil_prev_accel)); memcpy(&_hil_prev_accel[0], &hil_sensors.accelerometer_integral_m_s[0], sizeof(_hil_prev_accel));
hil_sensors.accelerometer_integral_dt[0] = dt * 1e6f; hil_sensors.accelerometer_integral_dt[0] = dt * 1e6f;
hil_sensors.accelerometer_mode[0] = 0; // TODO what is this? hil_sensors.accelerometer_mode[0] = 0; // TODO what is this?
src/modules/mavlink/mavlink_tests/module.mk
-53
View File
@@ -1,53 +0,0 @@
############################################################################
#
# Copyright (c) 2014 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.
#
############################################################################
#
# System state machine tests.
#
MODULE_COMMAND = mavlink_tests
SRCS = mavlink_tests.cpp \
mavlink_ftp_test.cpp \
../mavlink_stream.cpp \
../mavlink_ftp.cpp \
../mavlink.c
INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink
MODULE_STACKSIZE = 5000
MAXOPTIMIZATION = -Os
EXTRACXXFLAGS = -Weffc++ -DMAVLINK_FTP_UNIT_TEST -Wno-attributes -Wno-packed
EXTRACFLAGS = -Wno-packed
src/modules/navigator/CMakeLists.txt
-7
View File
@@ -39,24 +39,17 @@ px4_add_module(
-Os -Os
SRCS SRCS
navigator_main.cpp navigator_main.cpp
navigator_params.c
navigator_mode.cpp navigator_mode.cpp
mission_block.cpp mission_block.cpp
mission.cpp mission.cpp
mission_params.c
loiter.cpp loiter.cpp
rtl.cpp rtl.cpp
rtl_params.c
mission_feasibility_checker.cpp mission_feasibility_checker.cpp
geofence.cpp geofence.cpp
geofence_params.c
datalinkloss.cpp datalinkloss.cpp
datalinkloss_params.c
rcloss.cpp rcloss.cpp
rcloss_params.c
enginefailure.cpp enginefailure.cpp
gpsfailure.cpp gpsfailure.cpp
gpsfailure_params.c
DEPENDS DEPENDS
platforms__common platforms__common
) )
src/modules/navigator/datalinkloss_params.c
-4
View File
@@ -39,10 +39,6 @@
* @author Thomas Gubler <thomas@px4.io> * @author Thomas Gubler <thomas@px4.io>
*/ */
#include <px4_config.h>
#include <systemlib/param/param.h>
/* /*
* Data Link Loss parameters, accessible via MAVLink * Data Link Loss parameters, accessible via MAVLink
*/ */
src/modules/navigator/geofence_params.c
-4
View File
@@ -39,10 +39,6 @@
* @author Thomas Gubler <thomasgubler@gmail.com> * @author Thomas Gubler <thomasgubler@gmail.com>
*/ */
#include <px4_config.h>
#include <systemlib/param/param.h>
/* /*
* Geofence parameters, accessible via MAVLink * Geofence parameters, accessible via MAVLink
*/ */
src/modules/navigator/gpsfailure_params.c
-4
View File
@@ -39,10 +39,6 @@
* @author Thomas Gubler <thomasgubler@gmail.com> * @author Thomas Gubler <thomasgubler@gmail.com>
*/ */
#include <px4_config.h>
#include <systemlib/param/param.h>
/* /*
* GPS Failure Navigation Mode parameters, accessible via MAVLink * GPS Failure Navigation Mode parameters, accessible via MAVLink
*/ */
src/modules/navigator/mission_feasibility_checker.cpp
+1 -1
View File
@@ -336,7 +336,7 @@ MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionI
} else { } else {
/* item is too far from home */ /* 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; warning_issued = true;
return false; return false;
} }
src/modules/navigator/mission_params.c
-4
View File
@@ -39,10 +39,6 @@
* @author Julian Oes <joes@student.ethz.ch> * @author Julian Oes <joes@student.ethz.ch>
*/ */
#include <px4_config.h>
#include <systemlib/param/param.h>
/* /*
* Mission parameters, accessible via MAVLink * Mission parameters, accessible via MAVLink
*/ */
src/modules/navigator/navigator_params.c
-4
View File
@@ -40,10 +40,6 @@
* @author Thomas Gubler <thomas@px4.io> * @author Thomas Gubler <thomas@px4.io>
*/ */
#include <px4_config.h>
#include <systemlib/param/param.h>
/** /**
* Loiter radius (FW only) * Loiter radius (FW only)
* *
src/modules/navigator/rcloss_params.c
-4
View File
@@ -39,10 +39,6 @@
* @author Thomas Gubler <thomasgubler@gmail.com> * @author Thomas Gubler <thomasgubler@gmail.com>
*/ */
#include <px4_config.h>
#include <systemlib/param/param.h>
/* /*
* OBC RC Loss mode parameters, accessible via MAVLink * OBC RC Loss mode parameters, accessible via MAVLink
*/ */
src/modules/navigator/rtl_params.c
-4
View File
@@ -39,10 +39,6 @@
* @author Julian Oes <julian@oes.ch> * @author Julian Oes <julian@oes.ch>
*/ */
#include <px4_config.h>
#include <systemlib/param/param.h>
/* /*
* RTL parameters, accessible via MAVLink * RTL parameters, accessible via MAVLink
*/ */
src/modules/position_estimator_inav/position_estimator_inav_main.c
+1 -1
View File
@@ -897,7 +897,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
} }
float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f; float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f;
dt = fmaxf(fminf(0.02, dt), 0.002); // constrain dt from 2 to 20 ms dt = fmaxf(fminf(0.02, dt), 0.0002); // constrain dt from 0.2 to 20 ms
t_prev = t; t_prev = t;
/* increase EPH/EPV on each step */ /* increase EPH/EPV on each step */
src/modules/sdlog2/logbuffer.c
+12 -2
View File
@@ -50,8 +50,8 @@ int logbuffer_init(struct logbuffer_s *lb, int size)
lb->size = size; lb->size = size;
lb->write_ptr = 0; lb->write_ptr = 0;
lb->read_ptr = 0; lb->read_ptr = 0;
lb->data = malloc(lb->size); lb->data = NULL;
return (lb->data == 0) ? PX4_ERROR : PX4_OK; return PX4_OK;
} }
int logbuffer_count(struct logbuffer_s *lb) 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) 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 // bytes available to write
int available = lb->read_ptr - lb->write_ptr - 1; int available = lb->read_ptr - lb->write_ptr - 1;
src/modules/sensors/CMakeLists.txt
-1
View File
@@ -40,7 +40,6 @@ px4_add_module(
-O3 -O3
SRCS SRCS
sensors.cpp sensors.cpp
sensor_params.c
DEPENDS DEPENDS
platforms__common platforms__common
) )
src/modules/sensors/sensor_params.c
+28
View File
@@ -585,6 +585,34 @@ PARAM_DEFINE_FLOAT(CAL_ACC2_YSCALE, 1.0f);
*/ */
PARAM_DEFINE_FLOAT(CAL_ACC2_ZSCALE, 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 * Differential pressure sensor offset
* *
src/modules/sensors/sensors.cpp
+16
View File
@@ -1412,6 +1412,8 @@ Sensors::parameter_update_poll(bool forced)
(void)sprintf(str, "CAL_MAG%u_ID", i); (void)sprintf(str, "CAL_MAG%u_ID", i);
int device_id; int device_id;
failed = failed || (OK != param_get(param_find(str), &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) { if (failed) {
px4_close(fd); px4_close(fd);
@@ -2020,6 +2022,11 @@ Sensors::task_main()
* do subscriptions * do subscriptions
*/ */
unsigned gcount_prev = _gyro_count;
unsigned mcount_prev = _mag_count;
unsigned acount_prev = _accel_count;
unsigned bcount_prev = _baro_count;
_gyro_count = init_sensor_class(ORB_ID(sensor_gyro), &_gyro_sub[0], _gyro_count = init_sensor_class(ORB_ID(sensor_gyro), &_gyro_sub[0],
&raw.gyro_priority[0], &raw.gyro_errcount[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], _baro_count = init_sensor_class(ORB_ID(sensor_baro), &_baro_sub[0],
&raw.baro_priority[0], &raw.baro_errcount[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)); _rc_sub = orb_subscribe(ORB_ID(input_rc));
_diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); _diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
src/modules/uavcan/CMakeLists.txt
+1 -1
View File
@@ -63,7 +63,7 @@ px4_add_module(
COMPILE_FLAGS COMPILE_FLAGS
-Wframe-larger-than=1500 -Wframe-larger-than=1500
-Wno-deprecated-declarations -Wno-deprecated-declarations
-O3 -Os
SRCS SRCS
# Main # Main
uavcan_main.cpp uavcan_main.cpp
src/modules/uavcan/uavcan_main.hpp
+1 -1
View File
@@ -94,7 +94,7 @@ class UavcanNode : public device::CDev
*/ */
static constexpr unsigned RxQueueLenPerIface = FramePerMSecond * PollTimeoutMs; // At static constexpr unsigned RxQueueLenPerIface = FramePerMSecond * PollTimeoutMs; // At
static constexpr unsigned StackSize = 1600; static constexpr unsigned StackSize = 1800;
public: public:
typedef uavcan::Node<MemPoolSize> Node; typedef uavcan::Node<MemPoolSize> Node;
src/modules/uavcan/uavcan_servers.cpp
+849 -7
View File
File diff suppressed because it is too large Load Diff
src/modules/uavcan/uavcan_servers.hpp
+94 -10
View File
@@ -42,16 +42,20 @@
#include <uavcan/node/sub_node.hpp> #include <uavcan/node/sub_node.hpp>
#include <uavcan/protocol/node_status_monitor.hpp> #include <uavcan/protocol/node_status_monitor.hpp>
# include <uavcan/protocol/dynamic_node_id_server/centralized.hpp> #include <uavcan/protocol/dynamic_node_id_server/centralized.hpp>
# include <uavcan/protocol/node_info_retriever.hpp> #include <uavcan/protocol/node_info_retriever.hpp>
# include <uavcan_posix/basic_file_server_backend.hpp> #include <uavcan_posix/basic_file_server_backend.hpp>
# include <uavcan/protocol/firmware_update_trigger.hpp> #include <uavcan/protocol/firmware_update_trigger.hpp>
# include <uavcan/protocol/file_server.hpp> #include <uavcan/protocol/file_server.hpp>
# include <uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp> #include <uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp>
# include <uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp> #include <uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp>
# include <uavcan_posix/firmware_version_checker.hpp> #include <uavcan_posix/firmware_version_checker.hpp>
#include <uavcan/equipment/esc/RawCommand.hpp>
#include <uavcan/equipment/indication/BeepCommand.hpp>
#include <uavcan/protocol/enumeration/Begin.hpp>
#include <uavcan/protocol/enumeration/Indication.hpp>
# include "uavcan_virtual_can_driver.hpp" #include "uavcan_virtual_can_driver.hpp"
/** /**
* @file uavcan_servers.hpp * @file uavcan_servers.hpp
@@ -65,6 +69,9 @@
#define UAVCAN_DEVICE_PATH "/dev/uavcan/esc" #define UAVCAN_DEVICE_PATH "/dev/uavcan/esc"
#define UAVCAN_NODE_DB_PATH "/fs/microsd/uavcan.db" #define UAVCAN_NODE_DB_PATH "/fs/microsd/uavcan.db"
#define UAVCAN_FIRMWARE_PATH "/fs/microsd/fw" #define UAVCAN_FIRMWARE_PATH "/fs/microsd/fw"
#define UAVCAN_ROMFS_FW_PATH "/etc/uavcan/fw"
#define UAVCAN_ROMFS_FW_PREFIX "romfs_"
#define UAVCAN_MAX_PATH_LENGTH (128 + 40)
#define UAVCAN_LOG_FILE UAVCAN_NODE_DB_PATH"/trace.log" #define UAVCAN_LOG_FILE UAVCAN_NODE_DB_PATH"/trace.log"
/** /**
@@ -88,7 +95,7 @@ class UavcanServers
static constexpr unsigned QueuePoolSize = static constexpr unsigned QueuePoolSize =
(NumIfaces * uavcan::MemPoolBlockSize * MaxCanFramesPerTransfer); (NumIfaces * uavcan::MemPoolBlockSize * MaxCanFramesPerTransfer);
static constexpr unsigned StackSize = 3500; static constexpr unsigned StackSize = 6000;
static constexpr unsigned Priority = 120; static constexpr unsigned Priority = 120;
typedef uavcan::SubNode<MemPoolSize> SubNode; typedef uavcan::SubNode<MemPoolSize> SubNode;
@@ -140,7 +147,84 @@ private:
uavcan::FirmwareUpdateTrigger _fw_upgrade_trigger; uavcan::FirmwareUpdateTrigger _fw_upgrade_trigger;
uavcan::BasicFileServer _fw_server; uavcan::BasicFileServer _fw_server;
/*
* The MAVLink parameter bridge needs to know the maximum parameter index
* of each node so that clients can determine when parameter listings have
* finished. We do that by querying a node's entire parameter set whenever
* a parameter message is received for a node with a zero _param_count,
* and storing the count here. If a node doesn't respond, or doesn't have
* any parameters, its count will stay at zero and we'll try to query the
* set again next time.
*
* The node's UAVCAN ID is used as the index into the _param_counts array.
*/
uint8_t _param_counts[128];
bool _count_in_progress;
uint8_t _count_index;
bool _param_in_progress;
uint8_t _param_index;
bool _param_list_in_progress;
bool _param_list_all_nodes;
uint8_t _param_list_node_id;
uint32_t _param_dirty_bitmap[4];
uint8_t _param_save_opcode;
bool _cmd_in_progress;
// uORB topic handle for MAVLink parameter responses
orb_advert_t _param_response_pub;
typedef uavcan::MethodBinder<UavcanServers *,
void (UavcanServers::*)(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &)> GetSetCallback;
typedef uavcan::MethodBinder<UavcanServers *,
void (UavcanServers::*)(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &)> ExecuteOpcodeCallback;
typedef uavcan::MethodBinder<UavcanServers *,
void (UavcanServers::*)(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &)> RestartNodeCallback;
void cb_getset(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result);
void cb_count(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result);
void cb_opcode(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result);
void cb_restart(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &result);
uavcan::ServiceClient<uavcan::protocol::param::GetSet, GetSetCallback> _param_getset_client;
uavcan::ServiceClient<uavcan::protocol::param::ExecuteOpcode, ExecuteOpcodeCallback> _param_opcode_client;
uavcan::ServiceClient<uavcan::protocol::RestartNode, RestartNodeCallback> _param_restartnode_client;
void param_count(uavcan::NodeID node_id);
void param_opcode(uavcan::NodeID node_id);
uint8_t get_next_active_node_id(uint8_t base);
uint8_t get_next_dirty_node_id(uint8_t base);
void set_node_params_dirty(uint8_t node_id) { _param_dirty_bitmap[node_id >> 5] |= 1 << (node_id & 31); }
void clear_node_params_dirty(uint8_t node_id) { _param_dirty_bitmap[node_id >> 5] &= ~(1 << (node_id & 31)); }
bool are_node_params_dirty(uint8_t node_id) const { return bool((_param_dirty_bitmap[node_id >> 5] >> (node_id & 31)) & 1); }
bool _mutex_inited; bool _mutex_inited;
volatile bool _check_fw; volatile bool _check_fw;
// ESC enumeration
bool _esc_enumeration_active;
uint8_t _esc_enumeration_ids[uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize];
uint8_t _esc_enumeration_index;
uint8_t _esc_set_index;
uint8_t _esc_count;
typedef uavcan::MethodBinder<UavcanServers *,
void (UavcanServers::*)(const uavcan::ServiceCallResult<uavcan::protocol::enumeration::Begin> &)> EnumerationBeginCallback;
typedef uavcan::MethodBinder<UavcanServers *,
void (UavcanServers::*)(const uavcan::ReceivedDataStructure<uavcan::protocol::enumeration::Indication>&)>
EnumerationIndicationCallback;
void cb_enumeration_begin(const uavcan::ServiceCallResult<uavcan::protocol::enumeration::Begin> &result);
void cb_enumeration_indication(const uavcan::ReceivedDataStructure<uavcan::protocol::enumeration::Indication> &msg);
void cb_enumeration_getset(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result);
void cb_enumeration_save(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result);
uavcan::Publisher<uavcan::equipment::indication::BeepCommand> _beep_pub;
uavcan::Subscriber<uavcan::protocol::enumeration::Indication, EnumerationIndicationCallback> _enumeration_indication_sub;
uavcan::ServiceClient<uavcan::protocol::enumeration::Begin, EnumerationBeginCallback> _enumeration_client;
uavcan::ServiceClient<uavcan::protocol::param::GetSet, GetSetCallback> _enumeration_getset_client;
uavcan::ServiceClient<uavcan::protocol::param::ExecuteOpcode, ExecuteOpcodeCallback> _enumeration_save_client;
void unpackFwFromROMFS(const char* sd_path, const char* romfs_path);
int copyFw(const char* dst, const char* src);
}; };
src/platforms/px4_log.h
+6 -6
View File
@@ -130,7 +130,7 @@ __EXPORT extern int __px4_log_level_current;
__END_DECLS __END_DECLS
// __px4_log_level_current will be initialized to PX4_LOG_LEVEL_AT_RUN_TIME // __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 * 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 * 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_PANIC(FMT, ...) __px4_log(_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_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_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__) #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 * 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_PANIC(FMT, ...) __px4_log(_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_ERR(FMT, ...) __px4_log(_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_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__) #define PX4_DEBUG(FMT, ...) __px4_log_omit(_PX4_LOG_LEVEL_DEBUG, FMT, ##__VA_ARGS__)
#endif #endif