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

Merge branch 'astyle' of github.com:dagar/Firmware

Browse Source
This commit is contained in:
Lorenz Meier
2015-09-05 22:04:15 +02:00
parent 9d5a2e9815 9fcbc1fb6f
commit b1213a64f5
33 changed files with 496 additions and 316 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.travis.yml
+4
View File
@@ -56,6 +56,9 @@ before_script:
- ln -s /usr/bin/ccache ~/bin/g++-4.8
- ln -s /usr/bin/ccache ~/bin/gcc-4.8
- export PATH=~/bin:$PATH
# grab astyle 2.05.1
- wget -O ~/bin/astyle https://github.com/PX4/astyle/releases/download/2.05.1/astyle-linux && chmod +x ~/bin/astyle
- astyle --version
env:
global:
@@ -66,6 +69,7 @@ env:
- PX4_AWS_BUCKET=px4-travis
script:
- make check_format
- ccache -z
- arm-none-eabi-gcc --version
- echo 'Building POSIX Firmware..' && echo -en 'travis_fold:start:script.1\\r'
Makefile
+2 -2
View File
@@ -222,9 +222,9 @@ qurtrun:
tests: generateuorbtopicheaders
$(Q) (mkdir -p $(PX4_BASE)/unittests/build && cd $(PX4_BASE)/unittests/build && cmake .. && $(MAKE) --no-print-directory unittests)
.PHONY: format check_format
.PHONY: check_format
check_format:
$(Q) (./Tools/check_code_style.sh | sort -n)
$(Q) (./Tools/check_code_style.sh)
#
# Cleanup targets. 'clean' should remove all built products and force
Tools/check_code_style.sh
+15 -9
View File
@@ -1,16 +1,22 @@
#!/usr/bin/env bash
set -eu
failed=0
for fn in $(find . -path './src/lib/uavcan' -prune -o \
-path './src/lib/mathlib/CMSIS' -prune -o \
-path './src/lib/eigen' -prune -o \
-path './src/modules/attitude_estimator_ekf/codegen' -prune -o \
-path './NuttX' -prune -o \
for fn in $(find src/examples \
src/systemcmds \
-path './Build' -prune -o \
-path './mavlink' -prune -o \
-path './unittests/gtest' -prune -o \
-path './NuttX' -prune -o \
-path './src/lib/eigen' -prune -o \
-path './src/lib/mathlib/CMSIS' -prune -o \
-path './src/lib/uavcan' -prune -o \
-path './src/modules/attitude_estimator_ekf/codegen' -prune -o \
-path './src/modules/ekf_att_pos_estimator' -prune -o \
-path './src/modules/sdlog2' -prune -o \
-path './src/modules/uORB' -prune -o \
-path './src/modules/vtol_att_control' -prune -o \
-path './unittests/build' -prune -o \
-name '*.c' -o -name '*.cpp' -o -name '*.hpp' -o -name '*.h' -type f); do
-path './unittests/gtest' -prune -o \
-name '*.c' -o -name '*.cpp' -o -name '*.hpp' -o -name '*.h' -not -name '*generated*' -type f); do
if [ -f "$fn" ];
then
./Tools/fix_code_style.sh --quiet < $fn > $fn.pretty
@@ -18,7 +24,7 @@ for fn in $(find . -path './src/lib/uavcan' -prune -o \
rm -f $fn.pretty
if [ $diffsize -ne 0 ]; then
failed=1
echo $diffsize $fn
echo $fn 'bad formatting, please run "./Tools/fix_code_style.sh' $fn'"'
fi
fi
done
@@ -27,6 +33,6 @@ if [ $failed -eq 0 ]; then
echo "Format checks passed"
exit 0
else
echo "Format checks failed; please run ./Tools/fix_code_style.sh on each file"
echo "Format checks failed"
exit 1
fi
Tools/fix_code_style.sh
+11 -1
View File
@@ -1,4 +1,14 @@
#!/bin/sh
#!/bin/bash
ASTYLE_VER=`astyle --version`
ASTYLE_VER_REQUIRED="Artistic Style Version 2.05.1"
if [ "$ASTYLE_VER" != "$ASTYLE_VER_REQUIRED" ]; then
echo "Error: you're using ${ASTYLE_VER}, but PX4 requires ${ASTYLE_VER_REQUIRED}"
echo "You can get the correct version here: https://github.com/PX4/astyle/releases/tag/2.05.1"
exit 1
fi
DIR=$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )
astyle \
--options=$DIR/astylerc \
src/examples/fixedwing_control/main.c
+5 -5
View File
@@ -432,11 +432,11 @@ int ex_fixedwing_control_main(int argc, char *argv[])
thread_should_exit = false;
deamon_task = px4_task_spawn_cmd("ex_fixedwing_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 20,
2048,
fixedwing_control_thread_main,
(argv) ? (char * const *)&argv[2] : (char * const *)NULL);
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 20,
2048,
fixedwing_control_thread_main,
(argv) ? (char *const *)&argv[2] : (char *const *)NULL);
thread_running = true;
exit(0);
}
src/examples/flow_position_estimator/flow_position_estimator_main.c
+5 -5
View File
@@ -112,11 +112,11 @@ int flow_position_estimator_main(int argc, char *argv[])
thread_should_exit = false;
daemon_task = px4_task_spawn_cmd("flow_position_estimator",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
4000,
flow_position_estimator_thread_main,
(argv) ? (char * const *)&argv[2] : (char * const *)NULL);
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
4000,
flow_position_estimator_thread_main,
(argv) ? (char *const *)&argv[2] : (char *const *)NULL);
exit(0);
}
src/examples/matlab_csv_serial/matlab_csv_serial.c
+5 -5
View File
@@ -104,11 +104,11 @@ int matlab_csv_serial_main(int argc, char *argv[])
thread_should_exit = false;
daemon_task = px4_task_spawn_cmd("matlab_csv_serial",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
2000,
matlab_csv_serial_thread_main,
(argv) ? (char * const *)&argv[2] : (char * const *)NULL);
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
2000,
matlab_csv_serial_thread_main,
(argv) ? (char *const *)&argv[2] : (char *const *)NULL);
exit(0);
}
src/examples/publisher/publisher_start_nuttx.cpp
+5 -5
View File
@@ -69,11 +69,11 @@ int publisher_main(int argc, char *argv[])
task_should_exit = false;
daemon_task = px4_task_spawn_cmd("publisher",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
2000,
main,
(argv) ? (char* const*)&argv[2] : (char* const*)NULL);
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
2000,
main,
(argv) ? (char *const *)&argv[2] : (char *const *)NULL);
exit(0);
}
src/examples/px4_daemon_app/px4_daemon_app.c
+5 -5
View File
@@ -103,11 +103,11 @@ int px4_daemon_app_main(int argc, char *argv[])
thread_should_exit = false;
daemon_task = px4_task_spawn_cmd("daemon",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
2000,
px4_daemon_thread_main,
(argv) ? (char * const *)&argv[2] : (char * const *)NULL);
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
2000,
px4_daemon_thread_main,
(argv) ? (char *const *)&argv[2] : (char *const *)NULL);
return 0;
}
src/examples/rover_steering_control/main.cpp
+5 -5
View File
@@ -426,11 +426,11 @@ int rover_steering_control_main(int argc, char *argv[])
thread_should_exit = false;
deamon_task = px4_task_spawn_cmd("rover_steering_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 20,
2048,
rover_steering_control_thread_main,
(argv) ? (char * const *)&argv[2] : (char * const *)NULL);
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 20,
2048,
rover_steering_control_thread_main,
(argv) ? (char *const *)&argv[2] : (char *const *)NULL);
thread_running = true;
exit(0);
}
src/examples/subscriber/subscriber_start_nuttx.cpp
+5 -5
View File
@@ -69,11 +69,11 @@ int subscriber_main(int argc, char *argv[])
task_should_exit = false;
daemon_task = px4_task_spawn_cmd("subscriber",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
2000,
main,
(argv) ? (char* const*)&argv[2] : (char* const*)NULL);
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
2000,
main,
(argv) ? (char *const *)&argv[2] : (char *const *)NULL);
exit(0);
}
src/systemcmds/bl_update/bl_update.c
+20 -10
View File
@@ -61,33 +61,40 @@ static void setopt(void);
int
bl_update_main(int argc, char *argv[])
{
if (argc != 2)
if (argc != 2) {
errx(1, "missing firmware filename or command");
}
if (!strcmp(argv[1], "setopt"))
if (!strcmp(argv[1], "setopt")) {
setopt();
}
int fd = open(argv[1], O_RDONLY);
if (fd < 0)
if (fd < 0) {
err(1, "open %s", argv[1]);
}
struct stat s;
if (stat(argv[1], &s) != 0)
if (stat(argv[1], &s) != 0) {
err(1, "stat %s", argv[1]);
}
/* sanity-check file size */
if (s.st_size > BL_FILE_SIZE_LIMIT)
if (s.st_size > BL_FILE_SIZE_LIMIT) {
errx(1, "%s: file too large (limit: %u, actual: %d)", argv[1], BL_FILE_SIZE_LIMIT, s.st_size);
}
uint8_t *buf = malloc(s.st_size);
if (buf == NULL)
if (buf == NULL) {
errx(1, "failed to allocate %u bytes for firmware buffer", s.st_size);
}
if (read(fd, buf, s.st_size) != s.st_size)
if (read(fd, buf, s.st_size) != s.st_size) {
err(1, "firmware read error");
}
close(fd);
@@ -193,24 +200,27 @@ setopt(void)
const uint16_t opt_mask = (3 << 2); /* BOR_LEV bitmask */
const uint16_t opt_bits = (0 << 2); /* BOR = 0, setting for 2.7-3.6V operation */
if ((*optcr & opt_mask) == opt_bits)
if ((*optcr & opt_mask) == opt_bits) {
errx(0, "option bits are already set as required");
}
/* unlock the control register */
volatile uint32_t *optkeyr = (volatile uint32_t *)0x40023c08;
*optkeyr = 0x08192a3bU;
*optkeyr = 0x4c5d6e7fU;
if (*optcr & 1)
if (*optcr & 1) {
errx(1, "option control register unlock failed");
}
/* program the new option value */
*optcr = (*optcr & ~opt_mask) | opt_bits | (1 << 1);
usleep(1000);
if ((*optcr & opt_mask) == opt_bits)
if ((*optcr & opt_mask) == opt_bits) {
errx(0, "option bits set");
}
errx(1, "option bits setting failed; readback 0x%04x", *optcr);
src/systemcmds/config/config.c
+54 -34
View File
@@ -71,12 +71,15 @@ int
config_main(int argc, char *argv[])
{
if (argc >= 2) {
if (!strncmp(argv[1], "/dev/gyro",9)) {
if (!strncmp(argv[1], "/dev/gyro", 9)) {
do_gyro(argc - 1, argv + 1);
} else if (!strncmp(argv[1], "/dev/accel",10)) {
} else if (!strncmp(argv[1], "/dev/accel", 10)) {
do_accel(argc - 1, argv + 1);
} else if (!strncmp(argv[1], "/dev/mag",8)) {
} else if (!strncmp(argv[1], "/dev/mag", 8)) {
do_mag(argc - 1, argv + 1);
} else {
do_device(argc - 1, argv + 1);
}
@@ -109,16 +112,18 @@ do_device(int argc, char *argv[])
/* disable the device publications */
ret = ioctl(fd, DEVIOCSPUBBLOCK, 1);
if (ret)
errx(ret,"uORB publications could not be blocked");
if (ret) {
errx(ret, "uORB publications could not be blocked");
}
} else if (argc == 2 && !strcmp(argv[1], "unblock")) {
/* enable the device publications */
ret = ioctl(fd, DEVIOCSPUBBLOCK, 0);
if (ret)
errx(ret,"uORB publications could not be unblocked");
if (ret) {
errx(ret, "uORB publications could not be unblocked");
}
} else {
errx(1, "no valid command: %s", argv[1]);
@@ -148,24 +153,27 @@ do_gyro(int argc, char *argv[])
/* set the gyro internal sampling rate up to at least i Hz */
ret = ioctl(fd, GYROIOCSSAMPLERATE, strtoul(argv[2], NULL, 0));
if (ret)
errx(ret,"sampling rate could not be set");
if (ret) {
errx(ret, "sampling rate could not be set");
}
} else if (argc == 3 && !strcmp(argv[1], "rate")) {
/* set the driver to poll at i Hz */
ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[2], NULL, 0));
if (ret)
errx(ret,"pollrate could not be set");
if (ret) {
errx(ret, "pollrate could not be set");
}
} else if (argc == 3 && !strcmp(argv[1], "range")) {
/* set the range to i dps */
ret = ioctl(fd, GYROIOCSRANGE, strtoul(argv[2], NULL, 0));
if (ret)
errx(ret,"range could not be set");
if (ret) {
errx(ret, "range could not be set");
}
} else if (argc == 2 && !strcmp(argv[1], "check")) {
ret = ioctl(fd, GYROIOCSELFTEST, 0);
@@ -181,6 +189,7 @@ do_gyro(int argc, char *argv[])
warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_offset, (double)scale.y_offset, (double)scale.z_offset);
warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_scale, (double)scale.y_scale, (double)scale.z_scale);
} else {
warnx("gyro calibration and self test OK");
}
@@ -192,12 +201,13 @@ do_gyro(int argc, char *argv[])
int srate = ioctl(fd, GYROIOCGSAMPLERATE, 0);
int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0);
int range = ioctl(fd, GYROIOCGRANGE, 0);
int id = ioctl(fd, DEVIOCGDEVICEID,0);
int id = ioctl(fd, DEVIOCGDEVICEID, 0);
int32_t calibration_id = 0;
param_get(param_find("CAL_GYRO0_ID"), &(calibration_id));
warnx("gyro: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d dps", id, calibration_id, srate, prate, range);
warnx("gyro: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d dps",
id, calibration_id, srate, prate, range);
close(fd);
}
@@ -225,26 +235,29 @@ do_mag(int argc, char *argv[])
/* set the mag internal sampling rate up to at least i Hz */
ret = ioctl(fd, MAGIOCSSAMPLERATE, strtoul(argv[2], NULL, 0));
if (ret)
errx(ret,"sampling rate could not be set");
if (ret) {
errx(ret, "sampling rate could not be set");
}
} else if (argc == 3 && !strcmp(argv[1], "rate")) {
/* set the driver to poll at i Hz */
ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[2], NULL, 0));
if (ret)
errx(ret,"pollrate could not be set");
if (ret) {
errx(ret, "pollrate could not be set");
}
} else if (argc == 3 && !strcmp(argv[1], "range")) {
/* set the range to i G */
ret = ioctl(fd, MAGIOCSRANGE, strtoul(argv[2], NULL, 0));
if (ret)
errx(ret,"range could not be set");
if (ret) {
errx(ret, "range could not be set");
}
} else if(argc == 2 && !strcmp(argv[1], "check")) {
} else if (argc == 2 && !strcmp(argv[1], "check")) {
ret = ioctl(fd, MAGIOCSELFTEST, 0);
if (ret) {
@@ -258,6 +271,7 @@ do_mag(int argc, char *argv[])
warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_offset, (double)scale.y_offset, (double)scale.z_offset);
warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_scale, (double)scale.y_scale, (double)scale.z_scale);
} else {
warnx("mag calibration and self test OK");
}
@@ -269,12 +283,13 @@ do_mag(int argc, char *argv[])
int srate = ioctl(fd, MAGIOCGSAMPLERATE, 0);
int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0);
int range = ioctl(fd, MAGIOCGRANGE, 0);
int id = ioctl(fd, DEVIOCGDEVICEID,0);
int id = ioctl(fd, DEVIOCGDEVICEID, 0);
int32_t calibration_id = 0;
param_get(param_find("CAL_MAG0_ID"), &(calibration_id));
warnx("mag: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d Ga", id, calibration_id, srate, prate, range);
warnx("mag: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d Ga",
id, calibration_id, srate, prate, range);
close(fd);
}
@@ -302,26 +317,29 @@ do_accel(int argc, char *argv[])
/* set the accel internal sampling rate up to at least i Hz */
ret = ioctl(fd, ACCELIOCSSAMPLERATE, strtoul(argv[2], NULL, 0));
if (ret)
errx(ret,"sampling rate could not be set");
if (ret) {
errx(ret, "sampling rate could not be set");
}
} else if (argc == 3 && !strcmp(argv[1], "rate")) {
/* set the driver to poll at i Hz */
ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[2], NULL, 0));
if (ret)
errx(ret,"pollrate could not be set");
if (ret) {
errx(ret, "pollrate could not be set");
}
} else if (argc == 3 && !strcmp(argv[1], "range")) {
/* set the range to i G */
ret = ioctl(fd, ACCELIOCSRANGE, strtoul(argv[2], NULL, 0));
if (ret)
errx(ret,"range could not be set");
if (ret) {
errx(ret, "range could not be set");
}
} else if(argc == 2 && !strcmp(argv[1], "check")) {
} else if (argc == 2 && !strcmp(argv[1], "check")) {
ret = ioctl(fd, ACCELIOCSELFTEST, 0);
if (ret) {
@@ -335,23 +353,25 @@ do_accel(int argc, char *argv[])
warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_offset, (double)scale.y_offset, (double)scale.z_offset);
warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_scale, (double)scale.y_scale, (double)scale.z_scale);
} else {
warnx("accel calibration and self test OK");
}
} else {
errx(1,"no arguments given. Try: \n\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 4' to set measurement range to 4 G\n\t");
errx(1, "no arguments given. Try: \n\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 4' to set measurement range to 4 G\n\t");
}
int srate = ioctl(fd, ACCELIOCGSAMPLERATE, 0);
int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0);
int range = ioctl(fd, ACCELIOCGRANGE, 0);
int id = ioctl(fd, DEVIOCGDEVICEID,0);
int id = ioctl(fd, DEVIOCGDEVICEID, 0);
int32_t calibration_id = 0;
param_get(param_find("CAL_ACC0_ID"), &(calibration_id));
warnx("accel: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d G", id, calibration_id, srate, prate, range);
warnx("accel: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d G",
id, calibration_id, srate, prate, range);
close(fd);
}
src/systemcmds/esc_calib/esc_calib.c
+25 -7
View File
@@ -71,8 +71,9 @@ __EXPORT int esc_calib_main(int argc, char *argv[]);
static void
usage(const char *reason)
{
if (reason != NULL)
if (reason != NULL) {
PX4_ERR("%s", reason);
}
PX4_ERR(
"usage:\n"
@@ -117,6 +118,7 @@ esc_calib_main(int argc, char *argv[])
int myoptind = 1;
const char *myoptarg = NULL;
while ((ch = px4_getopt(argc, argv, "d:c:m:al:h:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
@@ -131,43 +133,54 @@ esc_calib_main(int argc, char *argv[])
while ((single_ch = channels % 10)) {
set_mask |= 1<<(single_ch-1);
set_mask |= 1 << (single_ch - 1);
channels /= 10;
}
break;
case 'm':
/* Read in mask directly */
set_mask = strtoul(myoptarg, &ep, 0);
if (*ep != '\0') {
usage("bad set_mask value");
return 1;
}
break;
case 'a':
/* Choose all channels */
for (unsigned i = 0; i<PWM_OUTPUT_MAX_CHANNELS; i++) {
set_mask |= 1<<i;
for (unsigned i = 0; i < PWM_OUTPUT_MAX_CHANNELS; i++) {
set_mask |= 1 << i;
}
break;
case 'l':
/* Read in custom low value */
pwm_low = strtoul(myoptarg, &ep, 0);
if (*ep != '\0' || pwm_low < PWM_LOWEST_MIN || pwm_low > PWM_HIGHEST_MIN) {
usage("low PWM invalid");
return 1;
}
break;
case 'h':
/* Read in custom high value */
pwm_high = strtoul(myoptarg, &ep, 0);
if (*ep != '\0' || pwm_high > PWM_HIGHEST_MAX || pwm_high < PWM_LOWEST_MAX) {
usage("high PWM invalid");
return 1;
}
break;
default:
usage(NULL);
return 1;
@@ -253,6 +266,7 @@ esc_calib_main(int argc, char *argv[])
/* get number of channels available on the device */
ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&max_channels);
if (ret != OK) {
PX4_ERR("PWM_SERVO_GET_COUNT");
return 1;
@@ -260,12 +274,15 @@ esc_calib_main(int argc, char *argv[])
/* tell IO/FMU that its ok to disable its safety with the switch */
ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0);
if (ret != OK) {
PX4_ERR("PWM_SERVO_SET_ARM_OK");
return 1;
}
/* tell IO/FMU that the system is armed (it will output values if safety is off) */
ret = ioctl(fd, PWM_SERVO_ARM, 0);
if (ret != OK) {
PX4_ERR("PWM_SERVO_ARM");
return 1;
@@ -285,11 +302,11 @@ esc_calib_main(int argc, char *argv[])
/* set max PWM */
for (unsigned i = 0; i < max_channels; i++) {
if (set_mask & 1<<i) {
if (set_mask & 1 << i) {
ret = ioctl(fd, PWM_SERVO_SET(i), pwm_high);
if (ret != OK) {
PX4_ERR( "PWM_SERVO_SET(%d), value: %d", i, pwm_high);
PX4_ERR("PWM_SERVO_SET(%d), value: %d", i, pwm_high);
goto cleanup;
}
}
@@ -322,7 +339,7 @@ esc_calib_main(int argc, char *argv[])
/* set disarmed PWM */
for (unsigned i = 0; i < max_channels; i++) {
if (set_mask & 1<<i) {
if (set_mask & 1 << i) {
ret = ioctl(fd, PWM_SERVO_SET(i), pwm_low);
if (ret != OK) {
@@ -352,6 +369,7 @@ esc_calib_main(int argc, char *argv[])
/* disarm */
ret = ioctl(fd, PWM_SERVO_DISARM, 0);
if (ret != OK) {
PX4_ERR("PWM_SERVO_DISARM");
goto cleanup;
src/systemcmds/i2c/i2c.c
+11 -5
View File
@@ -75,21 +75,25 @@ int i2c_main(int argc, char *argv[])
/* find the right I2C */
i2c = up_i2cinitialize(PX4_I2C_BUS_ONBOARD);
if (i2c == NULL)
if (i2c == NULL) {
errx(1, "failed to locate I2C bus");
}
usleep(100000);
uint8_t buf[] = { 0, 4};
int ret = transfer(PX4_I2C_OBDEV_PX4IO, buf, sizeof(buf), NULL, 0);
if (ret)
if (ret) {
errx(1, "send failed - %d", ret);
}
uint32_t val;
ret = transfer(PX4_I2C_OBDEV_PX4IO, NULL, 0, (uint8_t *)&val, sizeof(val));
if (ret)
if (ret) {
errx(1, "recive failed - %d", ret);
}
errx(0, "got 0x%08x", val);
}
@@ -121,8 +125,9 @@ transfer(uint8_t address, uint8_t *send, unsigned send_len, uint8_t *recv, unsig
msgs++;
}
if (msgs == 0)
if (msgs == 0) {
return -1;
}
/*
* I2C architecture means there is an unavoidable race here
@@ -133,8 +138,9 @@ transfer(uint8_t address, uint8_t *send, unsigned send_len, uint8_t *recv, unsig
ret = I2C_TRANSFER(i2c, &msgv[0], msgs);
// reset the I2C bus to unwedge on error
if (ret != OK)
if (ret != OK) {
up_i2creset(i2c);
}
return ret;
}
src/systemcmds/mixer/mixer.cpp
+8 -3
View File
@@ -76,22 +76,26 @@ mixer_main(int argc, char *argv[])
}
int ret = load(argv[2], argv[3]);
if(ret !=0) {
if (ret != 0) {
warnx("failed to load mixer");
return 1;
}
} else {
usage("Unknown command");
return 1;
}
return 0;
}
static void
usage(const char *reason)
{
if (reason)
if (reason) {
PX4_INFO("%s\n", reason);
}
PX4_INFO("usage:\n");
PX4_INFO(" mixer load <device> <filename>\n");
@@ -102,7 +106,7 @@ load(const char *devname, const char *fname)
{
// sleep a while to ensure device has been set up
usleep(20000);
int dev;
char buf[2048];
@@ -130,5 +134,6 @@ load(const char *devname, const char *fname)
warnx("error loading mixers from %s", fname);
return 1;
}
return 0;
}
src/systemcmds/motor_test/motor_test.c
+16 -13
View File
@@ -69,13 +69,14 @@ void motor_test(unsigned channel, float value)
_test_motor.timestamp = hrt_absolute_time();
_test_motor.value = value;
if (_test_motor_pub != nullptr) {
/* publish test state */
orb_publish(ORB_ID(test_motor), _test_motor_pub, &_test_motor);
} else {
/* advertise and publish */
_test_motor_pub = orb_advertise(ORB_ID(test_motor), &_test_motor);
}
if (_test_motor_pub != nullptr) {
/* publish test state */
orb_publish(ORB_ID(test_motor), _test_motor_pub, &_test_motor);
} else {
/* advertise and publish */
_test_motor_pub = orb_advertise(ORB_ID(test_motor), &_test_motor);
}
}
static void usage(const char *reason)
@@ -85,10 +86,10 @@ static void usage(const char *reason)
}
errx(1,
"usage:\n"
"motor_test\n"
" -m <channel> Motor to test (0..7)\n"
" -p <power> Power (0..100)\n");
"usage:\n"
"motor_test\n"
" -m <channel> Motor to test (0..7)\n"
" -p <power> Power (0..100)\n");
}
int motor_test_main(int argc, char *argv[])
@@ -114,11 +115,13 @@ int motor_test_main(int argc, char *argv[])
/* Read in power value */
lval = strtoul(optarg, NULL, 0);
if (lval > 100)
if (lval > 100) {
usage("value invalid");
}
value = ((float)lval)/100.f;
value = ((float)lval) / 100.f;
break;
default:
usage(NULL);
}
src/systemcmds/mtd/24xxxx_mtd.c
+8 -3
View File
@@ -239,11 +239,13 @@ void at24c_test(void)
for (count = 0; count < 10000; count++) {
ssize_t result = at24c_bread(&g_at24c.mtd, 0, 1, buf);
if (result == ERROR) {
if (errors++ > 2) {
vdbg("too many errors\n");
return;
}
} else if (result != 1) {
vdbg("unexpected %u\n", result);
}
@@ -311,8 +313,9 @@ static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock,
ret = I2C_TRANSFER(priv->dev, &msgv[0], 2);
perf_end(priv->perf_transfers);
if (ret >= 0)
if (ret >= 0) {
break;
}
fvdbg("read stall");
usleep(1000);
@@ -396,8 +399,9 @@ static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock, size_t
ret = I2C_TRANSFER(priv->dev, &msgv[0], 1);
perf_end(priv->perf_transfers);
if (ret >= 0)
if (ret >= 0) {
break;
}
fvdbg("write stall");
usleep(1000);
@@ -503,7 +507,8 @@ static int at24c_ioctl(FAR struct mtd_dev_s *dev, int cmd, unsigned long arg)
*
************************************************************************************/
FAR struct mtd_dev_s *at24c_initialize(FAR struct i2c_dev_s *dev) {
FAR struct mtd_dev_s *at24c_initialize(FAR struct i2c_dev_s *dev)
{
FAR struct at24c_dev_s *priv;
fvdbg("dev: %p\n", dev);
src/systemcmds/mtd/mtd.c
+94 -47
View File
@@ -92,8 +92,8 @@ static void mtd_erase(char *partition_names[], unsigned n_partitions);
static void mtd_readtest(char *partition_names[], unsigned n_partitions);
static void mtd_rwtest(char *partition_names[], unsigned n_partitions);
static void mtd_print_info(void);
static int mtd_get_geometry(unsigned long *blocksize, unsigned long *erasesize, unsigned long *neraseblocks,
unsigned *blkpererase, unsigned *nblocks, unsigned *partsize, unsigned n_partitions);
static int mtd_get_geometry(unsigned long *blocksize, unsigned long *erasesize, unsigned long *neraseblocks,
unsigned *blkpererase, unsigned *nblocks, unsigned *partsize, unsigned n_partitions);
static bool attached = false;
static bool started = false;
@@ -107,9 +107,10 @@ static const int n_partitions_default = sizeof(partition_names_default) / sizeof
static void
mtd_status(void)
{
if (!attached)
if (!attached) {
errx(1, "MTD driver not started");
}
mtd_print_info();
exit(0);
}
@@ -122,40 +123,46 @@ int mtd_main(int argc, char *argv[])
/* start mapping according to user request */
if (argc >= 3) {
mtd_start(argv + 2, argc - 2);
} else {
mtd_start(partition_names_default, n_partitions_default);
}
}
if (!strcmp(argv[1], "test"))
if (!strcmp(argv[1], "test")) {
mtd_test();
}
if (!strcmp(argv[1], "readtest")) {
if (argc >= 3) {
mtd_readtest(argv + 2, argc - 2);
} else {
mtd_readtest(partition_names_default, n_partitions_default);
}
}
}
if (!strcmp(argv[1], "rwtest")) {
if (argc >= 3) {
mtd_rwtest(argv + 2, argc - 2);
} else {
mtd_rwtest(partition_names_default, n_partitions_default);
}
}
}
if (!strcmp(argv[1], "status"))
if (!strcmp(argv[1], "status")) {
mtd_status();
}
if (!strcmp(argv[1], "erase")) {
if (argc >= 3) {
mtd_erase(argv + 2, argc - 2);
} else {
mtd_erase(partition_names_default, n_partitions_default);
}
}
}
}
errx(1, "expected a command, try 'start', 'erase', 'status', 'readtest', 'rwtest' or 'test'");
@@ -163,7 +170,7 @@ int mtd_main(int argc, char *argv[])
struct mtd_dev_s *ramtron_initialize(FAR struct spi_dev_s *dev);
struct mtd_dev_s *mtd_partition(FAR struct mtd_dev_s *mtd,
off_t firstblock, off_t nblocks);
off_t firstblock, off_t nblocks);
#ifdef CONFIG_MTD_RAMTRON
static void
@@ -181,8 +188,9 @@ ramtron_attach(void)
SPI_SETMODE(spi, SPIDEV_MODE3);
SPI_SELECT(spi, SPIDEV_FLASH, false);
if (spi == NULL)
if (spi == NULL) {
errx(1, "failed to locate spi bus");
}
/* start the RAMTRON driver, attempt 5 times */
for (int i = 0; i < 5; i++) {
@@ -199,10 +207,12 @@ ramtron_attach(void)
}
/* if last attempt is still unsuccessful, abort */
if (mtd_dev == NULL)
if (mtd_dev == NULL) {
errx(1, "failed to initialize mtd driver");
}
int ret = mtd_dev->ioctl(mtd_dev, MTDIOC_SETSPEED, (unsigned long)10 * 1000 * 1000);
int ret = mtd_dev->ioctl(mtd_dev, MTDIOC_SETSPEED, (unsigned long)10*1000*1000);
if (ret != OK) {
// FIXME: From the previous warnx call, it looked like this should have been an errx instead. Tried
// that but setting the bug speed does fail all the time. Which was then exiting and the board would
@@ -222,24 +232,28 @@ at24xxx_attach(void)
/* this resets the I2C bus, set correct bus speed again */
I2C_SETFREQUENCY(i2c, 400000);
if (i2c == NULL)
if (i2c == NULL) {
errx(1, "failed to locate I2C bus");
}
/* start the MTD driver, attempt 5 times */
for (int i = 0; i < 5; i++) {
mtd_dev = at24c_initialize(i2c);
if (mtd_dev) {
/* abort on first valid result */
if (i > 0) {
warnx("warning: EEPROM needed %d attempts to attach", i+1);
warnx("warning: EEPROM needed %d attempts to attach", i + 1);
}
break;
}
}
/* if last attempt is still unsuccessful, abort */
if (mtd_dev == NULL)
if (mtd_dev == NULL) {
errx(1, "failed to initialize EEPROM driver");
}
attached = true;
}
@@ -250,15 +264,16 @@ mtd_start(char *partition_names[], unsigned n_partitions)
{
int ret;
if (started)
if (started) {
errx(1, "mtd already mounted");
}
if (!attached) {
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
at24xxx_attach();
#else
#else
ramtron_attach();
#endif
#endif
}
if (!mtd_dev) {
@@ -270,8 +285,10 @@ mtd_start(char *partition_names[], unsigned n_partitions)
unsigned blkpererase, nblocks, partsize;
ret = mtd_get_geometry(&blocksize, &erasesize, &neraseblocks, &blkpererase, &nblocks, &partsize, n_partitions);
if (ret)
if (ret) {
exit(3);
}
/* Now create MTD FLASH partitions */
@@ -320,10 +337,10 @@ mtd_start(char *partition_names[], unsigned n_partitions)
exit(0);
}
int mtd_get_geometry(unsigned long *blocksize, unsigned long *erasesize, unsigned long *neraseblocks,
unsigned *blkpererase, unsigned *nblocks, unsigned *partsize, unsigned n_partitions)
int mtd_get_geometry(unsigned long *blocksize, unsigned long *erasesize, unsigned long *neraseblocks,
unsigned *blkpererase, unsigned *nblocks, unsigned *partsize, unsigned n_partitions)
{
/* Get the geometry of the FLASH device */
/* Get the geometry of the FLASH device */
FAR struct mtd_geometry_s geo;
@@ -358,24 +375,31 @@ static ssize_t mtd_get_partition_size(void)
unsigned long blocksize, erasesize, neraseblocks;
unsigned blkpererase, nblocks, partsize = 0;
int ret = mtd_get_geometry(&blocksize, &erasesize, &neraseblocks, &blkpererase, &nblocks, &partsize, n_partitions_current);
int ret = mtd_get_geometry(&blocksize, &erasesize, &neraseblocks, &blkpererase, &nblocks, &partsize,
n_partitions_current);
if (ret != OK) {
errx(1, "Failed to get geometry");
}
return partsize;
}
void mtd_print_info(void)
{
if (!attached)
if (!attached) {
exit(1);
}
unsigned long blocksize, erasesize, neraseblocks;
unsigned blkpererase, nblocks, partsize;
int ret = mtd_get_geometry(&blocksize, &erasesize, &neraseblocks, &blkpererase, &nblocks, &partsize, n_partitions_current);
if (ret)
int ret = mtd_get_geometry(&blocksize, &erasesize, &neraseblocks, &blkpererase, &nblocks, &partsize,
n_partitions_current);
if (ret) {
exit(3);
}
warnx("Flash Geometry:");
@@ -400,19 +424,24 @@ mtd_erase(char *partition_names[], unsigned n_partitions)
{
uint8_t v[64];
memset(v, 0xFF, sizeof(v));
for (uint8_t i = 0; i < n_partitions; i++) {
uint32_t count = 0;
printf("Erasing %s\n", partition_names[i]);
int fd = open(partition_names[i], O_WRONLY);
if (fd == -1) {
errx(1, "Failed to open partition");
}
while (write(fd, v, sizeof(v)) == sizeof(v)) {
count += sizeof(v);
}
printf("Erased %lu bytes\n", (unsigned long)count);
close(fd);
}
exit(0);
}
@@ -427,21 +456,27 @@ mtd_readtest(char *partition_names[], unsigned n_partitions)
ssize_t expected_size = mtd_get_partition_size();
uint8_t v[128];
for (uint8_t i = 0; i < n_partitions; i++) {
ssize_t count = 0;
printf("reading %s expecting %u bytes\n", partition_names[i], expected_size);
int fd = open(partition_names[i], O_RDONLY);
if (fd == -1) {
errx(1, "Failed to open partition");
}
while (read(fd, v, sizeof(v)) == sizeof(v)) {
count += sizeof(v);
}
if (count != expected_size) {
errx(1,"Failed to read partition - got %u/%u bytes", count, expected_size);
errx(1, "Failed to read partition - got %u/%u bytes", count, expected_size);
}
close(fd);
}
printf("readtest OK\n");
exit(0);
}
@@ -458,38 +493,50 @@ mtd_rwtest(char *partition_names[], unsigned n_partitions)
ssize_t expected_size = mtd_get_partition_size();
uint8_t v[128], v2[128];
for (uint8_t i = 0; i < n_partitions; i++) {
ssize_t count = 0;
off_t offset = 0;
off_t offset = 0;
printf("rwtest %s testing %u bytes\n", partition_names[i], expected_size);
int fd = open(partition_names[i], O_RDWR);
if (fd == -1) {
errx(1, "Failed to open partition");
}
while (read(fd, v, sizeof(v)) == sizeof(v)) {
count += sizeof(v);
if (lseek(fd, offset, SEEK_SET) != offset) {
errx(1, "seek failed");
}
if (write(fd, v, sizeof(v)) != sizeof(v)) {
errx(1, "write failed");
}
if (lseek(fd, offset, SEEK_SET) != offset) {
errx(1, "seek failed");
}
if (read(fd, v2, sizeof(v2)) != sizeof(v2)) {
errx(1, "read failed");
}
if (memcmp(v, v2, sizeof(v2)) != 0) {
errx(1, "memcmp failed");
}
offset += sizeof(v);
if (lseek(fd, offset, SEEK_SET) != offset) {
errx(1, "seek failed");
}
if (write(fd, v, sizeof(v)) != sizeof(v)) {
errx(1, "write failed");
}
if (lseek(fd, offset, SEEK_SET) != offset) {
errx(1, "seek failed");
}
if (read(fd, v2, sizeof(v2)) != sizeof(v2)) {
errx(1, "read failed");
}
if (memcmp(v, v2, sizeof(v2)) != 0) {
errx(1, "memcmp failed");
}
offset += sizeof(v);
}
if (count != expected_size) {
errx(1,"Failed to read partition - got %u/%u bytes", count, expected_size);
errx(1, "Failed to read partition - got %u/%u bytes", count, expected_size);
}
close(fd);
}
printf("rwtest OK\n");
exit(0);
}
src/systemcmds/nshterm/nshterm.c
+75 -70
View File
@@ -59,87 +59,92 @@ __EXPORT int nshterm_main(int argc, char *argv[]);
int
nshterm_main(int argc, char *argv[])
{
if (argc < 2) {
printf("Usage: nshterm <device>\n");
exit(1);
}
unsigned retries = 0;
int fd = -1;
int armed_fd = orb_subscribe(ORB_ID(actuator_armed));
struct actuator_armed_s armed;
if (argc < 2) {
printf("Usage: nshterm <device>\n");
exit(1);
}
/* back off 1800 ms to avoid running into the USB setup timing */
while (hrt_absolute_time() < 1800U * 1000U) {
usleep(50000);
}
unsigned retries = 0;
int fd = -1;
int armed_fd = orb_subscribe(ORB_ID(actuator_armed));
struct actuator_armed_s armed;
/* try to bring up the console - stop doing so if the system gets armed */
while (true) {
/* back off 1800 ms to avoid running into the USB setup timing */
while (hrt_absolute_time() < 1800U * 1000U) {
usleep(50000);
}
/* abort if an arming topic is published and system is armed */
bool updated = false;
orb_check(armed_fd, &updated);
if (updated) {
/* the system is now providing arming status feedback.
* instead of timing out, we resort to abort bringing
* up the terminal.
*/
orb_copy(ORB_ID(actuator_armed), armed_fd, &armed);
/* try to bring up the console - stop doing so if the system gets armed */
while (true) {
if (armed.armed) {
/* this is not an error, but we are done */
exit(0);
}
}
/* abort if an arming topic is published and system is armed */
bool updated = false;
orb_check(armed_fd, &updated);
/* the retries are to cope with the behaviour of /dev/ttyACM0 */
/* which may not be ready immediately. */
fd = open(argv[1], O_RDWR);
if (fd != -1) {
close(armed_fd);
break;
}
usleep(100000);
retries++;
}
if (fd == -1) {
perror(argv[1]);
exit(1);
}
if (updated) {
/* the system is now providing arming status feedback.
* instead of timing out, we resort to abort bringing
* up the terminal.
*/
orb_copy(ORB_ID(actuator_armed), armed_fd, &armed);
/* set up the serial port with output processing */
/* Try to set baud rate */
struct termios uart_config;
int termios_state;
if (armed.armed) {
/* this is not an error, but we are done */
exit(0);
}
}
/* Back up the original uart configuration to restore it after exit */
if ((termios_state = tcgetattr(fd, &uart_config)) < 0) {
warnx("ERR get config %s: %d\n", argv[1], termios_state);
close(fd);
return -1;
}
/* the retries are to cope with the behaviour of /dev/ttyACM0 */
/* which may not be ready immediately. */
fd = open(argv[1], O_RDWR);
/* Set ONLCR flag (which appends a CR for every LF) */
uart_config.c_oflag |= (ONLCR | OPOST);
if (fd != -1) {
close(armed_fd);
break;
}
if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) {
warnx("ERR set config %s\n", argv[1]);
close(fd);
return -1;
}
usleep(100000);
retries++;
}
/* setup standard file descriptors */
close(0);
close(1);
close(2);
dup2(fd, 0);
dup2(fd, 1);
dup2(fd, 2);
if (fd == -1) {
perror(argv[1]);
exit(1);
}
nsh_consolemain(0, NULL);
/* set up the serial port with output processing */
close(fd);
/* Try to set baud rate */
struct termios uart_config;
int termios_state;
return OK;
/* Back up the original uart configuration to restore it after exit */
if ((termios_state = tcgetattr(fd, &uart_config)) < 0) {
warnx("ERR get config %s: %d\n", argv[1], termios_state);
close(fd);
return -1;
}
/* Set ONLCR flag (which appends a CR for every LF) */
uart_config.c_oflag |= (ONLCR | OPOST);
if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) {
warnx("ERR set config %s\n", argv[1]);
close(fd);
return -1;
}
/* setup standard file descriptors */
close(0);
close(1);
close(2);
dup2(fd, 0);
dup2(fd, 1);
dup2(fd, 2);
nsh_consolemain(0, NULL);
close(fd);
return OK;
}
src/systemcmds/param/param.c
+7 -1
View File
@@ -191,6 +191,7 @@ param_main(int argc, char *argv[])
if (!strcmp(argv[1], "index_used")) {
if (argc >= 3) {
return do_show_index(argv[2], true);
} else {
warnx("no index provided");
return 1;
@@ -200,6 +201,7 @@ param_main(int argc, char *argv[])
if (!strcmp(argv[1], "index")) {
if (argc >= 3) {
return do_show_index(argv[2], false);
} else {
warnx("no index provided");
return 1;
@@ -299,6 +301,7 @@ do_show_index(const char *index, bool used_index)
if (used_index) {
param = param_for_used_index(i);
} else {
param = param_for_index(i);
}
@@ -326,6 +329,7 @@ do_show_index(const char *index, bool used_index)
}
break;
default:
printf("<unknown type %d>\n", 0 + param_type(param));
}
@@ -537,7 +541,7 @@ do_compare(const char *name, char *vals[], unsigned comparisons, enum COMPARE_OP
float g = strtod(vals[k], &end);
if (((cmp_op == COMPARE_OPERATOR_EQUAL) && (fabsf(f - g) < 1e-7f)) ||
if (((cmp_op == COMPARE_OPERATOR_EQUAL) && (fabsf(f - g) < 1e-7f)) ||
((cmp_op == COMPARE_OPERATOR_GREATER) && (f > g))) {
printf(" %4.4f: ", (double)f);
ret = 0;
@@ -575,6 +579,7 @@ do_reset(const char *excludes[], int num_excludes)
warnx("Param export failed.");
return 1;
}
return 0;
}
@@ -602,5 +607,6 @@ do_reset_nostart(const char *excludes[], int num_excludes)
return 1;
}
return 0;
}
src/systemcmds/pwm/pwm.c
+4 -2
View File
@@ -187,8 +187,7 @@ pwm_main(int argc, char *argv[])
break;
case 'p':
{
case 'p': {
/* check if this is a param name */
if (strncmp("p:", optarg, 2) == 0) {
@@ -203,12 +202,15 @@ pwm_main(int argc, char *argv[])
if (gret == 0) {
pwm_value = pwm_parm;
} else {
usage("PARAM LOAD FAIL");
}
} else {
usage("PARAM NAME NOT FOUND");
}
} else {
pwm_value = strtoul(optarg, &ep, 0);
src/systemcmds/reboot/reboot.c
+2 -1
View File
@@ -52,6 +52,7 @@ int reboot_main(int argc, char *argv[])
int myoptind = 1;
const char *myoptarg = NULL;
while ((ch = px4_getopt(argc, argv, "b", &myoptind, &myoptarg)) != -1) {
switch (ch) {
case 'b':
@@ -60,7 +61,7 @@ int reboot_main(int argc, char *argv[])
default:
PX4_ERR("usage: reboot [-b]\n"
" -b reboot into the bootloader");
" -b reboot into the bootloader");
}
}
src/systemcmds/reflect/reflect.c
+45 -36
View File
@@ -54,7 +54,7 @@ __EXPORT int reflect_main(int argc, char *argv[]);
#define MAX_BLOCKS 1000
static uint32_t nblocks;
struct block {
uint32_t v[256];
uint32_t v[256];
};
static struct block *blocks[MAX_BLOCKS];
@@ -62,50 +62,59 @@ static struct block *blocks[MAX_BLOCKS];
static void allocate_blocks(void)
{
while (nblocks < MAX_BLOCKS) {
blocks[nblocks] = calloc(1, sizeof(struct block));
if (blocks[nblocks] == NULL) {
break;
}
for (uint32_t i=0; i<sizeof(blocks[nblocks]->v)/sizeof(uint32_t); i++) {
blocks[nblocks]->v[i] = VALUE(i);
}
nblocks++;
}
printf("Allocated %u blocks\n", nblocks);
while (nblocks < MAX_BLOCKS) {
blocks[nblocks] = calloc(1, sizeof(struct block));
if (blocks[nblocks] == NULL) {
break;
}
for (uint32_t i = 0; i < sizeof(blocks[nblocks]->v) / sizeof(uint32_t); i++) {
blocks[nblocks]->v[i] = VALUE(i);
}
nblocks++;
}
printf("Allocated %u blocks\n", nblocks);
}
static void check_blocks(void)
{
for (uint32_t n=0; n<nblocks; n++) {
for (uint32_t i=0; i<sizeof(blocks[nblocks]->v)/sizeof(uint32_t); i++) {
assert(blocks[n]->v[i] == VALUE(i));
}
}
for (uint32_t n = 0; n < nblocks; n++) {
for (uint32_t i = 0; i < sizeof(blocks[nblocks]->v) / sizeof(uint32_t); i++) {
assert(blocks[n]->v[i] == VALUE(i));
}
}
}
int
reflect_main(int argc, char *argv[])
{
uint32_t total = 0;
printf("Starting reflector\n");
uint32_t total = 0;
printf("Starting reflector\n");
allocate_blocks();
allocate_blocks();
while (true) {
char buf[128];
ssize_t n = read(0, buf, sizeof(buf));
if (n < 0) {
break;
}
if (n > 0) {
write(1, buf, n);
}
total += n;
if (total > 1024000) {
check_blocks();
total = 0;
}
}
return OK;
while (true) {
char buf[128];
ssize_t n = read(0, buf, sizeof(buf));
if (n < 0) {
break;
}
if (n > 0) {
write(1, buf, n);
}
total += n;
if (total > 1024000) {
check_blocks();
total = 0;
}
}
return OK;
}
src/systemcmds/tests/test_bson.c
+3 -3
View File
@@ -39,7 +39,7 @@
#define __STDC_FORMAT_MACROS
#include <inttypes.h>
#include <px4_defines.h>
#include <stdio.h>
#include <string.h>
@@ -111,8 +111,8 @@ decode_callback(bson_decoder_t decoder, void *private, bson_node_t node)
if (node->b != sample_bool) {
PX4_ERR("FAIL: decoder: bool1 value %s, expected %s",
(node->b ? "true" : "false"),
(sample_bool ? "true" : "false"));
(node->b ? "true" : "false"),
(sample_bool ? "true" : "false"));
return 1;
}
src/systemcmds/tests/test_conv.cpp
+1 -1
View File
@@ -67,7 +67,7 @@ int test_conv(int argc, char *argv[])
if (fabsf(f - fres) > 0.0001f) {
PX4_ERR("conversion fail: input: %8.4f, intermediate: %d, result: %8.4f", (double)f, REG_TO_SIGNED(FLOAT_TO_REG(f)),
(double)fres);
(double)fres);
return 1;
}
}
src/systemcmds/tests/test_eigen.cpp
+35 -18
View File
@@ -89,9 +89,9 @@ uint8_t err_num;
#define EXPECT_QUATERNION(q_exp, q_act, epsilon) \
(fabsf(q_exp.x() - q_act.x()) <= epsilon && \
fabsf(q_exp.y() - q_act.y()) <= epsilon && \
fabsf(q_exp.z() - q_act.z()) <= epsilon && \
fabsf(q_exp.w() - q_act.w()) <= epsilon)
fabsf(q_exp.y() - q_act.y()) <= epsilon && \
fabsf(q_exp.z() - q_act.z()) <= epsilon && \
fabsf(q_exp.w() - q_act.w()) <= epsilon)
#define EXPECT_NEAR(expected, actual, epsilon) \
(fabsf(expected - actual) <= epsilon)
@@ -130,12 +130,13 @@ math::Quaternion px4qFromEigenq(const Eigen::Quaternionf &q);
* Construct new Eigen::Quaternion from euler angles
* Right order is YPR.
**/
Eigen::Quaternionf quatFromEuler(const Eigen::Vector3f &rpy){
Eigen::Quaternionf quatFromEuler(const Eigen::Vector3f &rpy)
{
return Eigen::Quaternionf(
Eigen::AngleAxisf(rpy.z(), Eigen::Vector3f::UnitZ()) *
Eigen::AngleAxisf(rpy.y(), Eigen::Vector3f::UnitY()) *
Eigen::AngleAxisf(rpy.x(), Eigen::Vector3f::UnitX())
);
Eigen::AngleAxisf(rpy.z(), Eigen::Vector3f::UnitZ()) *
Eigen::AngleAxisf(rpy.y(), Eigen::Vector3f::UnitY()) *
Eigen::AngleAxisf(rpy.x(), Eigen::Vector3f::UnitX())
);
}
/**
@@ -143,7 +144,8 @@ Eigen::Quaternionf quatFromEuler(const Eigen::Vector3f &rpy){
* Construct new Eigen::Vector3f of euler angles from quaternion
* Right order is YPR.
**/
Eigen::Vector3f eulerFromQuat(const Eigen::Quaternionf &q){
Eigen::Vector3f eulerFromQuat(const Eigen::Quaternionf &q)
{
return q.toRotationMatrix().eulerAngles(2, 1, 0).reverse();
}
@@ -151,7 +153,8 @@ Eigen::Vector3f eulerFromQuat(const Eigen::Quaternionf &q){
* @brief
* Construct new Eigen::Matrix3f from euler angles
**/
Eigen::Matrix3f matrixFromEuler(const Eigen::Vector3f &rpy){
Eigen::Matrix3f matrixFromEuler(const Eigen::Vector3f &rpy)
{
return quatFromEuler(rpy).toRotationMatrix();
}
@@ -159,7 +162,8 @@ Eigen::Matrix3f matrixFromEuler(const Eigen::Vector3f &rpy){
* @brief
* Adjust PX4 math::quaternion to Eigen::Quaternionf
**/
Eigen::Quaternionf eigenqFromPx4q(const math::Quaternion &q){
Eigen::Quaternionf eigenqFromPx4q(const math::Quaternion &q)
{
return Eigen::Quaternionf(q.data[1], q.data[2], q.data[3], q.data[0]);
}
@@ -167,7 +171,8 @@ Eigen::Quaternionf eigenqFromPx4q(const math::Quaternion &q){
* @brief
* Adjust Eigen::Quaternionf to PX4 math::quaternion
**/
math::Quaternion px4qFromEigenq(const Eigen::Quaternionf &q){
math::Quaternion px4qFromEigenq(const Eigen::Quaternionf &q)
{
return math::Quaternion(q.w(), q.x(), q.y(), q.z());
}
@@ -175,7 +180,8 @@ math::Quaternion px4qFromEigenq(const Eigen::Quaternionf &q){
* @brief
* Testing main routine
**/
int test_eigen(int argc, char *argv[]) {
int test_eigen(int argc, char *argv[])
{
int rc = 0;
warnx("Testing Eigen math...");
@@ -285,7 +291,7 @@ int test_eigen(int argc, char *argv[]) {
const Eigen::Matrix<float, 2, 3> m1_orig =
(Eigen::Matrix<float, 2, 3>() << 1, 2, 3,
4, 5, 6).finished();
4, 5, 6).finished();
Eigen::Matrix<float, 2, 3> m1(m1_orig);
@@ -362,6 +368,7 @@ int test_eigen(int argc, char *argv[]) {
++err_num;
rc = 1;
}
/********************************************************************************************************/
/******************************************** TEST 2 ****************************************************/
q_true = {1.0f, 0.0f, 0.0f, 0.0f};
@@ -376,6 +383,7 @@ int test_eigen(int argc, char *argv[]) {
++err_num;
rc = 1;
}
/********************************************************************************************************/
/******************************************** TEST 3 ****************************************************/
q_true = quatFromEuler(Eigen::Vector3f(0.3f, 0.2f, 0.1f));
@@ -388,6 +396,7 @@ int test_eigen(int argc, char *argv[]) {
++err_num;
rc = 1;
}
/********************************************************************************************************/
/******************************************** TEST 4 ****************************************************/
q_true = quatFromEuler(Eigen::Vector3f(-1.5f, -0.2f, 0.5f));
@@ -400,6 +409,7 @@ int test_eigen(int argc, char *argv[]) {
++err_num;
rc = 1;
}
/********************************************************************************************************/
/******************************************** TEST 5 ****************************************************/
q_true = quatFromEuler(Eigen::Vector3f(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3));
@@ -407,13 +417,15 @@ int test_eigen(int argc, char *argv[]) {
for (size_t i = 0; i < 4; i++) {
if (!EXPECT_QUATERNION(q_true, q, FLT_EPSILON)) {
printf("%llu: It[%d]: Value of: Quaternion [0.6830, 0.1830, -0.6830, 0.1830]\n", (unsigned long long)hrt_absolute_time(), (int)i);
printf("%llu: It[%d]: Value of: Quaternion [0.6830, 0.1830, -0.6830, 0.1830]\n",
(unsigned long long)hrt_absolute_time(), (int)i);
printf("Actual: \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q.w(), q.x(), q.y(), q.z());
printf("Expected: \t[%8.5f, %8.5f, %8.5f, %8.5f]\n", q_true.w(), q_true.x(), q_true.y(), q_true.z());
++err_num;
rc = 1;
}
}
/********************************************************************************************************/
/******************************************** TEST 6 ****************************************************/
printf("%llu: Conversion method: Testing transformation range...\n", (unsigned long long)hrt_absolute_time());
@@ -430,7 +442,8 @@ int test_eigen(int argc, char *argv[]) {
for (size_t i = 0; i < 3; i++) {
for (size_t j = 0; j < 3; j++) {
if (!EXPECT_NEAR(R_orig(i, j), R(i, j), epsilon_f)) {
printf("%llu: (%d, %d) Value of: Quaternion constructor or 'toRotationMatrix'\n", (unsigned long long)hrt_absolute_time(), (int)i, (int)j);
printf("%llu: (%d, %d) Value of: Quaternion constructor or 'toRotationMatrix'\n",
(unsigned long long)hrt_absolute_time(), (int)i, (int)j);
printf("Actual: \t%8.5f\n", R(i, j));
printf("Expected: \t%8.5f\n", R_orig(i, j));
++err_num;
@@ -457,7 +470,7 @@ int test_eigen(int argc, char *argv[]) {
/******************************************** TEST 1 ****************************************************/
q = quatFromEuler(Eigen::Vector3f(0.0f, 0.0f, M_PI_2_F));
vector_q = q._transformVector(vector);
Eigen::Vector3f vector_true = {-1.00f, 1.00f, 1.00f};
Eigen::Vector3f vector_true = { -1.00f, 1.00f, 1.00f};
for (size_t i = 0; i < 3; i++) {
if (!EXPECT_NEAR(vector_true(i), vector_q(i), FLT_EPSILON)) {
@@ -468,6 +481,7 @@ int test_eigen(int argc, char *argv[]) {
rc = 1;
}
}
/********************************************************************************************************/
/******************************************** TEST 2 ****************************************************/
q = quatFromEuler(Eigen::Vector3f(0.1f, 0.2f, 0.3f));
@@ -483,6 +497,7 @@ int test_eigen(int argc, char *argv[]) {
rc = 1;
}
}
/********************************************************************************************************/
/******************************************** TEST 3 ****************************************************/
q = quatFromEuler(Eigen::Vector3f(0.5f, -0.2f, -1.5f));
@@ -498,11 +513,12 @@ int test_eigen(int argc, char *argv[]) {
rc = 1;
}
}
/********************************************************************************************************/
/******************************************** TEST 4 ****************************************************/
q = quatFromEuler(Eigen::Vector3f(-M_PI_F / 3.0f, -M_PI_2_F, M_PI_2_F));
vector_q = q._transformVector(vector);
vector_true = {-1.366030f, 0.366025f, 1.000000f};
vector_true = { -1.366030f, 0.366025f, 1.000000f};
for (size_t i = 0; i < 3; i++) {
if (!EXPECT_NEAR(vector_true(i), vector_q(i), epsilon_f)) {
@@ -513,6 +529,7 @@ int test_eigen(int argc, char *argv[]) {
rc = 1;
}
}
/********************************************************************************************************/
/******************************************** TEST 5 ****************************************************/
printf("%llu: Rotation method: Testing transformation range...\n", (unsigned long long)hrt_absolute_time());
src/systemcmds/tests/test_file2.c
+1
View File
@@ -176,6 +176,7 @@ int test_file2(int argc, char *argv[])
int myoptind = 1;
const char *myoptarg = NULL;
while ((opt = px4_getopt(argc, argv, "c:s:FLh", &myoptind, &myoptarg)) != EOF) {
switch (opt) {
case 'F':
src/systemcmds/tests/test_hott_telemetry.c
+1 -1
View File
@@ -195,7 +195,7 @@ int test_hott_telemetry(int argc, char *argv[])
} else {
PX4_WARN("WARN: Received %d out of %d byte pairs of which %d were valid from the HoTT receiver device.", received_count,
max_polls, valid_count);
max_polls, valid_count);
}
} else {
src/systemcmds/tests/test_mixer.cpp
+10 -6
View File
@@ -204,7 +204,7 @@ int test_mixer(int argc, char *argv[])
mixed = mixer_group.mix(&outputs[0], output_max, NULL);
pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min,
r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
for (unsigned i = 0; i < mixed; i++) {
@@ -216,6 +216,7 @@ int test_mixer(int argc, char *argv[])
warnx("active servo < min");
return 1;
}
} else {
if (r_page_servos[i] != r_page_servo_disarmed[i]) {
warnx("throttle output != 0 (this check assumed the IO pass mixer!)");
@@ -244,7 +245,7 @@ int test_mixer(int argc, char *argv[])
mixed = mixer_group.mix(&outputs[0], output_max, NULL);
pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min,
r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
for (unsigned i = 0; i < mixed; i++) {
@@ -287,7 +288,8 @@ int test_mixer(int argc, char *argv[])
/* mix */
mixed = mixer_group.mix(&outputs[0], output_max, NULL);
pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs,
pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min,
r_page_servo_control_max, outputs,
r_page_servos, &pwm_limit);
PX4_INFO("mixed %d outputs (max %d)", mixed, output_max);
@@ -314,7 +316,8 @@ int test_mixer(int argc, char *argv[])
/* mix */
mixed = mixer_group.mix(&outputs[0], output_max, NULL);
pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs,
pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min,
r_page_servo_control_max, outputs,
r_page_servos, &pwm_limit);
//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
@@ -351,7 +354,8 @@ int test_mixer(int argc, char *argv[])
/* mix */
mixed = mixer_group.mix(&outputs[0], output_max, NULL);
pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs,
pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min,
r_page_servo_control_max, outputs,
r_page_servos, &pwm_limit);
//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
@@ -435,7 +439,7 @@ mixer_callback(uintptr_t handle,
control = actuator_controls[control_index];
if (should_prearm && control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE &&
control_index == actuator_controls_s::INDEX_THROTTLE) {
control_index == actuator_controls_s::INDEX_THROTTLE) {
control = NAN_VALUE;
}
src/systemcmds/tests/test_mount.c
+1 -1
View File
@@ -132,7 +132,7 @@ test_mount(int argc, char *argv[])
}
PX4_INFO("Iterations left: #%d / #%d of %d / %d\n(%s)", it_left_fsync, it_left_abort,
fsync_tries, abort_tries, buf);
fsync_tries, abort_tries, buf);
int it_left_fsync_prev = it_left_fsync;
src/systemcmds/tests/test_uart_baudchange.c
+1 -1
View File
@@ -165,5 +165,5 @@ int test_uart_baudchange(int argc, char *argv[])
cleanup:
close(uart2);
return ret;
}
src/systemcmds/ver/ver.c
+7 -6
View File
@@ -58,7 +58,7 @@ static const char sz_ver_all_str[] = "all";
static const char mcu_ver_str[] = "mcu";
static const char mcu_uid_str[] = "uid";
const char* px4_git_version = PX4_GIT_VERSION_STR;
const char *px4_git_version = PX4_GIT_VERSION_STR;
const uint64_t px4_git_version_binary = PX4_GIT_VERSION_BINARY;
static void usage(const char *reason)
@@ -89,6 +89,7 @@ int ver_main(int argc, char *argv[])
if (ret == 0) {
printf("ver hwcmp match: %s\n", HW_ARCH);
}
return ret;
} else {
@@ -127,7 +128,7 @@ int ver_main(int argc, char *argv[])
if (show_all || !strncmp(argv[1], mcu_ver_str, sizeof(mcu_ver_str))) {
char rev;
char* revstr;
char *revstr;
int chip_version = mcu_version(&rev, &revstr);
@@ -139,9 +140,9 @@ int ver_main(int argc, char *argv[])
if (chip_version < MCU_REV_STM32F4_REV_3) {
printf("\nWARNING WARNING WARNING!\n"
"Revision %c has a silicon errata\n"
"This device can only utilize a maximum of 1MB flash safely!\n"
"http://px4.io/help/errata\n\n", rev);
"Revision %c has a silicon errata\n"
"This device can only utilize a maximum of 1MB flash safely!\n"
"http://px4.io/help/errata\n\n", rev);
}
}
@@ -153,7 +154,7 @@ int ver_main(int argc, char *argv[])
mcu_unique_id(uid);
printf("UID: %X:%X:%X \n",uid[0],uid[1],uid[2]);
printf("UID: %X:%X:%X \n", uid[0], uid[1], uid[2]);
ret = 0;
}