/** * Start new task, fails if it is already running. Returns OK if successful **/ static int land_detector_start(const char *mode) { if (land_detector_task != nullptr) { PX4_WARN("already running"); return -1; } //Allocate memory if (!strcmp(mode, "fixedwing")) { land_detector_task = new FixedwingLandDetector(); } else if (!strcmp(mode, "multicopter")) { land_detector_task = new MulticopterLandDetector(); } else if (!strcmp(mode, "vtol")) { land_detector_task = new VtolLandDetector(); } else { PX4_WARN("[mode] must be either 'fixedwing' or 'multicopter'"); return -1; } //Check if alloc worked if (land_detector_task == nullptr) { PX4_WARN("alloc failed"); return -1; } //Start new thread task int ret = land_detector_task->start(); if (ret) { PX4_WARN("task start failed: %d", -errno); return -1; } /* avoid memory fragmentation by not exiting start handler until the task has fully started */ const uint64_t timeout = hrt_absolute_time() + 5000000; //5 second timeout /* avoid printing dots just yet and do one sleep before the first check */ usleep(10000); /* check if the waiting involving dots and a newline are still needed */ if (!land_detector_task->isRunning()) { while (!land_detector_task->isRunning()) { usleep(50000); if (hrt_absolute_time() > timeout) { PX4_WARN("start failed - timeout"); land_detector_stop(); return 1; } } } //Remember current active mode strncpy(_currentMode, mode, 12); return 0; }
/** * Start new task, fails if it is already running. Returns OK if successful */ static int land_detector_start(const char *mode) { if (land_detector_task != nullptr) { PX4_WARN("already running"); return -1; } //Allocate memory if (!strcmp(mode, "fixedwing")) { land_detector_task = new FixedwingLandDetector(); } else if (!strcmp(mode, "multicopter")) { land_detector_task = new MulticopterLandDetector(); } else if (!strcmp(mode, "vtol")) { land_detector_task = new VtolLandDetector(); } else { PX4_WARN("[mode] must be either 'fixedwing', 'multicopter', or 'vtol'"); return -1; } // Check if alloc worked if (land_detector_task == nullptr) { PX4_WARN("alloc failed"); return -1; } // Start new thread task int ret = land_detector_task->start(); if (ret) { PX4_WARN("task start failed: %d", -errno); return -1; } // Avoid memory fragmentation by not exiting start handler until the task has fully started const uint64_t timeout = hrt_absolute_time() + 5000000; // 5 second timeout // Do one sleep before the first check usleep(10000); if (!land_detector_task->is_running()) { while (!land_detector_task->is_running()) { usleep(50000); if (hrt_absolute_time() > timeout) { PX4_WARN("start failed - timeout"); land_detector_stop(); return 1; } } } // Remember current active mode strncpy(_currentMode, mode, sizeof(_currentMode) - 1); _currentMode[sizeof(_currentMode) - 1] = '\0'; return 0; }
/** * Main entry point for this module **/ int land_detector_main(int argc, char *argv[]) { if (argc < 2) { goto exiterr; } if (argc >= 2 && !strcmp(argv[1], "start")) { if (land_detector_start(argv[2]) != 0) { warnx("land_detector start failed"); return 1; } return 0; } if (!strcmp(argv[1], "stop")) { land_detector_stop(); return 0; } if (!strcmp(argv[1], "status")) { if (land_detector_task) { if (land_detector_task->isRunning()) { warnx("running (%s): %s", _currentMode, (land_detector_task->isLanded()) ? "LANDED" : "IN AIR"); } else { warnx("exists, but not running (%s)", _currentMode); } return 0; } else { warnx("not running"); return 1; } } exiterr: warnx("usage: land_detector {start|stop|status} [mode]"); warnx("mode can either be 'fixedwing' or 'multicopter'"); return 1; }
/** * Start new task, fails if it is already running. Returns OK if successful **/ static int land_detector_start(const char *mode) { if (land_detector_task != nullptr || _landDetectorTaskID != -1) { errx(1, "already running"); return -1; } //Allocate memory if (!strcmp(mode, "fixedwing")) { land_detector_task = new FixedwingLandDetector(); } else if (!strcmp(mode, "multicopter")) { land_detector_task = new MulticopterLandDetector(); } else { errx(1, "[mode] must be either 'fixedwing' or 'multicopter'"); return -1; } //Check if alloc worked if (land_detector_task == nullptr) { errx(1, "alloc failed"); return -1; } //Start new thread task _landDetectorTaskID = task_spawn_cmd("land_detector", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 1000, (main_t)&land_detector_deamon_thread, nullptr); if (_landDetectorTaskID < 0) { errx(1, "task start failed: %d", -errno); return -1; } /* avoid memory fragmentation by not exiting start handler until the task has fully started */ const uint32_t timeout = hrt_absolute_time() + 5000000; //5 second timeout /* avoid printing dots just yet and do one sleep before the first check */ usleep(10000); /* check if the waiting involving dots and a newline are still needed */ if (!land_detector_task->isRunning()) { while (!land_detector_task->isRunning()) { printf("."); fflush(stdout); usleep(50000); if (hrt_absolute_time() > timeout) { err(1, "start failed - timeout"); land_detector_stop(); exit(1); } } printf("\n"); } //Remember current active mode strncpy(_currentMode, mode, 12); exit(0); return 0; }
/** * Main entry point for this module */ int land_detector_main(int argc, char *argv[]) { if (argc < 2) { goto exiterr; } if (argc >= 2 && !strcmp(argv[1], "start")) { if (land_detector_start(argv[2]) != 0) { PX4_WARN("land_detector start failed"); return 1; } return 0; } if (!strcmp(argv[1], "stop")) { land_detector_stop(); return 0; } if (!strcmp(argv[1], "status")) { if (land_detector_task) { if (land_detector_task->is_running()) { PX4_INFO("running (%s)", _currentMode); LandDetector::LandDetectionState state = land_detector_task->get_state(); switch (state) { case LandDetector::LandDetectionState::FLYING: PX4_INFO("State: Flying"); break; case LandDetector::LandDetectionState::LANDED: PX4_INFO("State: Landed"); break; case LandDetector::LandDetectionState::FREEFALL: PX4_INFO("State: Freefall"); break; default: PX4_ERR("State: unknown"); break; } } else { PX4_WARN("exists, but not running (%s)", _currentMode); } return 0; } else { PX4_WARN("not running"); return 1; } } exiterr: PX4_WARN("usage: land_detector {start|stop|status} [mode]"); PX4_WARN("mode can either be 'fixedwing' or 'multicopter'"); return 1; }