land_detector initiate cycle immediately

- fixes #12190
This commit is contained in:
Daniel Agar
2019-06-15 21:30:09 -04:00
committed by GitHub
parent ce784d1ef0
commit 106ee280c5
3 changed files with 20 additions and 34 deletions
+10 -20
View File
@@ -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();
+1 -1
View File
@@ -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()