diff options
author | Johan Jansen <jnsn.johan@gmail.com> | 2015-01-06 13:33:35 +0100 |
---|---|---|
committer | Johan Jansen <jnsn.johan@gmail.com> | 2015-01-15 14:37:51 +0100 |
commit | 546b5727b442ac7520d7ce72e15732378a1a0799 (patch) | |
tree | 35124733632421ff12504da7c59f5b5af5c6d11d /src | |
parent | 98ab83142c1bf7e5170e7527dde1a9e5132b5422 (diff) | |
download | px4-firmware-546b5727b442ac7520d7ce72e15732378a1a0799.tar.gz px4-firmware-546b5727b442ac7520d7ce72e15732378a1a0799.tar.bz2 px4-firmware-546b5727b442ac7520d7ce72e15732378a1a0799.zip |
Formatting: Run AStyle to fix indenting
Diffstat (limited to 'src')
-rw-r--r-- | src/modules/fw_land_detector/FixedwingLandDetector.cpp | 182 | ||||
-rw-r--r-- | src/modules/fw_land_detector/FixedwingLandDetector.h | 88 | ||||
-rw-r--r-- | src/modules/fw_land_detector/fw_land_detector_main.cpp | 194 | ||||
-rw-r--r-- | src/modules/mc_land_detector/MulticopterLandDetector.cpp | 239 | ||||
-rw-r--r-- | src/modules/mc_land_detector/MulticopterLandDetector.h | 111 | ||||
-rw-r--r-- | src/modules/mc_land_detector/mc_land_detector_main.cpp | 30 |
6 files changed, 428 insertions, 416 deletions
diff --git a/src/modules/fw_land_detector/FixedwingLandDetector.cpp b/src/modules/fw_land_detector/FixedwingLandDetector.cpp index 6f5eca3ce..5fbeb2f45 100644 --- a/src/modules/fw_land_detector/FixedwingLandDetector.cpp +++ b/src/modules/fw_land_detector/FixedwingLandDetector.cpp @@ -46,37 +46,37 @@ #include <unistd.h> //usleep FixedwingLandDetector::FixedwingLandDetector() : - _landDetectedPub(-1), - _landDetected({0,false}), - - _vehicleLocalPositionSub(-1), - _vehicleLocalPosition({}), - _airspeedSub(-1), - _airspeed({}), - - _velocity_xy_filtered(0.0f), - _velocity_z_filtered(0.0f), - _airspeed_filtered(0.0f), - _landDetectTrigger(0), - - _taskShouldExit(false), - _taskIsRunning(false) + _landDetectedPub(-1), + _landDetected({0, false}), + + _vehicleLocalPositionSub(-1), + _vehicleLocalPosition({}), + _airspeedSub(-1), + _airspeed({}), + + _velocity_xy_filtered(0.0f), + _velocity_z_filtered(0.0f), + _airspeed_filtered(0.0f), + _landDetectTrigger(0), + + _taskShouldExit(false), + _taskIsRunning(false) { - //Advertise the first land detected uORB - _landDetected.timestamp = hrt_absolute_time(); - _landDetected.landed = false; - _landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected); + //Advertise the first land detected uORB + _landDetected.timestamp = hrt_absolute_time(); + _landDetected.landed = false; + _landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected); } FixedwingLandDetector::~FixedwingLandDetector() { - _taskShouldExit = true; - close(_landDetectedPub); - } + _taskShouldExit = true; + close(_landDetectedPub); +} void FixedwingLandDetector::shutdown() { - _taskShouldExit = true; + _taskShouldExit = true; } /** @@ -85,87 +85,89 @@ void FixedwingLandDetector::shutdown() **/ static bool orb_update(const struct orb_metadata *meta, int handle, void *buffer) { - bool newData = false; + bool newData = false; - //Check if there is new data to grab - if(orb_check(handle, &newData) != OK) { - return false; - } + //Check if there is new data to grab + if (orb_check(handle, &newData) != OK) { + return false; + } - if(!newData) { - return false; - } + if (!newData) { + return false; + } - if(orb_copy(meta, handle, buffer) != OK) { - return false; - } + if (orb_copy(meta, handle, buffer) != OK) { + return false; + } - return true; + return true; } void FixedwingLandDetector::updateSubscriptions() { - orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition); - orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed); + orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition); + orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed); } void FixedwingLandDetector::landDetectorLoop() { - //This should never happen! - if(_taskIsRunning) return; - - //Subscribe to local position and airspeed data - _vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position)); - _airspeedSub = orb_subscribe(ORB_ID(airspeed)); - - _taskIsRunning = true; - _taskShouldExit = false; - while (!_taskShouldExit) { - - //First poll for new data from our subscriptions - updateSubscriptions(); - - const uint64_t now = hrt_absolute_time(); - bool landDetected = false; - - //TODO: reset filtered values on arming? - _velocity_xy_filtered = 0.95f*_velocity_xy_filtered + 0.05f*sqrtf(_vehicleLocalPosition.vx*_vehicleLocalPosition.vx + _vehicleLocalPosition.vy*_vehicleLocalPosition.vy); - _velocity_z_filtered = 0.95f*_velocity_z_filtered + 0.05f*fabsf(_vehicleLocalPosition.vz); - _airspeed_filtered = 0.95f*_airspeed_filtered + 0.05f*_airspeed.true_airspeed_m_s; - - /* crude land detector for fixedwing */ - if (_velocity_xy_filtered < FW_LAND_DETECTOR_VELOCITY_MAX - && _velocity_z_filtered < FW_LAND_DETECTOR_CLIMBRATE_MAX - && _airspeed_filtered < FW_LAND_DETECTOR_AIRSPEED_MAX) { - - //These conditions need to be stable for a period of time before we trust them - if(now > _landDetectTrigger) { - landDetected = true; - } - } - else { - //reset land detect trigger - _landDetectTrigger = now + FW_LAND_DETECTOR_TRIGGER_TIME; - } - - //Publish if land detection state has changed - if(_landDetected.landed != landDetected) { - _landDetected.timestamp = now; - _landDetected.landed = landDetected; - - /* publish the land detected broadcast */ - orb_publish(ORB_ID(vehicle_land_detected), _landDetectedPub, &_landDetected); - } - - //Limit loop rate - usleep(1000000 / FW_LAND_DETECTOR_UPDATE_RATE); - } - - _taskIsRunning = false; - _exit(0); + //This should never happen! + if (_taskIsRunning) { return; } + + //Subscribe to local position and airspeed data + _vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position)); + _airspeedSub = orb_subscribe(ORB_ID(airspeed)); + + _taskIsRunning = true; + _taskShouldExit = false; + + while (!_taskShouldExit) { + + //First poll for new data from our subscriptions + updateSubscriptions(); + + const uint64_t now = hrt_absolute_time(); + bool landDetected = false; + + //TODO: reset filtered values on arming? + _velocity_xy_filtered = 0.95f * _velocity_xy_filtered + 0.05f * sqrtf(_vehicleLocalPosition.vx * + _vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy); + _velocity_z_filtered = 0.95f * _velocity_z_filtered + 0.05f * fabsf(_vehicleLocalPosition.vz); + _airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s; + + /* crude land detector for fixedwing */ + if (_velocity_xy_filtered < FW_LAND_DETECTOR_VELOCITY_MAX + && _velocity_z_filtered < FW_LAND_DETECTOR_CLIMBRATE_MAX + && _airspeed_filtered < FW_LAND_DETECTOR_AIRSPEED_MAX) { + + //These conditions need to be stable for a period of time before we trust them + if (now > _landDetectTrigger) { + landDetected = true; + } + + } else { + //reset land detect trigger + _landDetectTrigger = now + FW_LAND_DETECTOR_TRIGGER_TIME; + } + + //Publish if land detection state has changed + if (_landDetected.landed != landDetected) { + _landDetected.timestamp = now; + _landDetected.landed = landDetected; + + /* publish the land detected broadcast */ + orb_publish(ORB_ID(vehicle_land_detected), _landDetectedPub, &_landDetected); + } + + //Limit loop rate + usleep(1000000 / FW_LAND_DETECTOR_UPDATE_RATE); + } + + _taskIsRunning = false; + _exit(0); } bool FixedwingLandDetector::isRunning() const { - return _taskIsRunning; + return _taskIsRunning; } diff --git a/src/modules/fw_land_detector/FixedwingLandDetector.h b/src/modules/fw_land_detector/FixedwingLandDetector.h index 900445740..be9b17b74 100644 --- a/src/modules/fw_land_detector/FixedwingLandDetector.h +++ b/src/modules/fw_land_detector/FixedwingLandDetector.h @@ -46,61 +46,63 @@ #include <uORB/topics/vehicle_local_position.h> #include <uORB/topics/airspeed.h> -class FixedwingLandDetector { +class FixedwingLandDetector +{ public: - FixedwingLandDetector(); - ~FixedwingLandDetector(); + FixedwingLandDetector(); + ~FixedwingLandDetector(); - /** - * @brief Executes the main loop of the land detector in a separate deamon thread - * @returns OK if task was successfully launched - **/ - int createDeamonThread(); + /** + * @brief Executes the main loop of the land detector in a separate deamon thread + * @returns OK if task was successfully launched + **/ + int createDeamonThread(); - /** - * @returns true if this task is currently running - **/ - bool isRunning() const; + /** + * @returns true if this task is currently running + **/ + bool isRunning() const; - /** - * @brief blocking loop, should be run in a separate thread or task. Runs at 50Hz - **/ - void landDetectorLoop(); + /** + * @brief blocking loop, should be run in a separate thread or task. Runs at 50Hz + **/ + void landDetectorLoop(); - /** - * @brief Tells the Land Detector task that it should exit - **/ - void shutdown(); + /** + * @brief Tells the Land Detector task that it should exit + **/ + void shutdown(); protected: - /** - * @brief polls all subscriptions and pulls any data that has changed - **/ - void updateSubscriptions(); + /** + * @brief polls all subscriptions and pulls any data that has changed + **/ + void updateSubscriptions(); - //Algorithm parameters (TODO: should probably be externalized) - static constexpr uint32_t FW_LAND_DETECTOR_UPDATE_RATE = 50; /**< Run algorithm at 50Hz */ - static constexpr uint64_t FW_LAND_DETECTOR_TRIGGER_TIME = 2000000; /**< usec that landing conditions have to hold before triggering a land */ - static constexpr float FW_LAND_DETECTOR_VELOCITY_MAX = 5.0f; /**< maximum horizontal movement m/s*/ - static constexpr float FW_LAND_DETECTOR_CLIMBRATE_MAX = 10.00f; /**< +- climb rate in m/s*/ - static constexpr float FW_LAND_DETECTOR_AIRSPEED_MAX = 10.00f; /**< airspeed max m/s*/ + //Algorithm parameters (TODO: should probably be externalized) + static constexpr uint32_t FW_LAND_DETECTOR_UPDATE_RATE = 50; /**< Run algorithm at 50Hz */ + static constexpr uint64_t FW_LAND_DETECTOR_TRIGGER_TIME = + 2000000; /**< usec that landing conditions have to hold before triggering a land */ + static constexpr float FW_LAND_DETECTOR_VELOCITY_MAX = 5.0f; /**< maximum horizontal movement m/s*/ + static constexpr float FW_LAND_DETECTOR_CLIMBRATE_MAX = 10.00f; /**< +- climb rate in m/s*/ + static constexpr float FW_LAND_DETECTOR_AIRSPEED_MAX = 10.00f; /**< airspeed max m/s*/ private: - orb_advert_t _landDetectedPub; /**< publisher for position in local frame */ - struct vehicle_land_detected_s _landDetected; /**< local vehicle position */ + orb_advert_t _landDetectedPub; /**< publisher for position in local frame */ + struct vehicle_land_detected_s _landDetected; /**< local vehicle position */ - int _vehicleLocalPositionSub; /**< notification of local position */ - struct vehicle_local_position_s _vehicleLocalPosition; /**< the result from local position subscription */ - int _airspeedSub; - struct airspeed_s _airspeed; + int _vehicleLocalPositionSub; /**< notification of local position */ + struct vehicle_local_position_s _vehicleLocalPosition; /**< the result from local position subscription */ + int _airspeedSub; + struct airspeed_s _airspeed; - float _velocity_xy_filtered; - float _velocity_z_filtered; - float _airspeed_filtered; - uint64_t _landDetectTrigger; - - bool _taskShouldExit; /**< true if it is requested that this task should exit */ - bool _taskIsRunning; /**< task has reached main loop and is currently running */ + float _velocity_xy_filtered; + float _velocity_z_filtered; + float _airspeed_filtered; + uint64_t _landDetectTrigger; + + bool _taskShouldExit; /**< true if it is requested that this task should exit */ + bool _taskIsRunning; /**< task has reached main loop and is currently running */ }; #endif //__FIXED_WING_LAND_DETECTOR_H__ diff --git a/src/modules/fw_land_detector/fw_land_detector_main.cpp b/src/modules/fw_land_detector/fw_land_detector_main.cpp index a9eefc406..a45a51863 100644 --- a/src/modules/fw_land_detector/fw_land_detector_main.cpp +++ b/src/modules/fw_land_detector/fw_land_detector_main.cpp @@ -61,7 +61,7 @@ static void fw_land_detector_stop(); extern "C" __EXPORT int fw_land_detector_main(int argc, char *argv[]); //Private variables -static FixedwingLandDetector* fw_land_detector_task = nullptr; +static FixedwingLandDetector *fw_land_detector_task = nullptr; static int _landDetectorTaskID = -1; /** @@ -69,7 +69,7 @@ static int _landDetectorTaskID = -1; **/ static void fw_land_detector_deamon_thread(int argc, char *argv[]) { - fw_land_detector_task->landDetectorLoop(); + fw_land_detector_task->landDetectorLoop(); } /** @@ -77,31 +77,32 @@ static void fw_land_detector_deamon_thread(int argc, char *argv[]) **/ static void fw_land_detector_stop() { - if(fw_land_detector_task == nullptr || _landDetectorTaskID == -1) { - errx(1, "not running"); - return; - } - - fw_land_detector_task->shutdown(); - - //Wait for task to die - int i = 0; - do { - /* wait 20ms */ - usleep(20000); - - /* if we have given up, kill it */ - if (++i > 50) { - task_delete(_landDetectorTaskID); - break; - } - } while (fw_land_detector_task->isRunning()); - - - delete fw_land_detector_task; - fw_land_detector_task = nullptr; - _landDetectorTaskID = -1; - warn("fw_land_detector has been stopped"); + if (fw_land_detector_task == nullptr || _landDetectorTaskID == -1) { + errx(1, "not running"); + return; + } + + fw_land_detector_task->shutdown(); + + //Wait for task to die + int i = 0; + + do { + /* wait 20ms */ + usleep(20000); + + /* if we have given up, kill it */ + if (++i > 50) { + task_delete(_landDetectorTaskID); + break; + } + } while (fw_land_detector_task->isRunning()); + + + delete fw_land_detector_task; + fw_land_detector_task = nullptr; + _landDetectorTaskID = -1; + warn("fw_land_detector has been stopped"); } /** @@ -109,48 +110,51 @@ static void fw_land_detector_stop() **/ static int fw_land_detector_start() { - if(fw_land_detector_task != nullptr || _landDetectorTaskID != -1) { - errx(1, "already running"); - return -1; - } - - //Allocate memory - fw_land_detector_task = new FixedwingLandDetector(); - if (fw_land_detector_task == nullptr) { - errx(1, "alloc failed"); - return -1; - } - - //Start new thread task - _landDetectorTaskID = task_spawn_cmd("fw_land_detector", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 1024, - (main_t)&fw_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 - while (!fw_land_detector_task->isRunning()) { - usleep(50000); - printf("."); - fflush(stdout); - - if(hrt_absolute_time() > timeout) { - err(1, "start failed - timeout"); - fw_land_detector_stop(); - exit(1); - } - } - printf("\n"); - - exit(0); - return 0; + if (fw_land_detector_task != nullptr || _landDetectorTaskID != -1) { + errx(1, "already running"); + return -1; + } + + //Allocate memory + fw_land_detector_task = new FixedwingLandDetector(); + + if (fw_land_detector_task == nullptr) { + errx(1, "alloc failed"); + return -1; + } + + //Start new thread task + _landDetectorTaskID = task_spawn_cmd("fw_land_detector", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 1024, + (main_t)&fw_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 + + while (!fw_land_detector_task->isRunning()) { + usleep(50000); + printf("."); + fflush(stdout); + + if (hrt_absolute_time() > timeout) { + err(1, "start failed - timeout"); + fw_land_detector_stop(); + exit(1); + } + } + + printf("\n"); + + exit(0); + return 0; } /** @@ -159,37 +163,37 @@ static int fw_land_detector_start() int fw_land_detector_main(int argc, char *argv[]) { - if (argc < 1) { - warnx("usage: fw_land_detector {start|stop|status}"); - exit(0); - } + if (argc < 1) { + warnx("usage: fw_land_detector {start|stop|status}"); + exit(0); + } + + if (!strcmp(argv[1], "start")) { + fw_land_detector_start(); + } - if (!strcmp(argv[1], "start")) { - fw_land_detector_start(); - } + if (!strcmp(argv[1], "stop")) { + fw_land_detector_stop(); + exit(0); + } - if (!strcmp(argv[1], "stop")) { - fw_land_detector_stop(); - exit(0); - } + if (!strcmp(argv[1], "status")) { + if (fw_land_detector_task) { - if (!strcmp(argv[1], "status")) { - if (fw_land_detector_task) { + if (fw_land_detector_task->isRunning()) { + warnx("running"); - if(fw_land_detector_task->isRunning()) { - warnx("running"); - } - else { - errx(1, "exists, but not running"); - } + } else { + errx(1, "exists, but not running"); + } - exit(0); + exit(0); - } else { - errx(1, "not running"); - } - } + } else { + errx(1, "not running"); + } + } - warn("usage: fw_land_detector {start|stop|status}"); - return 1; + warn("usage: fw_land_detector {start|stop|status}"); + return 1; } diff --git a/src/modules/mc_land_detector/MulticopterLandDetector.cpp b/src/modules/mc_land_detector/MulticopterLandDetector.cpp index 90b36a795..d23ff9017 100644 --- a/src/modules/mc_land_detector/MulticopterLandDetector.cpp +++ b/src/modules/mc_land_detector/MulticopterLandDetector.cpp @@ -47,35 +47,35 @@ #include <unistd.h> //usleep MulticopterLandDetector::MulticopterLandDetector() : - _landDetectedPub(-1), - _landDetected({0,false}), - - _vehicleGlobalPositionSub(-1), - _sensorsCombinedSub(-1), - _waypointSub(-1), - _actuatorsSub(-1), - _armingSub(-1), - - _vehicleGlobalPosition({}), - _sensors({}), - _waypoint({}), - _actuators({}), - _arming({}), - - _taskShouldExit(false), - _taskIsRunning(false), - _landTimer(0) + _landDetectedPub(-1), + _landDetected({0, false}), + + _vehicleGlobalPositionSub(-1), + _sensorsCombinedSub(-1), + _waypointSub(-1), + _actuatorsSub(-1), + _armingSub(-1), + + _vehicleGlobalPosition({}), + _sensors({}), + _waypoint({}), + _actuators({}), + _arming({}), + + _taskShouldExit(false), + _taskIsRunning(false), + _landTimer(0) { - //Advertise the first land detected uORB - _landDetected.timestamp = hrt_absolute_time(); - _landDetected.landed = false; - _landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected); + //Advertise the first land detected uORB + _landDetected.timestamp = hrt_absolute_time(); + _landDetected.landed = false; + _landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected); } MulticopterLandDetector::~MulticopterLandDetector() { - _taskShouldExit = true; - close(_landDetectedPub); + _taskShouldExit = true; + close(_landDetectedPub); } /** @@ -84,120 +84,119 @@ MulticopterLandDetector::~MulticopterLandDetector() **/ static bool orb_update(const struct orb_metadata *meta, int handle, void *buffer) { - bool newData = false; + bool newData = false; - //Check if there is new data to grab - if(orb_check(handle, &newData) != OK) { - return false; - } + //Check if there is new data to grab + if (orb_check(handle, &newData) != OK) { + return false; + } - if(!newData) { - return false; - } + if (!newData) { + return false; + } - if(orb_copy(meta, handle, buffer) != OK) { - return false; - } + if (orb_copy(meta, handle, buffer) != OK) { + return false; + } - return true; + return true; } void MulticopterLandDetector::shutdown() { - _taskShouldExit = true; + _taskShouldExit = true; } void MulticopterLandDetector::updateSubscriptions() { - orb_update(ORB_ID(vehicle_global_position), _vehicleGlobalPositionSub, &_vehicleGlobalPosition); - orb_update(ORB_ID(sensor_combined), _sensorsCombinedSub, &_sensors); - orb_update(ORB_ID(position_setpoint_triplet), _waypointSub, &_waypoint); - orb_update(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _actuatorsSub, &_actuators); - orb_update(ORB_ID(actuator_armed), _armingSub, &_arming); + orb_update(ORB_ID(vehicle_global_position), _vehicleGlobalPositionSub, &_vehicleGlobalPosition); + orb_update(ORB_ID(sensor_combined), _sensorsCombinedSub, &_sensors); + orb_update(ORB_ID(position_setpoint_triplet), _waypointSub, &_waypoint); + orb_update(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _actuatorsSub, &_actuators); + orb_update(ORB_ID(actuator_armed), _armingSub, &_arming); } void MulticopterLandDetector::landDetectorLoop() { - //This should never happen! - if(_taskIsRunning) return; - - //Subscribe to position, attitude, arming and velocity changes - _vehicleGlobalPositionSub = orb_subscribe(ORB_ID(vehicle_global_position)); - _sensorsCombinedSub = orb_subscribe(ORB_ID(sensor_combined)); - _waypointSub = orb_subscribe(ORB_ID(position_setpoint_triplet)); - _actuatorsSub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); - _armingSub = orb_subscribe(ORB_ID(actuator_armed)); - - //Begin task - _taskIsRunning = true; - _taskShouldExit = false; - while (!_taskShouldExit) { - - //First poll for new data from our subscriptions - updateSubscriptions(); - - const uint64_t now = hrt_absolute_time(); - - //only detect landing if the autopilot is actively trying to land - if(!_waypoint.current.valid || _waypoint.current.type != SETPOINT_TYPE_LAND) { - _landTimer = now; - } - else { - - //Check if we are moving vertically - bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > MC_LAND_DETECTOR_CLIMBRATE_MAX; - - //Check if we are moving horizontally - bool horizontalMovement = sqrtf(_vehicleGlobalPosition.vel_n*_vehicleGlobalPosition.vel_n - + _vehicleGlobalPosition.vel_e*_vehicleGlobalPosition.vel_e) > MC_LAND_DETECTOR_VELOCITY_MAX; - - //Next look if all rotation angles are not moving - bool rotating = sqrtf(_sensors.gyro_rad_s[0]*_sensors.gyro_rad_s[0]+ - _sensors.gyro_rad_s[1]*_sensors.gyro_rad_s[1]+ - _sensors.gyro_rad_s[2]*_sensors.gyro_rad_s[2]) > MC_LAND_DETECTOR_ROTATION_MAX; - - //Check if thrust output is minimal (about half of default) - bool minimalThrust = _arming.armed && _actuators.control[3] <= MC_LAND_DETECTOR_THRUST_MAX; - - if(verticalMovement || rotating || !minimalThrust || horizontalMovement) { - //Sensed movement, so reset the land detector - _landTimer = now; - } - - } - - // if we have detected a landing for 2 continuous seconds - if(now-_landTimer > MC_LAND_DETECTOR_TRIGGER_TIME) { - if(!_landDetected.landed) - { - _landDetected.timestamp = now; - _landDetected.landed = true; - - /* publish the land detected broadcast */ - orb_publish(ORB_ID(vehicle_land_detected), _landDetectedPub, &_landDetected); - } - } - else { - // if we currently think we have landed, but the latest data says we are flying - if(_landDetected.landed) - { - _landDetected.timestamp = now; - _landDetected.landed = false; - - /* publish the land detected broadcast */ - orb_publish(ORB_ID(vehicle_land_detected), _landDetectedPub, &_landDetected); - } - } - - //Limit loop rate - usleep(1000000 / MC_LAND_DETECTOR_UPDATE_RATE); - } - - _taskIsRunning = false; - _exit(0); + //This should never happen! + if (_taskIsRunning) { return; } + + //Subscribe to position, attitude, arming and velocity changes + _vehicleGlobalPositionSub = orb_subscribe(ORB_ID(vehicle_global_position)); + _sensorsCombinedSub = orb_subscribe(ORB_ID(sensor_combined)); + _waypointSub = orb_subscribe(ORB_ID(position_setpoint_triplet)); + _actuatorsSub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + _armingSub = orb_subscribe(ORB_ID(actuator_armed)); + + //Begin task + _taskIsRunning = true; + _taskShouldExit = false; + + while (!_taskShouldExit) { + + //First poll for new data from our subscriptions + updateSubscriptions(); + + const uint64_t now = hrt_absolute_time(); + + //only detect landing if the autopilot is actively trying to land + if (!_waypoint.current.valid || _waypoint.current.type != SETPOINT_TYPE_LAND) { + _landTimer = now; + + } else { + + //Check if we are moving vertically + bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > MC_LAND_DETECTOR_CLIMBRATE_MAX; + + //Check if we are moving horizontally + bool horizontalMovement = sqrtf(_vehicleGlobalPosition.vel_n * _vehicleGlobalPosition.vel_n + + _vehicleGlobalPosition.vel_e * _vehicleGlobalPosition.vel_e) > MC_LAND_DETECTOR_VELOCITY_MAX; + + //Next look if all rotation angles are not moving + bool rotating = sqrtf(_sensors.gyro_rad_s[0] * _sensors.gyro_rad_s[0] + + _sensors.gyro_rad_s[1] * _sensors.gyro_rad_s[1] + + _sensors.gyro_rad_s[2] * _sensors.gyro_rad_s[2]) > MC_LAND_DETECTOR_ROTATION_MAX; + + //Check if thrust output is minimal (about half of default) + bool minimalThrust = _arming.armed && _actuators.control[3] <= MC_LAND_DETECTOR_THRUST_MAX; + + if (verticalMovement || rotating || !minimalThrust || horizontalMovement) { + //Sensed movement, so reset the land detector + _landTimer = now; + } + + } + + // if we have detected a landing for 2 continuous seconds + if (now - _landTimer > MC_LAND_DETECTOR_TRIGGER_TIME) { + if (!_landDetected.landed) { + _landDetected.timestamp = now; + _landDetected.landed = true; + + /* publish the land detected broadcast */ + orb_publish(ORB_ID(vehicle_land_detected), _landDetectedPub, &_landDetected); + } + + } else { + // if we currently think we have landed, but the latest data says we are flying + if (_landDetected.landed) { + _landDetected.timestamp = now; + _landDetected.landed = false; + + /* publish the land detected broadcast */ + orb_publish(ORB_ID(vehicle_land_detected), _landDetectedPub, &_landDetected); + } + } + + //Limit loop rate + usleep(1000000 / MC_LAND_DETECTOR_UPDATE_RATE); + } + + _taskIsRunning = false; + _exit(0); } bool MulticopterLandDetector::isRunning() const { - return _taskIsRunning; + return _taskIsRunning; } diff --git a/src/modules/mc_land_detector/MulticopterLandDetector.h b/src/modules/mc_land_detector/MulticopterLandDetector.h index 1aeaa0d62..fe18374c3 100644 --- a/src/modules/mc_land_detector/MulticopterLandDetector.h +++ b/src/modules/mc_land_detector/MulticopterLandDetector.h @@ -50,66 +50,67 @@ #include <uORB/topics/actuator_controls.h> #include <uORB/topics/actuator_armed.h> -class MulticopterLandDetector { +class MulticopterLandDetector +{ public: - MulticopterLandDetector(); - ~MulticopterLandDetector(); - - /** - * @brief Executes the main loop of the land detector in a separate deamon thread - * @returns OK if task was successfully launched - **/ - int createDeamonThread(); - - /** - * @returns true if this task is currently running - **/ - bool isRunning() const; - - /** - * @brief blocking loop, should be run in a separate thread or task. Runs at 50Hz - **/ - void landDetectorLoop(); - - /** - * @brief Tells the Land Detector task that it should exit - **/ - void shutdown(); + MulticopterLandDetector(); + ~MulticopterLandDetector(); + + /** + * @brief Executes the main loop of the land detector in a separate deamon thread + * @returns OK if task was successfully launched + **/ + int createDeamonThread(); + + /** + * @returns true if this task is currently running + **/ + bool isRunning() const; + + /** + * @brief blocking loop, should be run in a separate thread or task. Runs at 50Hz + **/ + void landDetectorLoop(); + + /** + * @brief Tells the Land Detector task that it should exit + **/ + void shutdown(); protected: - /** - * @brief polls all subscriptions and pulls any data that has changed - **/ - void updateSubscriptions(); - - //Algorithm parameters (TODO: should probably be externalized) - static constexpr float MC_LAND_DETECTOR_CLIMBRATE_MAX = 0.30f; /**< max +- climb rate in m/s */ - static constexpr float MC_LAND_DETECTOR_ROTATION_MAX = 0.5f; /**< max rotation in rad/sec (= 30 deg/s) */ - static constexpr float MC_LAND_DETECTOR_THRUST_MAX = 0.2f; - static constexpr float MC_LAND_DETECTOR_VELOCITY_MAX = 1.0f; /**< max +- horizontal movement in m/s */ - static constexpr uint32_t MC_LAND_DETECTOR_UPDATE_RATE = 50; /**< Run algorithm at 50Hz */ - static constexpr uint32_t MC_LAND_DETECTOR_TRIGGER_TIME = 2000000; /**< usec that landing conditions have to hold before triggering a land */ + /** + * @brief polls all subscriptions and pulls any data that has changed + **/ + void updateSubscriptions(); + + //Algorithm parameters (TODO: should probably be externalized) + static constexpr float MC_LAND_DETECTOR_CLIMBRATE_MAX = 0.30f; /**< max +- climb rate in m/s */ + static constexpr float MC_LAND_DETECTOR_ROTATION_MAX = 0.5f; /**< max rotation in rad/sec (= 30 deg/s) */ + static constexpr float MC_LAND_DETECTOR_THRUST_MAX = 0.2f; + static constexpr float MC_LAND_DETECTOR_VELOCITY_MAX = 1.0f; /**< max +- horizontal movement in m/s */ + static constexpr uint32_t MC_LAND_DETECTOR_UPDATE_RATE = 50; /**< Run algorithm at 50Hz */ + static constexpr uint32_t MC_LAND_DETECTOR_TRIGGER_TIME = 2000000; /**< usec that landing conditions have to hold before triggering a land */ private: - orb_advert_t _landDetectedPub; /**< publisher for position in local frame */ - struct vehicle_land_detected_s _landDetected; /**< local vehicle position */ - - int _vehicleGlobalPositionSub; /**< notification of global position */ - int _sensorsCombinedSub; - int _waypointSub; - int _actuatorsSub; - int _armingSub; - - struct vehicle_global_position_s _vehicleGlobalPosition; /**< the result from global position subscription */ - struct sensor_combined_s _sensors; /**< subscribe to sensor readings */ - struct position_setpoint_triplet_s _waypoint; /**< subscribe to autopilot navigation */ - struct actuator_controls_s _actuators; - struct actuator_armed_s _arming; - - bool _taskShouldExit; /**< true if it is requested that this task should exit */ - bool _taskIsRunning; /**< task has reached main loop and is currently running */ - - uint64_t _landTimer; /**< timestamp in microseconds since a possible land was detected*/ + orb_advert_t _landDetectedPub; /**< publisher for position in local frame */ + struct vehicle_land_detected_s _landDetected; /**< local vehicle position */ + + int _vehicleGlobalPositionSub; /**< notification of global position */ + int _sensorsCombinedSub; + int _waypointSub; + int _actuatorsSub; + int _armingSub; + + struct vehicle_global_position_s _vehicleGlobalPosition; /**< the result from global position subscription */ + struct sensor_combined_s _sensors; /**< subscribe to sensor readings */ + struct position_setpoint_triplet_s _waypoint; /**< subscribe to autopilot navigation */ + struct actuator_controls_s _actuators; + struct actuator_armed_s _arming; + + bool _taskShouldExit; /**< true if it is requested that this task should exit */ + bool _taskIsRunning; /**< task has reached main loop and is currently running */ + + uint64_t _landTimer; /**< timestamp in microseconds since a possible land was detected*/ }; #endif //__MULTICOPTER_LAND_DETECTOR_H__ diff --git a/src/modules/mc_land_detector/mc_land_detector_main.cpp b/src/modules/mc_land_detector/mc_land_detector_main.cpp index 2992ead99..6ad3f82a4 100644 --- a/src/modules/mc_land_detector/mc_land_detector_main.cpp +++ b/src/modules/mc_land_detector/mc_land_detector_main.cpp @@ -61,7 +61,7 @@ static void mc_land_detector_stop(); extern "C" __EXPORT int mc_land_detector_main(int argc, char *argv[]); //Private variables -static MulticopterLandDetector* mc_land_detector_task = nullptr; +static MulticopterLandDetector *mc_land_detector_task = nullptr; static int _landDetectorTaskID = -1; /** @@ -77,7 +77,7 @@ static void mc_land_detector_deamon_thread(int argc, char *argv[]) **/ static void mc_land_detector_stop() { - if(mc_land_detector_task == nullptr || _landDetectorTaskID == -1) { + if (mc_land_detector_task == nullptr || _landDetectorTaskID == -1) { errx(1, "not running"); return; } @@ -86,6 +86,7 @@ static void mc_land_detector_stop() //Wait for task to die int i = 0; + do { /* wait 20ms */ usleep(20000); @@ -109,13 +110,14 @@ static void mc_land_detector_stop() **/ static int mc_land_detector_start() { - if(mc_land_detector_task != nullptr || _landDetectorTaskID != -1) { + if (mc_land_detector_task != nullptr || _landDetectorTaskID != -1) { errx(1, "already running"); return -1; } //Allocate memory mc_land_detector_task = new MulticopterLandDetector(); + if (mc_land_detector_task == nullptr) { errx(1, "alloc failed"); return -1; @@ -123,11 +125,11 @@ static int mc_land_detector_start() //Start new thread task _landDetectorTaskID = task_spawn_cmd("mc_land_detector", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 1024, - (main_t)&mc_land_detector_deamon_thread, - nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 1024, + (main_t)&mc_land_detector_deamon_thread, + nullptr); if (_landDetectorTaskID < 0) { errx(1, "task start failed: %d", -errno); @@ -136,17 +138,19 @@ static int mc_land_detector_start() /* 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 + while (!mc_land_detector_task->isRunning()) { usleep(50000); printf("."); fflush(stdout); - if(hrt_absolute_time() > timeout) { + if (hrt_absolute_time() > timeout) { err(1, "start failed - timeout"); mc_land_detector_stop(); exit(1); } } + printf("\n"); exit(0); @@ -170,16 +174,16 @@ int mc_land_detector_main(int argc, char *argv[]) if (!strcmp(argv[1], "stop")) { mc_land_detector_stop(); - exit(0); + exit(0); } if (!strcmp(argv[1], "status")) { if (mc_land_detector_task) { - if(mc_land_detector_task->isRunning()) { + if (mc_land_detector_task->isRunning()) { warnx("running"); - } - else { + + } else { errx(1, "exists, but not running"); } |