<commit_before>/****************************************************************************
 *
 *   Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/**
 * @file MulticopterLandDetector.cpp
 * Land detection algorithm for multicopters
 *
 * @author <NAME> <<EMAIL>>
 * @author <NAME> <<EMAIL>>
 */

#include "MulticopterLandDetector.h"

#include <cmath>
#include <drivers/drv_hrt.h>
#include <mathlib/mathlib.h>

namespace landdetection
{

MulticopterLandDetector::MulticopterLandDetector() : LandDetector(),
	_paramHandle(),
	_params(),
	_vehicleLocalPositionSub(-1),
	_actuatorsSub(-1),
	_armingSub(-1),
	_parameterSub(-1),
	_attitudeSub(-1),
	_manualSub(-1),
	_vehicleLocalPosition{},
	_actuators{},
	_arming{},
	_vehicleAttitude{},
	_sensors{},
	_landTimer(0),
	_freefallTimer(0)
{
	_paramHandle.maxRotation = param_find("LNDMC_ROT_MAX");
	_paramHandle.maxVelocity = param_find("LNDMC_XY_VEL_MAX");
	_paramHandle.maxClimbRate = param_find("LNDMC_Z_VEL_MAX");
	_paramHandle.maxThrottle = param_find("LNDMC_THR_MAX");
	_paramHandle.acc_threshold_m_s2 = param_find("LNDMC_FFALL_THR");
	_paramHandle.ff_trigger_time_ms = param_find("LNDMC_FFALL_TRIG");
}

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));
	_sensorsSub = orb_subscribe(ORB_ID(sensor_combined));

	// download parameters
	updateParameterCache(true);
}

void MulticopterLandDetector::updateSubscriptions()
{
	orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition);
	orb_update(ORB_ID(vehicle_attitude), _attitudeSub, &_vehicleAttitude);
	orb_update(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _actuatorsSub, &_actuators);
	orb_update(ORB_ID(actuator_armed), _armingSub, &_arming);
	orb_update(ORB_ID(manual_control_setpoint), _manualSub, &_manual);
	orb_update(ORB_ID(sensor_combined), _sensorsSub, &_sensors);
}

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

bool MulticopterLandDetector::get_freefall_state()
{
	const uint64_t now = hrt_absolute_time();

	float acc_norm = 0.0;
	for (int i = 0; i < 3; i++) {
		acc_norm += _sensors.accelerometer_m_s2[i] * _sensors.accelerometer_m_s2[i];
	}
	acc_norm = sqrtf(acc_norm);	//norm of specific force

	int32_t elapsed_time_ms = (now - _freefallTimer) / 1000;
	bool freefall = (acc_norm < _params.acc_threshold_m_s2);
	if (!freefall || _freefallTimer == 0) {	//reset timer if uav not falling
		_freefallTimer = now;
		return false;
	}/* else {	//DEBUG
		PX4_WARN("[freefalldetector] FREE FALL !! \tacc_norm:\t%8.4f, \telapsed time:\t%8.4f ms", (double)acc_norm, (double)elapsed_time_ms);
	}
	bool falling = elapsed_time_ms > _params.ff_trigger_time_ms;
	if (falling) {
		PX4_WARN("[freefalldetector] TRIGGERED");
	}*/
	return elapsed_time_ms > _params.ff_trigger_time_ms;
}

bool MulticopterLandDetector::get_landed_state()<commit_msg>Removed debug parts<commit_after>/****************************************************************************
 *
 *   Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/**
 * @file MulticopterLandDetector.cpp
 * Land detection algorithm for multicopters
 *
 * @author <NAME> <<EMAIL>>
 * @author <NAME> <<EMAIL>>
 */

#include "MulticopterLandDetector.h"

#include <cmath>
#include <drivers/drv_hrt.h>
#include <mathlib/mathlib.h>

namespace landdetection
{

MulticopterLandDetector::MulticopterLandDetector() : LandDetector(),
	_paramHandle(),
	_params(),
	_vehicleLocalPositionSub(-1),
	_actuatorsSub(-1),
	_armingSub(-1),
	_parameterSub(-1),
	_attitudeSub(-1),
	_manualSub(-1),
	_vehicleLocalPosition{},
	_actuators{},
	_arming{},
	_vehicleAttitude{},
	_sensors{},
	_landTimer(0),
	_freefallTimer(0)
{
	_paramHandle.maxRotation = param_find("LNDMC_ROT_MAX");
	_paramHandle.maxVelocity = param_find("LNDMC_XY_VEL_MAX");
	_paramHandle.maxClimbRate = param_find("LNDMC_Z_VEL_MAX");
	_paramHandle.maxThrottle = param_find("LNDMC_THR_MAX");
	_paramHandle.acc_threshold_m_s2 = param_find("LNDMC_FFALL_THR");
	_paramHandle.ff_trigger_time_ms = param_find("LNDMC_FFALL_TRIG");
}

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));
	_sensorsSub = orb_subscribe(ORB_ID(sensor_combined));

	// download parameters
	updateParameterCache(true);
}

void MulticopterLandDetector::updateSubscriptions()
{
	orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition);
	orb_update(ORB_ID(vehicle_attitude), _attitudeSub, &_vehicleAttitude);
	orb_update(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _actuatorsSub, &_actuators);
	orb_update(ORB_ID(actuator_armed), _armingSub, &_arming);
	orb_update(ORB_ID(manual_control_setpoint), _manualSub, &_manual);
	orb_update(ORB_ID(sensor_combined), _sensorsSub, &_sensors);
}

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

bool MulticopterLandDetector::get_freefall_state()
{
	if(_params.acc_threshold_m_s2 < 0.1f || _params.acc_threshold_m_s2 > 10.0f){	//if parameter is set to zero or invalid, disable free-fall detection.
		return false;
	}

	const uint64_t now = hrt_absolute_time();

	float acc_norm = 0.0;
	for (int i = 0; i < 3; i++) {
		acc_norm += _sensors.accelerometer_m_s2[i] * _sensors.accelerometer_m_s2[i];
	}
	acc_norm = sqrtf(acc_norm);	//norm of specific force. Should be close to 9.8 m/s^2 when landed.

	bool freefall = (acc_norm < _params.acc_threshold_m_s2);	//true if we are currently falling
	if (!freefall || _freefallTimer == 0) {	//reset timer if uav not falling
		_freefallTimer = now;
		return false;
	}

	return (now - _freefallTimer) / 1000 > _params.ff_trigger_time_ms;
}

bool MulticopterLandDetector::get_landed_state()
{'fraction_non_alphanumeric': 0.12280529671407553, 'fraction_numerical': 0.008827856792545365, 'mean_word_length': 5.9360544217687075, 'pattern_counts': {'":': 0, '<': 26, '<?xml version=': 0, '>': 25, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 4, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '4225453', 'n_tokens_mistral': 3507, 'n_tokens_neox': 3244, 'n_words': 850}