fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 01:53:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

delete control_state and cleanup vehicle_attitude (#7882)

Browse Source
This commit is contained in:
Daniel Agar
2017-09-21 16:24:53 -04:00
committed by GitHub
parent 5bea264a5f
commit b4755297ec
42 changed files with 776 additions and 1061 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/land_detector/FixedwingLandDetector.cpp
+18 -11
View File
@@ -39,12 +39,12 @@
* @author Julian Oes <julian@oes.ch>
*/
#include <px4_config.h>
#include <px4_defines.h>
#include "FixedwingLandDetector.h"
#include <cmath>
#include "FixedwingLandDetector.h"
#include <px4_config.h>
#include <px4_defines.h>
namespace land_detector
{
@@ -62,16 +62,18 @@ FixedwingLandDetector::FixedwingLandDetector()
void FixedwingLandDetector::_initialize_topics()
{
_controlStateSub = orb_subscribe(ORB_ID(control_state));
_armingSub = orb_subscribe(ORB_ID(actuator_armed));
_airspeedSub = orb_subscribe(ORB_ID(airspeed));
_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
_sensor_bias_sub = orb_subscribe(ORB_ID(sensor_bias));
}
void FixedwingLandDetector::_update_topics()
{
_orb_update(ORB_ID(control_state), _controlStateSub, &_controlState);
_orb_update(ORB_ID(actuator_armed), _armingSub, &_arming);
_orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed);
_orb_update(ORB_ID(sensor_bias), _sensor_bias_sub, &_sensors);
_orb_update(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos);
}
void FixedwingLandDetector::_update_params()
@@ -87,7 +89,7 @@ float FixedwingLandDetector::_get_max_altitude()
// TODO
// This means no altitude limit as the limit
// is always current position plus 1000 meters
return roundf(-_controlState.z_pos + 1000);
return roundf(-_local_pos.z + 1000);
}
bool FixedwingLandDetector::_get_freefall_state()
@@ -117,15 +119,18 @@ bool FixedwingLandDetector::_get_landed_state()
bool landDetected = false;
if (hrt_elapsed_time(&_controlState.timestamp) < 500 * 1000) {
float val = 0.97f * _velocity_xy_filtered + 0.03f * sqrtf(_controlState.x_vel *
_controlState.x_vel + _controlState.y_vel * _controlState.y_vel);
if (hrt_elapsed_time(&_local_pos.timestamp) < 500 * 1000) {
// horizontal velocity
float val = 0.97f * _velocity_xy_filtered + 0.03f * sqrtf(_local_pos.vx * _local_pos.vx + _local_pos.vy *
_local_pos.vy);
if (PX4_ISFINITE(val)) {
_velocity_xy_filtered = val;
}
val = 0.99f * _velocity_z_filtered + 0.01f * fabsf(_controlState.z_vel);
// vertical velocity
val = 0.99f * _velocity_z_filtered + 0.01f * fabsf(_local_pos.vz);
if (PX4_ISFINITE(val)) {
_velocity_z_filtered = val;
@@ -135,7 +140,9 @@ bool FixedwingLandDetector::_get_landed_state()
// a leaking lowpass prevents biases from building up, but
// gives a mostly correct response for short impulses
_accel_horz_lp = _accel_horz_lp * 0.8f + _controlState.horz_acc_mag * 0.18f;
const float acc_hor = sqrtf(_sensors.accel_x * _sensors.accel_x +
_sensors.accel_y * _sensors.accel_y);
_accel_horz_lp = _accel_horz_lp * 0.8f + acc_hor * 0.18f;
// crude land detector for fixedwing
landDetected = _velocity_xy_filtered < _params.maxVelocity