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

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

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
Makefile
View File

@ -89,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
@ -104,13 +104,13 @@ endef
# --------------------------------------------------------------------
# Do not put any spaces between function arguments.
px4fmu-v1_default: git-init
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)
px4fmu-v2_simple: git-init
px4fmu-v2_simple:
$(call cmake-build,nuttx_px4fmu-v2_simple)
nuttx_sim_simple:
@ -172,19 +172,8 @@ check_format:
clean:
@rm -rf build_*/
distclean: clean
@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
@(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

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

23
Tools/generate_listener.py
View File

@ -24,11 +24,16 @@ for index,m in enumerate(raw_messages):
if ('float32[' in line.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))
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])
temp_list.append(("uint64_array",line.split(' ')[1].split('\t')[0].split('\n')[0],num_floats))
elif(line.split(' ')[0] == "float32"):
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:
temp_list.append(("uint64",line.split(' ')[1].split('\t')[0].split('\n')[0]))
elif(line.split(' ')[0] == "uint32") and len(line.split('=')) == 1:
@ -114,6 +119,10 @@ print("""
#define PRIu64 "llu"
#endif
#ifndef PRI64
#define PRI64 "lld"
#endif
""")
for m in messages:
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);")
for item in message_elements[index+1]:
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":
print("\t\t\tprintf(\"%s: \");" % item[1])
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\tprintf(\"\\n\");")
elif item[0] == "uint64":

3
Tools/px4airframes/rcout.py
View File

@ -1,5 +1,6 @@
from xml.sax.saxutils import escape
import codecs
import os
class RCOutput():
def __init__(self, groups, board):
@ -30,7 +31,7 @@ class RCOutput():
for group in groups:
result += "# GROUP: %s\n\n" % group.GetName()
for param in group.GetParams():
path = param.GetPath().rsplit('/', 1)[1]
path = os.path.split(param.GetPath())[1]
id_val = param.GetId()
name = param.GetFieldValue("short_desc")
long_desc = param.GetFieldValue("long_desc")

3
Tools/px4airframes/srcparser.py
View File

@ -1,5 +1,6 @@
import sys
import re
import os
class ParameterGroup(object):
"""
@ -160,7 +161,7 @@ class SourceParser(object):
"""
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
if (not self.IsNumber(airframe_id)):

17
cmake/common/px4_base.cmake
View File

@ -136,14 +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 update --init --recursive -f ${PATH}
COMMAND touch ${CMAKE_BINARY_DIR}/git_${NAME}.stamp
COMMAND git submodule init ${PATH}
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()
#=============================================================================
@ -418,7 +421,8 @@ function(px4_add_upload)
)
elseif(${CMAKE_HOST_SYSTEM_NAME} STREQUAL "Windows")
foreach(port RANGE 32 0)
list(APPEND "COM${port}")
list(APPEND serial_ports
"COM${port}")
endforeach()
endif()
px4_join(OUT serial_ports LIST "${serial_ports}" GLUE ",")
@ -543,6 +547,7 @@ function(px4_add_common_flags)
endif()
set(c_warnings
-Wbad-function-cast
-Wstrict-prototypes
-Wmissing-prototypes
-Wnested-externs

6
cmake/nuttx/px4_impl_nuttx.cmake
View File

@ -213,8 +213,8 @@ function(px4_nuttx_add_export)
# copy
add_custom_command(OUTPUT nuttx_copy_${CONFIG}.stamp
COMMAND ${MKDIR} -p ${CMAKE_BINARY_DIR}/${CONFIG}
COMMAND ${RM} -rf ${nuttx_src}
COMMAND ${CP} -r ${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})
@ -229,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})

8
msg/uavcan_parameter_request.msg Normal file
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

8
msg/uavcan_parameter_value.msg Normal file
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

1
msg/vehicle_command.msg
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_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_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_TEMPORARILY_REJECTED = 1 # Command TEMPORARY REJECTED/DENIED |

4
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

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 */

6
src/examples/hwtest/hwtest.c
View File

@ -57,8 +57,10 @@ __EXPORT 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("(run <commander stop> to do so)");
warnx("DO NOT FORGET TO STOP THE DEFAULT CONTROL APPS!");
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");
struct actuator_controls_s actuators;

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) {

1
src/modules/commander/commander.cpp
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_SET_SENSOR_OFFSETS:
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_1:
case vehicle_command_s::VEHICLE_CMD_CUSTOM_2:

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;

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

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

@ -1194,7 +1194,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);
@ -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,
(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],
@ -1338,7 +1339,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]) + 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];
@ -1414,21 +1415,21 @@ void AttitudePositionEstimatorEKF::pollData()
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("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->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.accelerometer_integral_m_s[0]),
(double)(_sensor_combined.accelerometer_integral_m_s[1]),
(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.accelerometer_m_s2[0] * 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")) {
PX4_INFO("running");
estimator::g_estimator->print_status();
return 0;

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

@ -38,7 +38,6 @@ px4_add_module(
-Os
SRCS
fw_att_control_main.cpp
fw_att_control_params.c
DEPENDS
platforms__common
)

5
src/modules/fw_att_control/fw_att_control_params.c
View File

@ -40,11 +40,6 @@
* @author Thomas Gubler <thomas@px4.io>
*/
#include <px4_config.h>
#include <systemlib/param/param.h>
/*
* Controller parameters, accessible via MAVLink
*

2
src/modules/fw_pos_control_l1/CMakeLists.txt
View File

@ -39,11 +39,9 @@ px4_add_module(
-Os
SRCS
fw_pos_control_l1_main.cpp
fw_pos_control_l1_params.c
landingslope.cpp
mtecs/mTecs.cpp
mtecs/limitoverride.cpp
mtecs/mTecs_params.c
DEPENDS
platforms__common
lib__external_lgpl

3
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
View File

@ -39,9 +39,6 @@
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
#include <px4_config.h>
#include <systemlib/param/param.h>
/*
* Controller parameters, accessible via MAVLink
*/

3
src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
View File

@ -39,9 +39,6 @@
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include <px4_config.h>
#include <systemlib/param/param.h>
/*
* Controller parameters, accessible via MAVLink
*/

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

@ -41,27 +41,29 @@
#include <stdio.h>
#include <uORB/topics/uavcan_parameter_request.h>
#include <uORB/topics/uavcan_parameter_value.h>
#include "mavlink_parameters.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"
MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkStream(mavlink),
_send_all_index(-1),
_rc_param_map_pub(nullptr),
_rc_param_map()
_rc_param_map(),
_uavcan_parameter_request_pub(nullptr),
_uavcan_parameter_value_sub(-1)
{
}
unsigned
MavlinkParametersManager::get_size()
{
if (_send_all_index >= 0) {
return MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
} else {
return 0;
}
return MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}
void
@ -78,36 +80,75 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
_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;
}
case MAVLINK_MSG_ID_PARAM_SET: {
/* set parameter */
if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) {
mavlink_param_set_t set;
mavlink_msg_param_set_decode(msg, &set);
mavlink_param_set_t set;
mavlink_msg_param_set_decode(msg, &set);
if (set.target_system == mavlink_system.sysid &&
(set.target_component == mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) {
if (set.target_system == mavlink_system.sysid &&
(set.target_component == mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) {
/* local name buffer to enforce null-terminated string */
char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
strncpy(name, set.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, set and send it */
param_t param = param_find_no_notification(name);
/* local name buffer to enforce null-terminated string */
char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
strncpy(name, set.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, set and send it */
param_t param = param_find_no_notification(name);
if (param == PARAM_INVALID) {
char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
sprintf(buf, "[pm] unknown param: %s", name);
_mavlink->send_statustext_info(buf);
if (param == PARAM_INVALID) {
char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
sprintf(buf, "[pm] unknown param: %s", name);
_mavlink->send_statustext_info(buf);
} else {
/* set and send parameter */
param_set(param, &(set.param_value));
send_param(param);
}
} else {
/* set and send parameter */
param_set(param, &(set.param_value));
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;
@ -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;
}
@ -208,11 +266,38 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
void
MavlinkParametersManager::send(const hrt_abstime t)
{
/* send all parameters if requested, but only after the system has booted */
if (_send_all_index >= 0 && _mavlink->boot_complete()) {
bool space_available = _mavlink->get_free_tx_buf() >= get_size();
/* 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 */
if (_mavlink->get_free_tx_buf() < get_size()) {
if (!space_available) {
return;
}

3
src/modules/mavlink/mavlink_parameters.h
View File

@ -118,4 +118,7 @@ protected:
orb_advert_t _rc_param_map_pub;
struct rc_parameter_map_s _rc_param_map;
orb_advert_t _uavcan_parameter_request_pub;
int _uavcan_parameter_value_sub;
};

8
src/modules/mavlink/mavlink_receiver.cpp
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[1] = imu.yacc / 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[1] = ((imu.yacc * dt + _hil_prev_accel[1]) / 2.0f) / dt;
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[0] = imu.xacc;
hil_sensors.accelerometer_m_s2[1] = imu.yacc;
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[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));
hil_sensors.accelerometer_integral_dt[0] = dt * 1e6f;
hil_sensors.accelerometer_mode[0] = 0; // TODO what is this?

53
src/modules/mavlink/mavlink_tests/module.mk
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

7
src/modules/navigator/CMakeLists.txt
View File

@ -39,24 +39,17 @@ px4_add_module(
-Os
SRCS
navigator_main.cpp
navigator_params.c
navigator_mode.cpp
mission_block.cpp
mission.cpp
mission_params.c
loiter.cpp
rtl.cpp
rtl_params.c
mission_feasibility_checker.cpp
geofence.cpp
geofence_params.c
datalinkloss.cpp
datalinkloss_params.c
rcloss.cpp
rcloss_params.c
enginefailure.cpp
gpsfailure.cpp
gpsfailure_params.c
DEPENDS
platforms__common
)

4
src/modules/navigator/datalinkloss_params.c
View File

@ -39,10 +39,6 @@
* @author Thomas Gubler <thomas@px4.io>
*/
#include <px4_config.h>
#include <systemlib/param/param.h>
/*
* Data Link Loss parameters, accessible via MAVLink
*/

4
src/modules/navigator/geofence_params.c
View File

@ -39,10 +39,6 @@
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include <px4_config.h>
#include <systemlib/param/param.h>
/*
* Geofence parameters, accessible via MAVLink
*/

4
src/modules/navigator/gpsfailure_params.c
View File

@ -39,10 +39,6 @@
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include <px4_config.h>
#include <systemlib/param/param.h>
/*
* GPS Failure Navigation Mode parameters, accessible via MAVLink
*/

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;
}

4
src/modules/navigator/mission_params.c
View File

@ -39,10 +39,6 @@
* @author Julian Oes <joes@student.ethz.ch>
*/
#include <px4_config.h>
#include <systemlib/param/param.h>
/*
* Mission parameters, accessible via MAVLink
*/

4
src/modules/navigator/navigator_params.c
View File

@ -40,10 +40,6 @@
* @author Thomas Gubler <thomas@px4.io>
*/
#include <px4_config.h>
#include <systemlib/param/param.h>
/**
* Loiter radius (FW only)
*

4
src/modules/navigator/rcloss_params.c
View File

@ -39,10 +39,6 @@
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include <px4_config.h>
#include <systemlib/param/param.h>
/*
* OBC RC Loss mode parameters, accessible via MAVLink
*/

4
src/modules/navigator/rtl_params.c
View File

@ -39,10 +39,6 @@
* @author Julian Oes <julian@oes.ch>
*/
#include <px4_config.h>
#include <systemlib/param/param.h>
/*
* RTL parameters, accessible via MAVLink
*/

2
src/modules/position_estimator_inav/position_estimator_inav_main.c
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;
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;
/* increase EPH/EPV on each step */

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
*/
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],
&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));

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

@ -63,7 +63,7 @@ px4_add_module(
COMPILE_FLAGS
-Wframe-larger-than=1500
-Wno-deprecated-declarations
-O3
-Os
SRCS
# Main
uavcan_main.cpp
@ -81,7 +81,7 @@ px4_add_module(
DEPENDS
platforms__common
uavcan
uavcan
)
## vim: set noet ft=cmake fenc=utf-8 ff=unix :

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;

856
src/modules/uavcan/uavcan_servers.cpp
View File

@ -36,6 +36,7 @@
#include <cstdlib>
#include <cstring>
#include <fcntl.h>
#include <dirent.h>
#include <memory>
#include <pthread.h>
#include <systemlib/err.h>
@ -53,14 +54,22 @@
#include "uavcan_servers.hpp"
#include "uavcan_virtual_can_driver.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/firmware_version_checker.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/firmware_version_checker.hpp>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/uavcan_parameter_request.h>
#include <uORB/topics/uavcan_parameter_value.h>
#include <v1.0/common/mavlink.h>
//todo:The Inclusion of file_server_backend is killing
// #include <sys/types.h> and leaving OK undefined
# define OK 0
/**
* @file uavcan_servers.cpp
*
@ -87,8 +96,29 @@ UavcanServers::UavcanServers(uavcan::INode &main_node) :
_node_info_retriever(_subnode),
_fw_upgrade_trigger(_subnode, _fw_version_checker),
_fw_server(_subnode, _fileserver_backend),
_count_in_progress(false),
_count_index(0),
_param_in_progress(0),
_param_index(0),
_param_list_in_progress(false),
_param_list_all_nodes(false),
_param_list_node_id(1),
_param_dirty_bitmap{0, 0, 0, 0},
_param_save_opcode(0),
_cmd_in_progress(false),
_param_response_pub(nullptr),
_param_getset_client(_subnode),
_param_opcode_client(_subnode),
_param_restartnode_client(_subnode),
_mutex_inited(false),
_check_fw(false)
_check_fw(false),
_esc_enumeration_active(false),
_esc_enumeration_index(0),
_beep_pub(_subnode),
_enumeration_indication_sub(_subnode),
_enumeration_client(_subnode),
_enumeration_getset_client(_subnode),
_enumeration_save_client(_subnode)
{
}
@ -270,13 +300,40 @@ int UavcanServers::init()
return OK;
}
__attribute__((optimize("-O0")))
pthread_addr_t UavcanServers::run(pthread_addr_t)
{
prctl(PR_SET_NAME, "uavcan fw srv", 0);
Lock lock(_subnode_mutex);
/*
Copy any firmware bundled in the ROMFS to the appropriate location on the
SD card, unless the user has copied other firmware for that device.
*/
unpackFwFromROMFS(UAVCAN_FIRMWARE_PATH, UAVCAN_ROMFS_FW_PATH);
/* the subscribe call needs to happen in the same thread,
* so not in the constructor */
int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
int param_request_sub = orb_subscribe(ORB_ID(uavcan_parameter_request));
int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
/* Set up shared service clients */
_param_getset_client.setCallback(GetSetCallback(this, &UavcanServers::cb_getset));
_param_opcode_client.setCallback(ExecuteOpcodeCallback(this, &UavcanServers::cb_opcode));
_param_restartnode_client.setCallback(RestartNodeCallback(this, &UavcanServers::cb_restart));
_enumeration_client.setCallback(EnumerationBeginCallback(this, &UavcanServers::cb_enumeration_begin));
_enumeration_indication_sub.start(EnumerationIndicationCallback(this, &UavcanServers::cb_enumeration_indication));
_enumeration_getset_client.setCallback(GetSetCallback(this, &UavcanServers::cb_enumeration_getset));
_enumeration_save_client.setCallback(ExecuteOpcodeCallback(this, &UavcanServers::cb_enumeration_save));
_count_in_progress = _param_in_progress = _param_list_in_progress = _cmd_in_progress = _param_list_all_nodes = false;
memset(_param_counts, 0, sizeof(_param_counts));
_esc_enumeration_active = false;
memset(_esc_enumeration_ids, 0, sizeof(_esc_enumeration_ids));
_esc_enumeration_index = 0;
while (1) {
if (_check_fw == true) {
@ -284,14 +341,799 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
_node_info_retriever.invalidateAll();
}
const int spin_res = _subnode.spin(uavcan::MonotonicDuration::fromMSec(100));
const int spin_res = _subnode.spin(uavcan::MonotonicDuration::fromMSec(10));
if (spin_res < 0) {
warnx("node spin error %i", spin_res);
}
// Check for parameter requests (get/set/list)
bool param_request_ready;
orb_check(param_request_sub, &param_request_ready);
if (param_request_ready && !_param_list_in_progress && !_param_in_progress && !_count_in_progress) {
struct uavcan_parameter_request_s request;
orb_copy(ORB_ID(uavcan_parameter_request), param_request_sub, &request);
if (_param_counts[request.node_id]) {
/*
* We know how many parameters are exposed by this node, so
* process the request.
*/
if (request.message_type == MAVLINK_MSG_ID_PARAM_REQUEST_READ) {
uavcan::protocol::param::GetSet::Request req;
if (request.param_index >= 0) {
req.index = request.param_index;
} else {
req.name = (char*)request.param_id;
}
int call_res = _param_getset_client.call(request.node_id, req);
if (call_res < 0) {
warnx("UAVCAN command bridge: couldn't send GetSet: %d", call_res);
} else {
_param_in_progress = true;
_param_index = request.param_index;
}
} else if (request.message_type == MAVLINK_MSG_ID_PARAM_SET) {
uavcan::protocol::param::GetSet::Request req;
if (request.param_index >= 0) {
req.index = request.param_index;
} else {
req.name = (char*)request.param_id;
}
if (request.param_type == MAV_PARAM_TYPE_REAL32) {
req.value.to<uavcan::protocol::param::Value::Tag::real_value>() = request.real_value;
} else if (request.param_type == MAV_PARAM_TYPE_UINT8) {
req.value.to<uavcan::protocol::param::Value::Tag::boolean_value>() = request.int_value;
} else {
req.value.to<uavcan::protocol::param::Value::Tag::integer_value>() = request.int_value;
}
// Set the dirty bit for this node
set_node_params_dirty(request.node_id);
int call_res = _param_getset_client.call(request.node_id, req);
if (call_res < 0) {
warnx("UAVCAN command bridge: couldn't send GetSet: %d", call_res);
} else {
_param_in_progress = true;
_param_index = request.param_index;
}
} else if (request.message_type == MAVLINK_MSG_ID_PARAM_REQUEST_LIST) {
// This triggers the _param_list_in_progress case below.
_param_index = 0;
_param_list_in_progress = true;
_param_list_node_id = request.node_id;
_param_list_all_nodes = false;
warnx("UAVCAN command bridge: starting component-specific param list");
}
} else if (request.node_id == MAV_COMP_ID_ALL) {
if (request.message_type == MAVLINK_MSG_ID_PARAM_REQUEST_LIST) {
/*
* This triggers the _param_list_in_progress case below,
* but additionally iterates over all active nodes.
*/
_param_index = 0;
_param_list_in_progress = true;
_param_list_node_id = get_next_active_node_id(1);
_param_list_all_nodes = true;
warnx("UAVCAN command bridge: starting global param list with node %hhu", _param_list_node_id);
if (_param_counts[_param_list_node_id] == 0) {
param_count(_param_list_node_id);
}
}
} else {
/*
* Need to know how many parameters this node has before we can
* continue; count them now and then process the request.
*/
param_count(request.node_id);
}
}
// Handle parameter listing index/node ID advancement
if (_param_list_in_progress && !_param_in_progress && !_count_in_progress) {
if (_param_index >= _param_counts[_param_list_node_id]) {
warnx("UAVCAN command bridge: completed param list for node %hhu", _param_list_node_id);
// Reached the end of the current node's parameter set.
_param_list_in_progress = false;
if (_param_list_all_nodes) {
// We're listing all parameters for all nodes -- get the next node ID
uint8_t next_id = get_next_active_node_id(_param_list_node_id);
if (next_id < 128) {
_param_list_node_id = next_id;
/*
* If there is a next node ID, check if that node's parameters
* have been counted before. If not, do it now.
*/
if (_param_counts[_param_list_node_id] == 0) {
param_count(_param_list_node_id);
}
// Keep on listing.
_param_index = 0;
_param_list_in_progress = true;
warnx("UAVCAN command bridge: started param list for node %hhu", _param_list_node_id);
}
}
}
}
// Check if we're still listing, and need to get the next parameter
if (_param_list_in_progress && !_param_in_progress && !_count_in_progress) {
// Ready to request the next value -- _param_index is incremented
// after each successful fetch by cb_getset
uavcan::protocol::param::GetSet::Request req;
req.index = _param_index;
int call_res = _param_getset_client.call(_param_list_node_id, req);
if (call_res < 0) {
_param_list_in_progress = false;
warnx("UAVCAN command bridge: couldn't send param list GetSet: %d", call_res);
} else {
_param_in_progress = true;
}
}
// Check for ESC enumeration commands
bool cmd_ready;
orb_check(cmd_sub, &cmd_ready);
if (cmd_ready && !_cmd_in_progress) {
struct vehicle_command_s cmd;
orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_UAVCAN) {
int command_id = static_cast<int>(cmd.param1 + 0.5f);
int node_id = static_cast<int>(cmd.param2 + 0.5f);
int call_res;
warnx("UAVCAN command bridge: received UAVCAN command ID %d, node ID %d", command_id, node_id);
switch (command_id) {
case 0:
case 1: {
_esc_enumeration_active = command_id;
_esc_enumeration_index = 0;
_esc_count = 0;
uavcan::protocol::enumeration::Begin::Request req;
req.parameter_name = "esc_index";
req.timeout_sec = _esc_enumeration_active ? 65535 : 0;
call_res = _enumeration_client.call(get_next_active_node_id(1), req);
if (call_res < 0) {
warnx("UAVCAN ESC enumeration: couldn't send initial Begin request: %d", call_res);
}
break;
}
default: {
warnx("UAVCAN command bridge: unknown command ID %d", command_id);
break;
}
}
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE) {
int command_id = static_cast<int>(cmd.param1 + 0.5f);
warnx("UAVCAN command bridge: received storage command ID %d", command_id);
switch (command_id) {
case 1: {
// Param save request
_param_save_opcode = uavcan::protocol::param::ExecuteOpcode::Request::OPCODE_SAVE;
param_opcode(get_next_dirty_node_id(1));
break;
}
case 2: {
// Command is a param erase request -- apply it to all active nodes by setting the dirty bit
_param_save_opcode = uavcan::protocol::param::ExecuteOpcode::Request::OPCODE_ERASE;
for (int i = 1; i < 128; i = get_next_active_node_id(i)) {
set_node_params_dirty(i);
}
param_opcode(get_next_dirty_node_id(1));
break;
}
}
}
}
// Shut down once armed
// TODO (elsewhere): start up again once disarmed?
bool updated;
orb_check(armed_sub, &updated);
if (updated) {
struct actuator_armed_s armed;
orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
if (armed.armed && !armed.lockdown) {
warnx("UAVCAN command bridge: system armed, exiting now.");
break;
}
}
}
warnx("exiting.");
return (pthread_addr_t) 0;
}
void UavcanServers::cb_getset(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result)
{
if (_count_in_progress) {
/*
* Currently in parameter count mode:
* Iterate over all parameters for the node to which the request was
* originally sent, in order to find the maximum parameter ID. If a
* request fails, set the node's parameter count to zero.
*/
uint8_t node_id = result.getCallID().server_node_id.get();
if (result.isSuccessful()) {
uavcan::protocol::param::GetSet::Response resp = result.getResponse();
if (resp.name.size()) {
_param_counts[node_id] = _count_index++;
uavcan::protocol::param::GetSet::Request req;
req.index = _count_index;
int call_res = _param_getset_client.call(result.getCallID().server_node_id, req);
if (call_res < 0) {
_count_in_progress = false;
_count_index = 0;
warnx("UAVCAN command bridge: couldn't send GetSet during param count: %d", call_res);
}
} else {
_count_in_progress = false;
_count_index = 0;
warnx("UAVCAN command bridge: completed param count for node %hhu: %hhu", node_id, _param_counts[node_id]);
}
} else {
_param_counts[node_id] = 0;
_count_in_progress = false;
_count_index = 0;
warnx("UAVCAN command bridge: GetSet error during param count");
}
} else {
/*
* Currently in parameter get/set mode:
* Publish a uORB uavcan_parameter_value message containing the current value
* of the parameter.
*/
if (result.isSuccessful()) {
uavcan::protocol::param::GetSet::Response param = result.getResponse();
struct uavcan_parameter_value_s response;
response.node_id = result.getCallID().server_node_id.get();
strncpy(response.param_id, param.name.c_str(), sizeof(response.param_id) - 1);
response.param_id[16] = '\0';
response.param_index = _param_index;
response.param_count = _param_counts[response.node_id];
if (param.value.is(uavcan::protocol::param::Value::Tag::integer_value)) {
response.param_type = MAV_PARAM_TYPE_INT64;
response.int_value = param.value.to<uavcan::protocol::param::Value::Tag::integer_value>();
} else if (param.value.is(uavcan::protocol::param::Value::Tag::real_value)) {
response.param_type = MAV_PARAM_TYPE_REAL32;
response.real_value = param.value.to<uavcan::protocol::param::Value::Tag::real_value>();
} else if (param.value.is(uavcan::protocol::param::Value::Tag::boolean_value)) {
response.param_type = MAV_PARAM_TYPE_UINT8;
response.int_value = param.value.to<uavcan::protocol::param::Value::Tag::boolean_value>();
}
if (_param_response_pub == nullptr) {
_param_response_pub = orb_advertise(ORB_ID(uavcan_parameter_value), &response);
} else {
orb_publish(ORB_ID(uavcan_parameter_value), _param_response_pub, &response);
}
} else {
warnx("UAVCAN command bridge: GetSet error");
}
_param_in_progress = false;
_param_index++;
}
}
void UavcanServers::param_count(uavcan::NodeID node_id)
{
uavcan::protocol::param::GetSet::Request req;
req.index = 0;
int call_res = _param_getset_client.call(node_id, req);
if (call_res < 0) {
warnx("UAVCAN command bridge: couldn't start parameter count: %d", call_res);
} else {
_count_in_progress = true;
_count_index = 0;
warnx("UAVCAN command bridge: starting param count");
}
}
void UavcanServers::param_opcode(uavcan::NodeID node_id)
{
uavcan::protocol::param::ExecuteOpcode::Request opcode_req;
opcode_req.opcode = _param_save_opcode;
int call_res = _param_opcode_client.call(node_id, opcode_req);
if (call_res < 0) {
warnx("UAVCAN command bridge: couldn't send ExecuteOpcode: %d", call_res);
} else {
_cmd_in_progress = true;
warnx("UAVCAN command bridge: sent ExecuteOpcode");
}
}
void UavcanServers::cb_opcode(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result)
{
bool success = result.isSuccessful();
uint8_t node_id = result.getCallID().server_node_id.get();
uavcan::protocol::param::ExecuteOpcode::Response resp = result.getResponse();
success &= resp.ok;
_cmd_in_progress = false;
if (!result.isSuccessful()) {
warnx("UAVCAN command bridge: save request for node %hhu timed out.", node_id);
} else if (!result.getResponse().ok) {
warnx("UAVCAN command bridge: save request for node %hhu rejected.", node_id);
} else {
warnx("UAVCAN command bridge: save request for node %hhu completed OK, restarting.", node_id);
uavcan::protocol::RestartNode::Request restart_req;
restart_req.magic_number = restart_req.MAGIC_NUMBER;
int call_res = _param_restartnode_client.call(node_id, restart_req);
if (call_res < 0) {
warnx("UAVCAN command bridge: couldn't send RestartNode: %d", call_res);
} else {
warnx("UAVCAN command bridge: sent RestartNode");
_cmd_in_progress = true;
}
}
if (!_cmd_in_progress) {
/*
* Something went wrong, so cb_restart is never going to be called as a result of this request.
* To ensure we try to execute the opcode on all nodes that permit it, get the next dirty node
* ID and keep processing here. The dirty bit on the current node is still set, so the
* save/erase attempt will occur when the next save/erase command is received over MAVLink.
*/
node_id = get_next_dirty_node_id(node_id);
if (node_id < 128) {
param_opcode(node_id);
}
}
}
void UavcanServers::cb_restart(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &result)
{
bool success = result.isSuccessful();
uint8_t node_id = result.getCallID().server_node_id.get();
uavcan::protocol::RestartNode::Response resp = result.getResponse();
success &= resp.ok;
_cmd_in_progress = false;
if (success) {
warnx("UAVCAN command bridge: restart request for node %hhu completed OK.", node_id);
// Clear the dirty flag
clear_node_params_dirty(node_id);
} else {
warnx("UAVCAN command bridge: restart request for node %hhu failed.", node_id);
}
// Get the next dirty node ID and send the same command to it
node_id = get_next_dirty_node_id(node_id);
if (node_id < 128) {
param_opcode(node_id);
}
}
uint8_t UavcanServers::get_next_active_node_id(uint8_t base)
{
base++;
for (; base < 128 && (!_node_info_retriever.isNodeKnown(base) ||
_subnode.getNodeID().get() == base); base++);
return base;
}
uint8_t UavcanServers::get_next_dirty_node_id(uint8_t base)
{
base++;
for (; base < 128 && !are_node_params_dirty(base); base++);
return base;
}
void UavcanServers::cb_enumeration_begin(const uavcan::ServiceCallResult<uavcan::protocol::enumeration::Begin> &result)
{
uint8_t next_id = get_next_active_node_id(result.getCallID().server_node_id.get());
if (!result.isSuccessful()) {
warnx("UAVCAN ESC enumeration: begin request for node %hhu timed out.", result.getCallID().server_node_id.get());
} else if (result.getResponse().error) {
warnx("UAVCAN ESC enumeration: begin request for node %hhu rejected: %hhu", result.getCallID().server_node_id.get(), result.getResponse().error);
} else {
_esc_count++;
warnx("UAVCAN ESC enumeration: begin request for node %hhu completed OK.", result.getCallID().server_node_id.get());
}
if (next_id < 128) {
// Still other active nodes to send the request to
uavcan::protocol::enumeration::Begin::Request req;
req.parameter_name = "esc_index";
req.timeout_sec = _esc_enumeration_active ? 65535 : 0;
int call_res = _enumeration_client.call(next_id, req);
if (call_res < 0) {
warnx("UAVCAN ESC enumeration: couldn't send Begin request: %d", call_res);
} else {
warnx("UAVCAN ESC enumeration: sent Begin request");
}
} else {
warnx("UAVCAN ESC enumeration: begun enumeration on all nodes.");
}
}
void UavcanServers::cb_enumeration_indication(const uavcan::ReceivedDataStructure<uavcan::protocol::enumeration::Indication> &msg)
{
// Called whenever an ESC thinks it has received user input.
warnx("UAVCAN ESC enumeration: got indication");
if (!_esc_enumeration_active) {
// Ignore any messages received when we're not expecting them
return;
}
// First, check if we've already seen an indication from this ESC. If so,
// just re-issue the previous get/set request.
int i;
for (i = 0; i < _esc_enumeration_index; i++) {
if (_esc_enumeration_ids[i] == msg.getSrcNodeID().get()) {
warnx("UAVCAN ESC enumeration: already enumerated ESC ID %hhu as index %d", _esc_enumeration_ids[i], i);
break;
}
}
uavcan::protocol::param::GetSet::Request req;
req.name = "esc_index";
req.value.to<uavcan::protocol::param::Value::Tag::integer_value>() = i;
int call_res = _enumeration_getset_client.call(msg.getSrcNodeID(), req);
if (call_res < 0) {
warnx("UAVCAN ESC enumeration: couldn't send GetSet: %d", call_res);
} else {
warnx("UAVCAN ESC enumeration: sent GetSet to node %hhu (index %d)", _esc_enumeration_ids[i], i);
}
}
void UavcanServers::cb_enumeration_getset(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result)
{
if (!result.isSuccessful()) {
warnx("UAVCAN ESC enumeration: save request for node %hhu timed out.", result.getCallID().server_node_id.get());
} else {
warnx("UAVCAN ESC enumeration: save request for node %hhu completed OK.", result.getCallID().server_node_id.get());
uavcan::protocol::param::GetSet::Response resp = result.getResponse();
uint8_t esc_index = (uint8_t)resp.value.to<uavcan::protocol::param::Value::Tag::integer_value>();
esc_index = std::min((uint8_t)(uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize - 1), esc_index);
_esc_enumeration_index = std::max(_esc_enumeration_index, (uint8_t)(esc_index + 1));
_esc_enumeration_ids[esc_index] = result.getCallID().server_node_id.get();
uavcan::protocol::param::ExecuteOpcode::Request opcode_req;
opcode_req.opcode = opcode_req.OPCODE_SAVE;
int call_res = _enumeration_save_client.call(result.getCallID().server_node_id, opcode_req);
if (call_res < 0) {
warnx("UAVCAN ESC enumeration: couldn't send ExecuteOpcode: %d", call_res);
} else {
warnx("UAVCAN ESC enumeration: sent ExecuteOpcode to node %hhu (index %hhu)", _esc_enumeration_ids[esc_index], esc_index);
}
}
}
void UavcanServers::cb_enumeration_save(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result)
{
uavcan::equipment::indication::BeepCommand beep;
if (!result.isSuccessful()) {
warnx("UAVCAN ESC enumeration: save request for node %hhu timed out.", result.getCallID().server_node_id.get());
beep.frequency = 880.0f;
beep.duration = 1.0f;
} else if (!result.getResponse().ok) {
warnx("UAVCAN ESC enumeration: save request for node %hhu rejected", result.getCallID().server_node_id.get());
beep.frequency = 880.0f;
beep.duration = 1.0f;
} else {
warnx("UAVCAN ESC enumeration: save request for node %hhu completed OK.", result.getCallID().server_node_id.get());
beep.frequency = 440.0f;
beep.duration = 0.25f;
}
(void)_beep_pub.broadcast(beep);
warnx("UAVCAN ESC enumeration: completed %hhu of %hhu", _esc_enumeration_index, _esc_count);
if (_esc_enumeration_index == uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize - 1 ||
_esc_enumeration_index == _esc_count) {
_esc_enumeration_active = false;
// Tell all ESCs to stop enumerating
uavcan::protocol::enumeration::Begin::Request req;
req.parameter_name = "esc_index";
req.timeout_sec = 0;
int call_res = _enumeration_client.call(get_next_active_node_id(1), req);
if (call_res < 0) {
warnx("UAVCAN ESC enumeration: couldn't send Begin request to stop enumeration: %d", call_res);
} else {
warnx("UAVCAN ESC enumeration: sent Begin request to stop enumeration");
}
}
}
void UavcanServers::unpackFwFromROMFS(const char* sd_path, const char* romfs_path) {
/*
Copy the ROMFS firmware directory to the appropriate location on SD, without
overriding any firmware the user has already loaded there.
The SD firmware directory structure is along the lines of:
/fs/microsd/fw
- /c
- - /1a2b3c4d.bin cache file (copy of /fs/microsd/fw/org.pixhawk.nodename-v1/1.1/1a2b3c4d.bin)
- /org.pixhawk.nodename-v1 device directory for org.pixhawk.nodename-v1
- - /1.0 version directory for hardware 1.0
- - /1.1 version directory for hardware 1.1
- - - /1a2b3c4d.bin firmware file for org.pixhawk.nodename-v1 nodes, hardware version 1.1
- /com.example.othernode-v3 device directory for com.example.othernode-v3
- - /1.0 version directory for hardawre 1.0
- - - /deadbeef.bin firmware file for com.example.othernode-v3, hardware version 1.0
The ROMFS directory structure is the same, but located at /etc/uavcan/fw
We iterate over all device directories in the ROMFS base directory, and create
corresponding device directories on the SD card if they don't already exist.
In each device directory, we iterate over each version directory and create a
corresponding version directory on the SD card if it doesn't already exist.
In each version directory, we remove any files with a name starting with "romfs_"
in the corresponding directory on the SD card that don't match the bundled firmware
filename; if the directory is empty after that process, we copy the bundled firmware.
*/
const size_t maxlen = UAVCAN_MAX_PATH_LENGTH;
const size_t sd_path_len = strlen(sd_path);
const size_t romfs_path_len = strlen(romfs_path);
struct stat sb;
int rv;
char dstpath[maxlen + 1];
char srcpath[maxlen + 1];
DIR* const romfs_dir = opendir(romfs_path);
if (!romfs_dir) {
warnx("base: couldn't open %s", romfs_path);
return;
}
memcpy(dstpath, sd_path, sd_path_len + 1);
memcpy(srcpath, romfs_path, romfs_path_len + 1);
// Iterate over all device directories in ROMFS
struct dirent* dev_dirent = NULL;
while ((dev_dirent = readdir(romfs_dir)) != NULL) {
// Skip if not a directory
if (!DIRENT_ISDIRECTORY(dev_dirent->d_type)) {
continue;
}
// Make sure the path fits
size_t dev_dirname_len = strlen(dev_dirent->d_name);
size_t srcpath_dev_len = romfs_path_len + 1 + dev_dirname_len;
if (srcpath_dev_len > maxlen) {
warnx("dev: srcpath '%s/%s' too long", romfs_path, dev_dirent->d_name);
continue;
}
size_t dstpath_dev_len = sd_path_len + 1 + dev_dirname_len;
if (dstpath_dev_len > maxlen) {
warnx("dev: dstpath '%s/%s' too long", sd_path, dev_dirent->d_name);
continue;
}
// Create the device name directory on the SD card if it doesn't already exist
dstpath[sd_path_len] = '/';
memcpy(&dstpath[sd_path_len + 1], dev_dirent->d_name, dev_dirname_len + 1);
if (stat(dstpath, &sb) != 0 || !S_ISDIR(sb.st_mode)) {
rv = mkdir(dstpath, S_IRWXU | S_IRWXG | S_IRWXO);
if (rv != 0) {
warnx("dev: couldn't create '%s'", dstpath);
continue;
}
}
// Set up the source path
srcpath[romfs_path_len] = '/';
memcpy(&srcpath[romfs_path_len + 1], dev_dirent->d_name, dev_dirname_len + 1);
DIR* const dev_dir = opendir(srcpath);
if (!dev_dir) {
warnx("dev: couldn't open '%s'", srcpath);
continue;
}
// Iterate over all version directories in the current ROMFS device directory
struct dirent* ver_dirent = NULL;
while ((ver_dirent = readdir(dev_dir)) != NULL) {
// Skip if not a directory
if (!DIRENT_ISDIRECTORY(ver_dirent->d_type)) {
continue;
}
// Make sure the path fits
size_t ver_dirname_len = strlen(ver_dirent->d_name);
size_t srcpath_ver_len = srcpath_dev_len + 1 + ver_dirname_len;
if (srcpath_ver_len > maxlen) {
warnx("ver: srcpath '%s/%s' too long", srcpath, ver_dirent->d_name);
continue;
}
size_t dstpath_ver_len = dstpath_dev_len + 1 + ver_dirname_len;
if (dstpath_ver_len > maxlen) {
warnx("ver: dstpath '%s/%s' too long", dstpath, ver_dirent->d_name);
continue;
}
// Create the device version directory on the SD card if it doesn't already exist
dstpath[dstpath_dev_len] = '/';
memcpy(&dstpath[dstpath_dev_len + 1], ver_dirent->d_name, ver_dirname_len + 1);
if (stat(dstpath, &sb) != 0 || !S_ISDIR(sb.st_mode)) {
rv = mkdir(dstpath, S_IRWXU | S_IRWXG | S_IRWXO);
if (rv != 0) {
warnx("ver: couldn't create '%s'", dstpath);
continue;
}
}
// Set up the source path
srcpath[srcpath_dev_len] = '/';
memcpy(&srcpath[srcpath_dev_len + 1], ver_dirent->d_name, ver_dirname_len + 1);
// Find the name of the bundled firmware file, or move on to the
// next directory if there's no file here.
DIR* const src_ver_dir = opendir(srcpath);
if (!src_ver_dir) {
warnx("ver: couldn't open '%s'", srcpath);
continue;
}
struct dirent* src_fw_dirent = NULL;
while ((src_fw_dirent = readdir(src_ver_dir)) != NULL &&
!DIRENT_ISFILE(src_fw_dirent->d_type));
if (!src_fw_dirent) {
(void)closedir(src_ver_dir);
continue;
}
size_t fw_len = strlen(src_fw_dirent->d_name);
bool copy_fw = true;
// Clear out any romfs_ files in the version directory on the SD card
DIR* const dst_ver_dir = opendir(dstpath);
if (!dst_ver_dir) {
warnx("unlink: couldn't open '%s'", dstpath);
} else {
struct dirent* fw_dirent = NULL;
while ((fw_dirent = readdir(dst_ver_dir)) != NULL) {
// Skip if not a file
if (!DIRENT_ISFILE(fw_dirent->d_type)) {
continue;
}
if (!memcmp(&fw_dirent->d_name[sizeof(UAVCAN_ROMFS_FW_PREFIX) - 1], src_fw_dirent->d_name, fw_len)) {
/*
* Exact match between SD card filename and ROMFS filename; must be the same version
* so don't bother deleting and rewriting it.
*/
copy_fw = false;
} else if (!memcmp(fw_dirent->d_name, UAVCAN_ROMFS_FW_PREFIX, sizeof(UAVCAN_ROMFS_FW_PREFIX) - 1)) {
size_t fw_len = strlen(fw_dirent->d_name);
size_t dstpath_fw_len = dstpath_ver_len + sizeof(UAVCAN_ROMFS_FW_PREFIX) + fw_len;
if (dstpath_fw_len > maxlen) {
// sizeof(prefix) includes trailing NUL, cancelling out the +1 for the path separator
warnx("unlink: path '%s/%s%s' too long", dstpath, UAVCAN_ROMFS_FW_PREFIX, fw_dirent->d_name);
} else {
// File name starts with "romfs_", delete it.
dstpath[dstpath_ver_len] = '/';
memcpy(&dstpath[dstpath_ver_len + 1], fw_dirent->d_name, fw_len + 1);
unlink(dstpath);
warnx("unlink: removed '%s/%s%s'", dstpath, UAVCAN_ROMFS_FW_PREFIX, fw_dirent->d_name);
}
} else {
// User file, don't copy firmware
copy_fw = false;
}
}
(void)closedir(dst_ver_dir);
}
// If we need to, copy the file from ROMFS to the SD card
if (copy_fw) {
size_t srcpath_fw_len = srcpath_ver_len + 1 + fw_len;
size_t dstpath_fw_len = dstpath_ver_len + sizeof(UAVCAN_ROMFS_FW_PREFIX) + fw_len;
if (srcpath_fw_len > maxlen) {
warnx("copy: srcpath '%s/%s' too long", srcpath, src_fw_dirent->d_name);
} else if (dstpath_fw_len > maxlen) {
warnx("copy: dstpath '%s/%s%s' too long", dstpath, UAVCAN_ROMFS_FW_PREFIX, src_fw_dirent->d_name);
} else {
// All OK, make the paths and copy the file
srcpath[srcpath_ver_len] = '/';
memcpy(&srcpath[srcpath_ver_len + 1], src_fw_dirent->d_name, fw_len + 1);
dstpath[dstpath_ver_len] = '/';
memcpy(&dstpath[dstpath_ver_len + 1], UAVCAN_ROMFS_FW_PREFIX, sizeof(UAVCAN_ROMFS_FW_PREFIX));
memcpy(&dstpath[dstpath_ver_len + sizeof(UAVCAN_ROMFS_FW_PREFIX)], src_fw_dirent->d_name, fw_len + 1);
rv = copyFw(dstpath, srcpath);
if (rv != 0) {
warnx("copy: '%s' -> '%s' failed: %d", srcpath, dstpath, rv);
} else {
warnx("copy: '%s' -> '%s' succeeded", srcpath, dstpath);
}
}
}
(void)closedir(src_ver_dir);
}
(void)closedir(dev_dir);
}
(void)closedir(romfs_dir);
}
int UavcanServers::copyFw(const char* dst, const char* src) {
int rv = 0;
int dfd, sfd;
uint8_t buffer[512];
dfd = open(dst, O_WRONLY | O_CREAT, 0666);
if (dfd < 0) {
warnx("copyFw: couldn't open dst");
return -errno;
}
sfd = open(src, O_RDONLY, 0);
if (sfd < 0) {
(void)close(dfd);
warnx("copyFw: couldn't open src");
return -errno;
}
ssize_t size = 0;
do {
size = read(sfd, buffer, sizeof(buffer));
if (size < 0) {
warnx("copyFw: couldn't read");
rv = -errno;
} else if (size > 0) {
rv = 0;
ssize_t remaining = size;
ssize_t total_written = 0;
ssize_t written = 0;
do {
written = write(dfd, &buffer[total_written], remaining);
if (written < 0) {
warnx("copyFw: couldn't write");
rv = -errno;
} else {
total_written += written;
remaining -= written;
}
} while (written > 0 && remaining > 0);
}
} while (rv == 0 && size != 0);
(void)close(dfd);
(void)close(sfd);
return rv;
}

104
src/modules/uavcan/uavcan_servers.hpp
View File

@ -42,16 +42,20 @@
#include <uavcan/node/sub_node.hpp>
#include <uavcan/protocol/node_status_monitor.hpp>
# include <uavcan/protocol/dynamic_node_id_server/centralized.hpp>
# include <uavcan/protocol/node_info_retriever.hpp>
# include <uavcan_posix/basic_file_server_backend.hpp>
# include <uavcan/protocol/firmware_update_trigger.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_storage_backend.hpp>
# include <uavcan_posix/firmware_version_checker.hpp>
#include <uavcan/protocol/dynamic_node_id_server/centralized.hpp>
#include <uavcan/protocol/node_info_retriever.hpp>
#include <uavcan_posix/basic_file_server_backend.hpp>
#include <uavcan/protocol/firmware_update_trigger.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_storage_backend.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
@ -65,6 +69,9 @@
#define UAVCAN_DEVICE_PATH "/dev/uavcan/esc"
#define UAVCAN_NODE_DB_PATH "/fs/microsd/uavcan.db"
#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"
/**
@ -88,7 +95,7 @@ class UavcanServers
static constexpr unsigned QueuePoolSize =
(NumIfaces * uavcan::MemPoolBlockSize * MaxCanFramesPerTransfer);
static constexpr unsigned StackSize = 3500;
static constexpr unsigned StackSize = 6000;
static constexpr unsigned Priority = 120;
typedef uavcan::SubNode<MemPoolSize> SubNode;
@ -140,7 +147,84 @@ private:
uavcan::FirmwareUpdateTrigger _fw_upgrade_trigger;
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;
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);
};

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