/** * 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; }