diff --git a/.travis.yml b/.travis.yml index ecc69a0c16..20b15d323d 100644 --- a/.travis.yml +++ b/.travis.yml @@ -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' diff --git a/Makefile b/Makefile index bf399d19bf..3c47aad3e0 100644 --- a/Makefile +++ b/Makefile @@ -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 diff --git a/Tools/check_code_style.sh b/Tools/check_code_style.sh index b8806f3164..d123b3623c 100755 --- a/Tools/check_code_style.sh +++ b/Tools/check_code_style.sh @@ -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 diff --git a/Tools/fix_code_style.sh b/Tools/fix_code_style.sh index f1818f2f54..d8b424ae56 100755 --- a/Tools/fix_code_style.sh +++ b/Tools/fix_code_style.sh @@ -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 \ diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c index d6e78fd821..3d6aa88269 100644 --- a/src/examples/fixedwing_control/main.c +++ b/src/examples/fixedwing_control/main.c @@ -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); } diff --git a/src/examples/flow_position_estimator/flow_position_estimator_main.c b/src/examples/flow_position_estimator/flow_position_estimator_main.c index 027511df20..bcf13c4de5 100644 --- a/src/examples/flow_position_estimator/flow_position_estimator_main.c +++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c @@ -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); } diff --git a/src/examples/matlab_csv_serial/matlab_csv_serial.c b/src/examples/matlab_csv_serial/matlab_csv_serial.c index 139a182b22..b949400e47 100644 --- a/src/examples/matlab_csv_serial/matlab_csv_serial.c +++ b/src/examples/matlab_csv_serial/matlab_csv_serial.c @@ -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); } diff --git a/src/examples/publisher/publisher_start_nuttx.cpp b/src/examples/publisher/publisher_start_nuttx.cpp index 2379d7aa68..b07207e8a5 100644 --- a/src/examples/publisher/publisher_start_nuttx.cpp +++ b/src/examples/publisher/publisher_start_nuttx.cpp @@ -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); } diff --git a/src/examples/px4_daemon_app/px4_daemon_app.c b/src/examples/px4_daemon_app/px4_daemon_app.c index 9154c7e5fc..1495b1122d 100644 --- a/src/examples/px4_daemon_app/px4_daemon_app.c +++ b/src/examples/px4_daemon_app/px4_daemon_app.c @@ -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; } diff --git a/src/examples/rover_steering_control/main.cpp b/src/examples/rover_steering_control/main.cpp index 887ea86309..7a227c2366 100644 --- a/src/examples/rover_steering_control/main.cpp +++ b/src/examples/rover_steering_control/main.cpp @@ -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); } diff --git a/src/examples/subscriber/subscriber_start_nuttx.cpp b/src/examples/subscriber/subscriber_start_nuttx.cpp index 424e49e9c1..16644280be 100644 --- a/src/examples/subscriber/subscriber_start_nuttx.cpp +++ b/src/examples/subscriber/subscriber_start_nuttx.cpp @@ -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); } diff --git a/src/systemcmds/bl_update/bl_update.c b/src/systemcmds/bl_update/bl_update.c index 082865a866..24271c3315 100644 --- a/src/systemcmds/bl_update/bl_update.c +++ b/src/systemcmds/bl_update/bl_update.c @@ -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); diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c index 210b4caa1f..362af65353 100644 --- a/src/systemcmds/config/config.c +++ b/src/systemcmds/config/config.c @@ -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); } diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c index 8b432f1638..8da9580207 100644 --- a/src/systemcmds/esc_calib/esc_calib.c +++ b/src/systemcmds/esc_calib/esc_calib.c @@ -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_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< \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; } diff --git a/src/systemcmds/motor_test/motor_test.c b/src/systemcmds/motor_test/motor_test.c index 2108b2004e..3581c2004b 100644 --- a/src/systemcmds/motor_test/motor_test.c +++ b/src/systemcmds/motor_test/motor_test.c @@ -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 Motor to test (0..7)\n" - " -p Power (0..100)\n"); + "usage:\n" + "motor_test\n" + " -m Motor to test (0..7)\n" + " -p 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); } diff --git a/src/systemcmds/mtd/24xxxx_mtd.c b/src/systemcmds/mtd/24xxxx_mtd.c index 2aba9dbb69..9fc2786831 100644 --- a/src/systemcmds/mtd/24xxxx_mtd.c +++ b/src/systemcmds/mtd/24xxxx_mtd.c @@ -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); diff --git a/src/systemcmds/mtd/mtd.c b/src/systemcmds/mtd/mtd.c index c1dbcad709..0f54c9c4c3 100644 --- a/src/systemcmds/mtd/mtd.c +++ b/src/systemcmds/mtd/mtd.c @@ -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); } diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c index 784c64bf1f..56d4f9f3af 100644 --- a/src/systemcmds/nshterm/nshterm.c +++ b/src/systemcmds/nshterm/nshterm.c @@ -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 \n"); - exit(1); - } - unsigned retries = 0; - int fd = -1; - int armed_fd = orb_subscribe(ORB_ID(actuator_armed)); - struct actuator_armed_s armed; - - /* back off 1800 ms to avoid running into the USB setup timing */ - while (hrt_absolute_time() < 1800U * 1000U) { - usleep(50000); - } - - /* try to bring up the console - stop doing so if the system gets armed */ - while (true) { - - /* 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); - - if (armed.armed) { - /* this is not an error, but we are done */ - exit(0); - } - } - - /* 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); - } - - /* set up the serial port with output processing */ - - /* Try to set baud rate */ - struct termios uart_config; - int termios_state; - - /* 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; + if (argc < 2) { + printf("Usage: nshterm \n"); + exit(1); + } + + unsigned retries = 0; + int fd = -1; + int armed_fd = orb_subscribe(ORB_ID(actuator_armed)); + struct actuator_armed_s armed; + + /* back off 1800 ms to avoid running into the USB setup timing */ + while (hrt_absolute_time() < 1800U * 1000U) { + usleep(50000); + } + + /* try to bring up the console - stop doing so if the system gets armed */ + while (true) { + + /* 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); + + if (armed.armed) { + /* this is not an error, but we are done */ + exit(0); + } + } + + /* 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); + } + + /* set up the serial port with output processing */ + + /* Try to set baud rate */ + struct termios uart_config; + int termios_state; + + /* 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; } diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index 614c7dc0d6..69d56dffcd 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -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("\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; } diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index 024a54060c..a119952963 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -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); diff --git a/src/systemcmds/reboot/reboot.c b/src/systemcmds/reboot/reboot.c index 8de6b09985..8309d74ea4 100644 --- a/src/systemcmds/reboot/reboot.c +++ b/src/systemcmds/reboot/reboot.c @@ -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"); } } diff --git a/src/systemcmds/reflect/reflect.c b/src/systemcmds/reflect/reflect.c index fe5f0f0359..683ff7c6ea 100644 --- a/src/systemcmds/reflect/reflect.c +++ b/src/systemcmds/reflect/reflect.c @@ -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; iv)/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; nv)/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"); - - 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; + uint32_t total = 0; + printf("Starting reflector\n"); + + 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; } diff --git a/src/systemcmds/tests/test_bson.c b/src/systemcmds/tests/test_bson.c index bba1ae4f12..945b00aa51 100644 --- a/src/systemcmds/tests/test_bson.c +++ b/src/systemcmds/tests/test_bson.c @@ -39,7 +39,7 @@ #define __STDC_FORMAT_MACROS #include - + #include #include #include @@ -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; } diff --git a/src/systemcmds/tests/test_conv.cpp b/src/systemcmds/tests/test_conv.cpp index 8718342eb9..e451755959 100644 --- a/src/systemcmds/tests/test_conv.cpp +++ b/src/systemcmds/tests/test_conv.cpp @@ -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; } } diff --git a/src/systemcmds/tests/test_eigen.cpp b/src/systemcmds/tests/test_eigen.cpp index 0f71260e5d..ba3922784b 100644 --- a/src/systemcmds/tests/test_eigen.cpp +++ b/src/systemcmds/tests/test_eigen.cpp @@ -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 m1_orig = (Eigen::Matrix() << 1, 2, 3, - 4, 5, 6).finished(); + 4, 5, 6).finished(); Eigen::Matrix 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()); diff --git a/src/systemcmds/tests/test_file2.c b/src/systemcmds/tests/test_file2.c index 630b9adc48..56d9f0d83a 100644 --- a/src/systemcmds/tests/test_file2.c +++ b/src/systemcmds/tests/test_file2.c @@ -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': diff --git a/src/systemcmds/tests/test_hott_telemetry.c b/src/systemcmds/tests/test_hott_telemetry.c index 430d0445d8..d1e9281191 100644 --- a/src/systemcmds/tests/test_hott_telemetry.c +++ b/src/systemcmds/tests/test_hott_telemetry.c @@ -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 { diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index a8a4878ccb..98b65f10d1 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -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; } diff --git a/src/systemcmds/tests/test_mount.c b/src/systemcmds/tests/test_mount.c index 64eefa0b47..f92c5f6316 100644 --- a/src/systemcmds/tests/test_mount.c +++ b/src/systemcmds/tests/test_mount.c @@ -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; diff --git a/src/systemcmds/tests/test_uart_baudchange.c b/src/systemcmds/tests/test_uart_baudchange.c index 8c9a5ff35f..7c138efca1 100644 --- a/src/systemcmds/tests/test_uart_baudchange.c +++ b/src/systemcmds/tests/test_uart_baudchange.c @@ -165,5 +165,5 @@ int test_uart_baudchange(int argc, char *argv[]) cleanup: close(uart2); return ret; - + } diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c index 0328621b3d..28cd988627 100644 --- a/src/systemcmds/ver/ver.c +++ b/src/systemcmds/ver/ver.c @@ -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; }