From a06b3e50ab4d4c73fd400ec2891b1ab7a9eb8451 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 29 Oct 2013 13:38:44 +0100 Subject: [PATCH 01/10] Only read 5 values, then return --- src/examples/px4_simple_app/px4_simple_app.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/examples/px4_simple_app/px4_simple_app.c b/src/examples/px4_simple_app/px4_simple_app.c index 7f655964d9..44e6dc7f35 100644 --- a/src/examples/px4_simple_app/px4_simple_app.c +++ b/src/examples/px4_simple_app/px4_simple_app.c @@ -71,7 +71,7 @@ int px4_simple_app_main(int argc, char *argv[]) int error_counter = 0; - while (true) { + for (int i = 0; i < 5; i++) { /* wait for sensor update of 1 file descriptor for 1000 ms (1 second) */ int poll_ret = poll(fds, 1, 1000); From 2fd1aed6be03de42d09c062871838ee7e852aa4a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 1 Nov 2013 12:02:45 +0100 Subject: [PATCH 02/10] Disable the segway app, as its GCC 4.7 incompatible and not generally used --- makefiles/config_px4fmu-v1_default.mk | 2 +- makefiles/config_px4fmu-v2_default.mk | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index f9061c1100..3068270865 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -79,7 +79,7 @@ MODULES += examples/flow_position_estimator # # Vehicle Control # -MODULES += modules/segway +#MODULES += modules/segway # XXX Needs GCC 4.7 fix MODULES += modules/fw_pos_control_l1 MODULES += modules/fw_att_control MODULES += modules/multirotor_att_control diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index ed2f2da5ed..761fb8d9d6 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -78,7 +78,7 @@ MODULES += examples/flow_position_estimator # # Vehicle Control # -MODULES += modules/segway +#MODULES += modules/segway # XXX Needs GCC 4.7 fix MODULES += modules/fw_pos_control_l1 MODULES += modules/fw_att_control MODULES += modules/multirotor_att_control @@ -120,7 +120,7 @@ MODULES += lib/conversion #MODULES += examples/math_demo # Tutorial code from # https://pixhawk.ethz.ch/px4/dev/hello_sky -#MODULES += examples/px4_simple_app +MODULES += examples/px4_simple_app # Tutorial code from # https://pixhawk.ethz.ch/px4/dev/daemon From 3eac9ce1596f90acf2e86f807b1b4efd3904a80b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 2 Nov 2013 16:16:49 +0100 Subject: [PATCH 03/10] fix usage of wrong value for max airspeed parameter --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index ffa7915a7d..a6653bfc7c 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -451,7 +451,7 @@ FixedwingPositionControl::parameters_update() _tecs.set_speed_weight(_parameters.speed_weight); _tecs.set_pitch_damping(_parameters.pitch_damping); _tecs.set_indicated_airspeed_min(_parameters.airspeed_min); - _tecs.set_indicated_airspeed_max(_parameters.airspeed_min); + _tecs.set_indicated_airspeed_max(_parameters.airspeed_max); _tecs.set_max_climb_rate(_parameters.max_climb_rate); /* sanity check parameters */ From ef7a425a45397fed510920b98a4ad08e62170f4c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 2 Nov 2013 17:33:45 +0100 Subject: [PATCH 04/10] fix vehicle_airspeed_poll logic: _tecs.enable_airspeed was not called before on valid airspeed --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index ffa7915a7d..28ca9776a0 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -501,7 +501,6 @@ FixedwingPositionControl::vehicle_airspeed_poll() orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); _airspeed_valid = true; _airspeed_last_valid = hrt_absolute_time(); - return true; } else { @@ -514,7 +513,7 @@ FixedwingPositionControl::vehicle_airspeed_poll() /* update TECS state */ _tecs.enable_airspeed(_airspeed_valid); - return false; + return airspeed_updated; } void From a4c99225c02e719d7900a533b777fd682eb5bd5c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 3 Nov 2013 16:49:02 +0100 Subject: [PATCH 05/10] initialize _vel_dot and _STEdotErrLast --- src/lib/external_lgpl/tecs/tecs.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index 4a98c8e974..f8f832ed76 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -54,7 +54,9 @@ public: _SPE_est(0.0f), _SKE_est(0.0f), _SPEdot(0.0f), - _SKEdot(0.0f) { + _SKEdot(0.0f), + _vel_dot(0.0f), + _STEdotErrLast(0.0f) { } From 98f5a77574fde4b2c41d28cc0d694f6ca250fba1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 3 Nov 2013 17:52:27 +0100 Subject: [PATCH 06/10] Fix to cancel pending callbacks for closing ORB topics --- src/modules/uORB/uORB.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index 7abbf42aed..50e433ec3c 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -249,9 +249,10 @@ ORBDevNode::close(struct file *filp) } else { SubscriberData *sd = filp_to_sd(filp); - if (sd != nullptr) + if (sd != nullptr) { + hrt_cancel(&sd->update_call); delete sd; - } + } return CDev::close(filp); } From 4865814f92c4a085972e317204c37042b609fdf8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 3 Nov 2013 17:58:28 +0100 Subject: [PATCH 07/10] Fixed typo, added testing - previous corner case now cleanly prevented --- src/modules/uORB/uORB.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index 50e433ec3c..149b8f6d24 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -253,6 +253,7 @@ ORBDevNode::close(struct file *filp) hrt_cancel(&sd->update_call); delete sd; } + } return CDev::close(filp); } From ba0687bc5e471809ae311ef98f3ddda3c29713b4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 3 Nov 2013 18:06:58 +0100 Subject: [PATCH 08/10] Matrix and Vector printing cleanup --- src/lib/mathlib/math/arm/Matrix.hpp | 6 +++--- src/lib/mathlib/math/arm/Vector.hpp | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/lib/mathlib/math/arm/Matrix.hpp b/src/lib/mathlib/math/arm/Matrix.hpp index 715fd3a5e8..1945bb02db 100644 --- a/src/lib/mathlib/math/arm/Matrix.hpp +++ b/src/lib/mathlib/math/arm/Matrix.hpp @@ -121,10 +121,10 @@ public: for (size_t i = 0; i < getRows(); i++) { for (size_t j = 0; j < getCols(); j++) { float sig; - int exp; + int exponent; float num = (*this)(i, j); - float2SigExp(num, sig, exp); - printf("%6.3fe%03.3d,", (double)sig, exp); + float2SigExp(num, sig, exponent); + printf("%6.3fe%03d ", (double)sig, exponent); } printf("\n"); diff --git a/src/lib/mathlib/math/arm/Vector.hpp b/src/lib/mathlib/math/arm/Vector.hpp index 4155800e86..52220fc152 100644 --- a/src/lib/mathlib/math/arm/Vector.hpp +++ b/src/lib/mathlib/math/arm/Vector.hpp @@ -109,10 +109,10 @@ public: inline void print() const { for (size_t i = 0; i < getRows(); i++) { float sig; - int exp; + int exponent; float num = (*this)(i); - float2SigExp(num, sig, exp); - printf("%6.3fe%03.3d,", (double)sig, exp); + float2SigExp(num, sig, exponent); + printf("%6.3fe%03d ", (double)sig, exponent); } printf("\n"); From b53d86ed681eb6c9f979bb10d5f487fa9c94d81b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 3 Nov 2013 18:26:02 +0100 Subject: [PATCH 09/10] Hotfix for mag calibration --- src/modules/commander/mag_calibration.cpp | 29 +++++++++++++++-------- 1 file changed, 19 insertions(+), 10 deletions(-) diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 09f4104f89..4ebf266f47 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -73,7 +73,7 @@ int do_mag_calibration(int mavlink_fd) /* maximum 500 values */ const unsigned int calibration_maxcount = 500; - unsigned int calibration_counter = 0; + unsigned int calibration_counter; struct mag_scale mscale_null = { 0.0f, @@ -99,28 +99,34 @@ int do_mag_calibration(int mavlink_fd) res = ioctl(fd, MAGIOCCALIBRATE, fd); if (res != OK) { - mavlink_log_critical(mavlink_fd, "ERROR: failed to calibrate scale"); + mavlink_log_critical(mavlink_fd, "Skipped scale calibration"); + /* this is non-fatal - mark it accordingly */ + res = OK; } } close(fd); - float *x; - float *y; - float *z; + float *x = NULL; + float *y = NULL; + float *z = NULL; if (res == OK) { /* allocate memory */ mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20); - x = (float *)malloc(sizeof(float) * calibration_maxcount); - y = (float *)malloc(sizeof(float) * calibration_maxcount); - z = (float *)malloc(sizeof(float) * calibration_maxcount); + x = reinterpret_cast(malloc(sizeof(float) * calibration_maxcount)); + y = reinterpret_cast(malloc(sizeof(float) * calibration_maxcount)); + z = reinterpret_cast(malloc(sizeof(float) * calibration_maxcount)); if (x == NULL || y == NULL || z == NULL) { mavlink_log_critical(mavlink_fd, "ERROR: out of memory"); res = ERROR; + return res; } + } else { + /* exit */ + return ERROR; } if (res == OK) { @@ -136,6 +142,8 @@ int do_mag_calibration(int mavlink_fd) mavlink_log_info(mavlink_fd, "rotate in a figure 8 around all axis"); + calibration_counter = 0; + while (hrt_absolute_time() < calibration_deadline && calibration_counter < calibration_maxcount) { @@ -178,6 +186,7 @@ int do_mag_calibration(int mavlink_fd) float sphere_radius; if (res == OK) { + /* sphere fit */ mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 70); sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius); @@ -270,7 +279,7 @@ int do_mag_calibration(int mavlink_fd) } else { mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); } - - return res; } + + return res; } From 791695ccd008859f6abe1a12d86b7be2ba811fec Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 3 Nov 2013 18:26:39 +0100 Subject: [PATCH 10/10] Hotfix: Check for out of range accel values --- .../preflight_check/preflight_check.c | 28 +++++++++++++++++-- 1 file changed, 26 insertions(+), 2 deletions(-) diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c index 1c58a2db69..07581459b4 100644 --- a/src/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -109,7 +109,7 @@ int preflight_check_main(int argc, char *argv[]) /* ---- ACCEL ---- */ close(fd); - fd = open(ACCEL_DEVICE_PATH, 0); + fd = open(ACCEL_DEVICE_PATH, O_RDONLY); ret = ioctl(fd, ACCELIOCSELFTEST, 0); if (ret != OK) { @@ -119,6 +119,29 @@ int preflight_check_main(int argc, char *argv[]) goto system_eval; } + /* check measurement result range */ + struct accel_report acc; + ret = read(fd, &acc, sizeof(acc)); + + if (ret == sizeof(acc)) { + /* evaluate values */ + if (sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z) > 30.0f /* m/s^2 */) { + warnx("accel with spurious values"); + mavlink_log_critical(mavlink_fd, "SENSOR FAIL: |ACCEL| > 30 m/s^2"); + /* this is frickin' fatal */ + fail_on_error = true; + system_ok = false; + goto system_eval; + } + } else { + warnx("accel read failed"); + mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACCEL READ"); + /* this is frickin' fatal */ + fail_on_error = true; + system_ok = false; + goto system_eval; + } + /* ---- GYRO ---- */ close(fd); @@ -193,9 +216,10 @@ system_eval: /* stop alarm */ ioctl(buzzer, TONE_SET_ALARM, TONE_STOP_TUNE); - /* switch on leds */ + /* switch off leds */ led_on(leds, LED_BLUE); led_on(leds, LED_AMBER); + close(leds); if (fail_on_error) { /* exit with error message */