mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-15 21:23:34 +08:00
@@ -57,6 +57,14 @@ LandDetector::LandDetector() :
|
||||
ScheduledWorkItem(px4::wq_configurations::hp_default),
|
||||
_cycle_perf(perf_alloc(PC_ELAPSED, "land_detector_cycle"))
|
||||
{
|
||||
_landDetected.timestamp = hrt_absolute_time();
|
||||
_landDetected.freefall = false;
|
||||
_landDetected.landed = true;
|
||||
_landDetected.ground_contact = false;
|
||||
_landDetected.maybe_landed = false;
|
||||
|
||||
_p_total_flight_time_high = param_find("LND_FLIGHT_T_HI");
|
||||
_p_total_flight_time_low = param_find("LND_FLIGHT_T_LO");
|
||||
}
|
||||
|
||||
LandDetector::~LandDetector()
|
||||
@@ -64,33 +72,15 @@ LandDetector::~LandDetector()
|
||||
perf_free(_cycle_perf);
|
||||
}
|
||||
|
||||
int LandDetector::start()
|
||||
void LandDetector::start()
|
||||
{
|
||||
ScheduleOnInterval(LAND_DETECTOR_UPDATE_INTERVAL, 10000);
|
||||
|
||||
return PX4_OK;
|
||||
ScheduleOnInterval(LAND_DETECTOR_UPDATE_INTERVAL);
|
||||
}
|
||||
|
||||
void LandDetector::Run()
|
||||
{
|
||||
perf_begin(_cycle_perf);
|
||||
|
||||
if (_object.load() == nullptr) { // not initialized yet
|
||||
// Advertise the first land detected uORB.
|
||||
_landDetected.timestamp = hrt_absolute_time();
|
||||
_landDetected.freefall = false;
|
||||
_landDetected.landed = true;
|
||||
_landDetected.ground_contact = false;
|
||||
_landDetected.maybe_landed = false;
|
||||
|
||||
_p_total_flight_time_high = param_find("LND_FLIGHT_T_HI");
|
||||
_p_total_flight_time_low = param_find("LND_FLIGHT_T_LO");
|
||||
|
||||
_check_params(true);
|
||||
|
||||
_object.store(this);
|
||||
}
|
||||
|
||||
_check_params(false);
|
||||
_armingSub.update(&_arming);
|
||||
_update_topics();
|
||||
|
||||
@@ -96,7 +96,7 @@ public:
|
||||
/**
|
||||
* Get the work queue going.
|
||||
*/
|
||||
int start();
|
||||
void start();
|
||||
|
||||
protected:
|
||||
|
||||
|
||||
@@ -62,10 +62,10 @@ int LandDetector::task_spawn(int argc, char *argv[])
|
||||
{
|
||||
if (argc < 2) {
|
||||
print_usage();
|
||||
return -1;
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
LandDetector *obj;
|
||||
LandDetector *obj = nullptr;
|
||||
|
||||
if (strcmp(argv[1], "fixedwing") == 0) {
|
||||
obj = new FixedwingLandDetector();
|
||||
@@ -81,28 +81,24 @@ int LandDetector::task_spawn(int argc, char *argv[])
|
||||
|
||||
} else {
|
||||
print_usage("unknown mode");
|
||||
return -1;
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
if (obj == nullptr) {
|
||||
PX4_ERR("alloc failed");
|
||||
return -1;
|
||||
}
|
||||
|
||||
int ret = obj->start();
|
||||
|
||||
if (ret < 0) {
|
||||
delete obj;
|
||||
return ret;
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
// Remember current active mode
|
||||
strncpy(_currentMode, argv[1], sizeof(_currentMode) - 1);
|
||||
_currentMode[sizeof(_currentMode) - 1] = '\0';
|
||||
|
||||
wait_until_running(); // this will wait until _object is set from the cycle method
|
||||
_object.store(obj);
|
||||
_task_id = task_id_is_work_queue;
|
||||
return 0;
|
||||
|
||||
obj->start();
|
||||
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
int LandDetector::print_status()
|
||||
|
||||
Reference in New Issue
Block a user