diff options
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/att_pos_estimator_ekf/kalman_main.cpp | 16 | ||||
-rw-r--r-- | src/modules/fixedwing_backside/fixedwing_backside_main.cpp | 18 | ||||
-rw-r--r-- | src/modules/px4iofirmware/mixer.cpp | 18 |
3 files changed, 31 insertions, 21 deletions
diff --git a/src/modules/att_pos_estimator_ekf/kalman_main.cpp b/src/modules/att_pos_estimator_ekf/kalman_main.cpp index 3b411031a..890184427 100644 --- a/src/modules/att_pos_estimator_ekf/kalman_main.cpp +++ b/src/modules/att_pos_estimator_ekf/kalman_main.cpp @@ -44,6 +44,7 @@ #include <string.h> #include <systemlib/systemlib.h> #include <systemlib/param/param.h> +#include <systemlib/err.h> #include <drivers/drv_hrt.h> #include <math.h> #include "KalmanNav.hpp" @@ -73,7 +74,7 @@ usage(const char *reason) if (reason) fprintf(stderr, "%s\n", reason); - fprintf(stderr, "usage: kalman_demo {start|stop|status} [-p <additional params>]\n\n"); + warnx("usage: att_pos_estimator_ekf {start|stop|status} [-p <additional params>]"); exit(1); } @@ -94,13 +95,14 @@ int att_pos_estimator_ekf_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { if (thread_running) { - printf("kalman_demo already running\n"); + warnx("already running"); /* this is not an error */ exit(0); } thread_should_exit = false; - deamon_task = task_spawn_cmd("kalman_demo", + + deamon_task = task_spawn_cmd("att_pos_estimator_ekf", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 4096, @@ -116,10 +118,10 @@ int att_pos_estimator_ekf_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { - printf("\tkalman_demo app is running\n"); + warnx("is running\n"); } else { - printf("\tkalman_demo app not started\n"); + warnx("not started\n"); } exit(0); @@ -132,7 +134,7 @@ int att_pos_estimator_ekf_main(int argc, char *argv[]) int kalman_demo_thread_main(int argc, char *argv[]) { - printf("[kalman_demo] starting\n"); + warnx("starting\n"); using namespace math; @@ -144,7 +146,7 @@ int kalman_demo_thread_main(int argc, char *argv[]) nav.update(); } - printf("[kalman_demo] exiting.\n"); + printf("exiting.\n"); thread_running = false; diff --git a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp index e2330427d..4803a526e 100644 --- a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp +++ b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp @@ -47,6 +47,7 @@ #include <systemlib/systemlib.h> #include <controllib/fixedwing.hpp> #include <systemlib/param/param.h> +#include <systemlib/err.h> #include <drivers/drv_hrt.h> #include <math.h> @@ -80,7 +81,7 @@ usage(const char *reason) if (reason) fprintf(stderr, "%s\n", reason); - fprintf(stderr, "usage: control_demo {start|stop|status} [-p <additional params>]\n\n"); + fprintf(stderr, "usage: fixedwing_backside {start|stop|status} [-p <additional params>]\n\n"); exit(1); } @@ -101,13 +102,14 @@ int fixedwing_backside_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { if (thread_running) { - printf("control_demo already running\n"); + warnx("already running"); /* this is not an error */ exit(0); } thread_should_exit = false; - deamon_task = task_spawn_cmd("control_demo", + + deamon_task = task_spawn_cmd("fixedwing_backside", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 10, 5120, @@ -128,10 +130,10 @@ int fixedwing_backside_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { - printf("\tcontrol_demo app is running\n"); + warnx("is running"); } else { - printf("\tcontrol_demo app not started\n"); + warnx("not started"); } exit(0); @@ -144,7 +146,7 @@ int fixedwing_backside_main(int argc, char *argv[]) int control_demo_thread_main(int argc, char *argv[]) { - printf("[control_Demo] starting\n"); + warnx("starting"); using namespace control; @@ -156,7 +158,7 @@ int control_demo_thread_main(int argc, char *argv[]) autopilot.update(); } - printf("[control_demo] exiting.\n"); + warnx("exiting."); thread_running = false; @@ -165,6 +167,6 @@ int control_demo_thread_main(int argc, char *argv[]) void test() { - printf("beginning control lib test\n"); + warnx("beginning control lib test"); control::basicBlocksTest(); } diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 0b8ed6dc5..a2193b526 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -294,8 +294,7 @@ mixer_handle_text(const void *buffer, size_t length) case F2I_MIXER_ACTION_APPEND: isr_debug(2, "append %d", length); - /* check for overflow - this is really fatal */ - /* XXX could add just what will fit & try to parse, then repeat... */ + /* check for overflow - this would be really fatal */ if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) { r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; return; @@ -314,8 +313,13 @@ mixer_handle_text(const void *buffer, size_t length) /* if anything was parsed */ if (resid != mixer_text_length) { - /* ideally, this should test resid == 0 ? */ - r_status_flags |= PX4IO_P_STATUS_FLAGS_MIXER_OK; + /* only set mixer ok if no residual is left over */ + if (resid == 0) { + r_status_flags |= PX4IO_P_STATUS_FLAGS_MIXER_OK; + } else { + /* not yet reached the end of the mixer, set as not ok */ + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; + } isr_debug(2, "used %u", mixer_text_length - resid); @@ -338,11 +342,13 @@ mixer_set_failsafe() { /* * Check if a custom failsafe value has been written, - * else use the opportunity to set decent defaults. + * or if the mixer is not ok and bail out. */ - if (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) + if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) || + !(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) return; + /* set failsafe defaults to the values for all inputs = 0 */ float outputs[IO_SERVO_COUNT]; unsigned mixed; |