fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-20 17:13:34 +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
src/modules/land_detector/FixedwingLandDetector.h
+6 -3
View File
@@ -42,9 +42,10 @@
#pragma once
#include <uORB/topics/control_state.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/sensor_bias.h>
#include <uORB/topics/vehicle_local_position.h>
#include "LandDetector.h"
@@ -85,13 +86,15 @@ private:
float maxIntVelocity;
} _params{};
int _controlStateSub{-1};
int _armingSub{-1};
int _airspeedSub{-1};
int _sensor_bias_sub{-1};
int _local_pos_sub{-1};
control_state_s _controlState{};
actuator_armed_s _arming{};
airspeed_s _airspeed{};
sensor_bias_s _sensors{};
vehicle_local_position_s _local_pos{};
float _velocity_xy_filtered{0.0f};
float _velocity_z_filtered{0.0f};
src/modules/land_detector/MulticopterLandDetector.cpp
+9 -9
View File
@@ -80,7 +80,7 @@ MulticopterLandDetector::MulticopterLandDetector() :
_armingSub(-1),
_attitudeSub(-1),
_manualSub(-1),
_ctrl_state_sub(-1),
_sensor_bias_sub(-1),
_vehicle_control_mode_sub(-1),
_battery_sub(-1),
_vehicleLocalPosition{},
@@ -89,7 +89,7 @@ MulticopterLandDetector::MulticopterLandDetector() :
_arming{},
_vehicleAttitude{},
_manual{},
_ctrl_state{},
_sensors{},
_control_mode{},
_battery{},
_min_trust_start(0),
@@ -125,7 +125,7 @@ void MulticopterLandDetector::_initialize_topics()
_armingSub = orb_subscribe(ORB_ID(actuator_armed));
_parameterSub = orb_subscribe(ORB_ID(parameter_update));
_manualSub = orb_subscribe(ORB_ID(manual_control_setpoint));
_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
_sensor_bias_sub = orb_subscribe(ORB_ID(sensor_bias));
_vehicle_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_battery_sub = orb_subscribe(ORB_ID(battery_status));
}
@@ -138,7 +138,7 @@ void MulticopterLandDetector::_update_topics()
_orb_update(ORB_ID(actuator_controls_0), _actuatorsSub, &_actuators);
_orb_update(ORB_ID(actuator_armed), _armingSub, &_arming);
_orb_update(ORB_ID(manual_control_setpoint), _manualSub, &_manual);
_orb_update(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
_orb_update(ORB_ID(sensor_bias), _sensor_bias_sub, &_sensors);
_orb_update(ORB_ID(vehicle_control_mode), _vehicle_control_mode_sub, &_control_mode);
_orb_update(ORB_ID(battery_status), _battery_sub, &_battery);
}
@@ -170,14 +170,14 @@ bool MulticopterLandDetector::_get_freefall_state()
return false;
}
if (_ctrl_state.timestamp == 0) {
// _ctrl_state is not valid yet, we have to assume we're not falling.
if (_sensors.timestamp == 0) {
// _sensors is not valid yet, we have to assume we're not falling.
return false;
}
float acc_norm = _ctrl_state.x_acc * _ctrl_state.x_acc
+ _ctrl_state.y_acc * _ctrl_state.y_acc
+ _ctrl_state.z_acc * _ctrl_state.z_acc;
float acc_norm = _sensors.accel_x * _sensors.accel_x
+ _sensors.accel_y * _sensors.accel_y
+ _sensors.accel_z * _sensors.accel_z;
acc_norm = sqrtf(acc_norm); //norm of specific force. Should be close to 9.8 m/s^2 when landed.
return (acc_norm < _params.freefall_acc_threshold); //true if we are currently falling
src/modules/land_detector/MulticopterLandDetector.h
+11 -8
View File
@@ -42,19 +42,22 @@
#pragma once
#include "LandDetector.h"
#include <systemlib/param/param.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/battery_status.h>
#include "LandDetector.h"
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_bias.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_local_position.h>
namespace land_detector
{
@@ -135,7 +138,7 @@ private:
int _armingSub;
int _attitudeSub;
int _manualSub;
int _ctrl_state_sub;
int _sensor_bias_sub;
int _vehicle_control_mode_sub;
int _battery_sub;
@@ -145,7 +148,7 @@ private:
struct actuator_armed_s _arming;
struct vehicle_attitude_s _vehicleAttitude;
struct manual_control_setpoint_s _manual;
struct control_state_s _ctrl_state;
struct sensor_bias_s _sensors;
struct vehicle_control_mode_s _control_mode;
struct battery_status_s _battery;