fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-18 06:10:36 +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/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