From 3defabfbea145242f7a78910241f114831046917 Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 26 Jun 2015 15:00:59 +0200 Subject: [PATCH 1/4] build land detector for posix --- makefiles/posix/config_posix_sitl.mk | 1 + 1 file changed, 1 insertion(+) diff --git a/makefiles/posix/config_posix_sitl.mk b/makefiles/posix/config_posix_sitl.mk index c317264143..806e59e7cb 100644 --- a/makefiles/posix/config_posix_sitl.mk +++ b/makefiles/posix/config_posix_sitl.mk @@ -40,6 +40,7 @@ MODULES += modules/position_estimator_inav MODULES += modules/navigator MODULES += modules/mc_pos_control MODULES += modules/mc_att_control +MODULES += modules/land_detector # # Library modules From 40cc11a5eda0161458a81f55d1c44a8f288655e1 Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 26 Jun 2015 15:01:17 +0200 Subject: [PATCH 2/4] ported land detector --- .../land_detector/FixedwingLandDetector.cpp | 4 +- .../land_detector/land_detector_main.cpp | 39 +++++++++++-------- 2 files changed, 25 insertions(+), 18 deletions(-) diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index 5f7ded9cb2..b98f3fd4ce 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -87,12 +87,12 @@ bool FixedwingLandDetector::update() if (hrt_elapsed_time(&_vehicleLocalPosition.timestamp) < 500 * 1000) { float val = 0.95f * _velocity_xy_filtered + 0.05f * sqrtf(_vehicleLocalPosition.vx * _vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy); - if (isfinite(val)) { + if (PX4_ISFINITE(val)) { _velocity_xy_filtered = val; } val = 0.95f * _velocity_z_filtered + 0.05f * fabsf(_vehicleLocalPosition.vz); - if (isfinite(val)) { + if (PX4_ISFINITE(val)) { _velocity_z_filtered = val; } } diff --git a/src/modules/land_detector/land_detector_main.cpp b/src/modules/land_detector/land_detector_main.cpp index 1ca319ce63..58753d5ca7 100644 --- a/src/modules/land_detector/land_detector_main.cpp +++ b/src/modules/land_detector/land_detector_main.cpp @@ -38,6 +38,10 @@ * @author Johan Jansen */ +#include +#include +#include +#include #include //usleep #include #include @@ -80,7 +84,7 @@ static void land_detector_deamon_thread(int argc, char *argv[]) static void land_detector_stop() { if (land_detector_task == nullptr || _landDetectorTaskID == -1) { - errx(1, "not running"); + warnx("not running"); return; } @@ -95,7 +99,7 @@ static void land_detector_stop() /* if we have given up, kill it */ if (++i > 50) { - task_delete(_landDetectorTaskID); + px4_task_delete(_landDetectorTaskID); break; } } while (land_detector_task->isRunning()); @@ -104,7 +108,7 @@ static void land_detector_stop() delete land_detector_task; land_detector_task = nullptr; _landDetectorTaskID = -1; - errx(0, "land_detector has been stopped"); + warnx("land_detector has been stopped"); } /** @@ -113,7 +117,7 @@ static void land_detector_stop() static int land_detector_start(const char *mode) { if (land_detector_task != nullptr || _landDetectorTaskID != -1) { - errx(1, "already running"); + warnx("already running"); return -1; } @@ -125,13 +129,13 @@ static int land_detector_start(const char *mode) land_detector_task = new MulticopterLandDetector(); } else { - errx(1, "[mode] must be either 'fixedwing' or 'multicopter'"); + warnx("[mode] must be either 'fixedwing' or 'multicopter'"); return -1; } //Check if alloc worked if (land_detector_task == nullptr) { - errx(1, "alloc failed"); + warnx("alloc failed"); return -1; } @@ -140,11 +144,11 @@ static int land_detector_start(const char *mode) SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 1000, - (main_t)&land_detector_deamon_thread, + (px4_main_t)&land_detector_deamon_thread, nullptr); if (_landDetectorTaskID < 0) { - errx(1, "task start failed: %d", -errno); + warnx("task start failed: %d", -errno); return -1; } @@ -163,9 +167,9 @@ static int land_detector_start(const char *mode) usleep(50000); if (hrt_absolute_time() > timeout) { - err(1, "start failed - timeout"); + warnx("start failed - timeout"); land_detector_stop(); - exit(1); + return 1; } } printf("\n"); @@ -174,7 +178,6 @@ static int land_detector_start(const char *mode) //Remember current active mode strncpy(_currentMode, mode, 12); - exit(0); return 0; } @@ -189,12 +192,15 @@ int land_detector_main(int argc, char *argv[]) } if (argc >= 2 && !strcmp(argv[1], "start")) { - land_detector_start(argv[2]); + if (land_detector_start(argv[2]) != 0) { + warnx("land_detector start failed"); + return 1; + } } if (!strcmp(argv[1], "stop")) { land_detector_stop(); - exit(0); + return 0; } if (!strcmp(argv[1], "status")) { @@ -204,13 +210,14 @@ int land_detector_main(int argc, char *argv[]) warnx("running (%s): %s", _currentMode, (land_detector_task->isLanded()) ? "LANDED" : "IN AIR"); } else { - errx(1, "exists, but not running (%s)", _currentMode); + warnx("exists, but not running (%s)", _currentMode); } - exit(0); + return 0; } else { - errx(1, "not running"); + warnx("not running"); + return 1; } } From c49511fb665127138bea698803067da1a9bdb5f3 Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 26 Jun 2015 15:01:53 +0200 Subject: [PATCH 3/4] start land detector for SITL --- posix-configs/SITL/init/rcS | 1 + 1 file changed, 1 insertion(+) diff --git a/posix-configs/SITL/init/rcS b/posix-configs/SITL/init/rcS index 445b7ce339..d9f94c5ce0 100644 --- a/posix-configs/SITL/init/rcS +++ b/posix-configs/SITL/init/rcS @@ -35,6 +35,7 @@ gpssim start hil mode_pwm commander start sensors start +land_detector start multicopter navigator start attitude_estimator_q start position_estimator_inav start From 47c4ece9eb01eb66e97df2b968a9878da284a79c Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 26 Jun 2015 15:02:12 +0200 Subject: [PATCH 4/4] use px4_poll instead of poll --- src/modules/navigator/navigator_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index d7f971f067..e4ef373d1b 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -309,7 +309,7 @@ Navigator::task_main() const hrt_abstime mavlink_open_interval = 500000; /* wakeup source(s) */ - struct pollfd fds[8]; + px4_pollfd_struct_t fds[8]; /* Setup of loop */ fds[0].fd = _global_pos_sub; @@ -332,7 +332,7 @@ Navigator::task_main() while (!_task_should_exit) { /* wait for up to 100ms for data */ - int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); if (pret == 0) { /* timed out - periodic check for _task_should_exit, etc. */