bool VtolLandDetector::update()
{
updateSubscriptions();
updateParameterCache(false);
// this is returned from the mutlicopter land detector
bool landed = get_landed_state();
// for vtol we additionally consider airspeed
if (hrt_elapsed_time(&_airspeed.timestamp) < 500 * 1000) {
_airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s;
} else {
// if airspeed does not update, set it to zero and rely on multicopter land detector
_airspeed_filtered = 0.0f;
}
// only consider airspeed if we have been in air before to avoid false
// detections in the case of wind on the ground
if (_was_in_air && _airspeed_filtered > _params.maxAirSpeed) {
landed = false;
}
_was_in_air = !landed;
return landed;
}
void VtolLandDetector::initialize()
{
MulticopterLandDetector::initialize();
_airspeedSub = orb_subscribe(ORB_ID(airspeed));
// download parameters
updateParameterCache(true);
}
void FixedwingLandDetector::initialize()
{
// Subscribe to local position and airspeed data
_vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position));
_airspeedSub = orb_subscribe(ORB_ID(airspeed));
updateParameterCache(true);
}
bool MulticopterLandDetector::update()
{
// first poll for new data from our subscriptions
updateSubscriptions();
updateParameterCache(false);
return get_landed_state();
}
void MulticopterLandDetector::initialize()
{
// 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));
_parameterSub = orb_subscribe(ORB_ID(parameter_update));
// download parameters
updateParameterCache(true);
}
void MulticopterLandDetector::initialize()
{
// subscribe to position, attitude, arming and velocity changes
_vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position));
_attitudeSub = orb_subscribe(ORB_ID(vehicle_attitude));
_actuatorsSub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
_armingSub = orb_subscribe(ORB_ID(actuator_armed));
_parameterSub = orb_subscribe(ORB_ID(parameter_update));
_manualSub = orb_subscribe(ORB_ID(manual_control_setpoint));
// download parameters
updateParameterCache(true);
}
LandDetectionResult MulticopterLandDetector::update()
{
// first poll for new data from our subscriptions
updateSubscriptions();
updateParameterCache(false);
if (get_freefall_state()) {
_state = LANDDETECTION_RES_FREEFALL;
} else if (get_landed_state()) {
_state = LANDDETECTION_RES_LANDED;
} else {
_state = LANDDETECTION_RES_FLYING;
}
return _state;
}