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

updated to master (solved merge conflicts)

Browse Source
This commit is contained in:
Youssef Demitri
2015-10-27 09:26:27 +01:00
parent 560efcbc5b 4241e526aa
commit dd59409e31
289 changed files with 10555 additions and 4461 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf_att_pos_estimator/CMakeLists.txt
+1 -1
View File
@@ -32,7 +32,7 @@
############################################################################
set(MODULE_CFLAGS )
if(NOT ${OS} STREQUAL "qurt")
if(${OS} STREQUAL "nuttx")
list(APPEND MODULE_CFLAGS -Wframe-larger-than=3400)
endif()
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+7 -7
View File
@@ -1194,7 +1194,7 @@ int AttitudePositionEstimatorEKF::start()
_estimator_task = px4_task_spawn_cmd("ekf_att_pos_estimator",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 20,
3900,
4600,
(px4_main_t)&AttitudePositionEstimatorEKF::task_main_trampoline,
nullptr);
@@ -1317,9 +1317,9 @@ void AttitudePositionEstimatorEKF::pollData()
_ekf->dAngIMU.y = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 1];
_ekf->dAngIMU.z = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 2];
} else {
_ekf->dAngIMU.x = 0.5f * (_ekf->angRate.x + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 0]);
_ekf->dAngIMU.y = 0.5f * (_ekf->angRate.y + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 1]);
_ekf->dAngIMU.z = 0.5f * (_ekf->angRate.z + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 2]);
_ekf->dAngIMU.x = 0.5f * (_ekf->angRate.x + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 0]) * _ekf->dtIMU;
_ekf->dAngIMU.y = 0.5f * (_ekf->angRate.y + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 1]) * _ekf->dtIMU;
_ekf->dAngIMU.z = 0.5f * (_ekf->angRate.z + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 2]) * _ekf->dtIMU;
}
_ekf->angRate.x = _sensor_combined.gyro_rad_s[_gyro_main * 3 + 0];
@@ -1337,9 +1337,9 @@ void AttitudePositionEstimatorEKF::pollData()
_ekf->dVelIMU.y = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 1];
_ekf->dVelIMU.z = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 2];
} else {
_ekf->dVelIMU.x = 0.5f * (_ekf->accel.x + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0]);
_ekf->dVelIMU.y = 0.5f * (_ekf->accel.y + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 1]);
_ekf->dVelIMU.z = 0.5f * (_ekf->accel.z + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 2]);
_ekf->dVelIMU.x = 0.5f * (_ekf->accel.x + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0]) * _ekf->dtIMU;
_ekf->dVelIMU.y = 0.5f * (_ekf->accel.y + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 1]) * _ekf->dtIMU;
_ekf->dVelIMU.z = 0.5f * (_ekf->accel.z + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 2]) * _ekf->dtIMU;
}
_ekf->accel.x = _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0];
src/modules/ekf_att_pos_estimator/estimator_22states.cpp
+10 -11
View File
@@ -43,7 +43,6 @@
#include <stdio.h>
#include <stdarg.h>
#include <cmath>
#include <algorithm>
#ifndef M_PI_F
#define M_PI_F static_cast<float>(M_PI)
@@ -1853,7 +1852,7 @@ void AttPosEKF::FuseOptFlow()
Vector3f relVelSensor;
// Perform sequential fusion of optical flow measurements only with valid tilt and height
flowStates[1] = std::max(flowStates[1], statesAtFlowTime[9] + minFlowRng);
flowStates[1] = fmax(flowStates[1], statesAtFlowTime[9] + minFlowRng);
float heightAboveGndEst = flowStates[1] - statesAtFlowTime[9];
bool validTilt = Tnb.z.z > 0.71f;
if (validTilt)
@@ -2113,7 +2112,7 @@ void AttPosEKF::OpticalFlowEKF()
} else {
return;
}
distanceTravelledSq = std::min(distanceTravelledSq, 100.0f);
distanceTravelledSq = fmin(distanceTravelledSq, 100.0f);
Popt[1][1] += (distanceTravelledSq * sq(gndHgtSigma));
}
@@ -2153,7 +2152,7 @@ void AttPosEKF::OpticalFlowEKF()
varInnovRng = 1.0f/SK_RNG[1];
// constrain terrain height to be below the vehicle
flowStates[1] = std::max(flowStates[1], statesAtRngTime[9] + minFlowRng);
flowStates[1] = fmax(flowStates[1], statesAtRngTime[9] + minFlowRng);
// estimate range to centre of image
range = (flowStates[1] - statesAtRngTime[9]) * SK_RNG[2];
@@ -2173,7 +2172,7 @@ void AttPosEKF::OpticalFlowEKF()
}
// constrain the states
flowStates[0] = ConstrainFloat(flowStates[0], 0.1f, 10.0f);
flowStates[1] = std::max(flowStates[1], statesAtRngTime[9] + minFlowRng);
flowStates[1] = fmax(flowStates[1], statesAtRngTime[9] + minFlowRng);
// correct the covariance matrix
float nextPopt[2][2];
@@ -2182,8 +2181,8 @@ void AttPosEKF::OpticalFlowEKF()
nextPopt[1][0] = -Popt[1][0]*((Popt[1][1]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2] - 1.0f);
nextPopt[1][1] = -Popt[1][1]*((Popt[1][1]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2] - 1.0f);
// prevent the state variances from becoming negative and maintain symmetry
Popt[0][0] = std::max(nextPopt[0][0],0.0f);
Popt[1][1] = std::max(nextPopt[1][1],0.0f);
Popt[0][0] = fmax(nextPopt[0][0],0.0f);
Popt[1][1] = fmax(nextPopt[1][1],0.0f);
Popt[0][1] = 0.5f * (nextPopt[0][1] + nextPopt[1][0]);
Popt[1][0] = Popt[0][1];
}
@@ -2222,7 +2221,7 @@ void AttPosEKF::OpticalFlowEKF()
vel.z = statesAtFlowTime[6];
// constrain terrain height to be below the vehicle
flowStates[1] = std::max(flowStates[1], statesAtFlowTime[9] + minFlowRng);
flowStates[1] = fmax(flowStates[1], statesAtFlowTime[9] + minFlowRng);
// estimate range to centre of image
range = (flowStates[1] - statesAtFlowTime[9]) / Tnb_flow.z.z;
@@ -2290,7 +2289,7 @@ void AttPosEKF::OpticalFlowEKF()
}
// constrain the states
flowStates[0] = ConstrainFloat(flowStates[0], 0.1f, 10.0f);
flowStates[1] = std::max(flowStates[1], statesAtFlowTime[9] + minFlowRng);
flowStates[1] = fmax(flowStates[1], statesAtFlowTime[9] + minFlowRng);
// correct the covariance matrix
for (uint8_t i = 0; i < 2 ; i++) {
@@ -2306,8 +2305,8 @@ void AttPosEKF::OpticalFlowEKF()
}
// prevent the state variances from becoming negative and maintain symmetry
Popt[0][0] = std::max(nextPopt[0][0],0.0f);
Popt[1][1] = std::max(nextPopt[1][1],0.0f);
Popt[0][0] = fmax(nextPopt[0][0],0.0f);
Popt[1][1] = fmax(nextPopt[1][1],0.0f);
Popt[0][1] = 0.5f * (nextPopt[0][1] + nextPopt[1][0]);
Popt[1][0] = Popt[0][1];
}