dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

lpe: style fix

This commit is contained in:
TSC21 2017-11-02 16:09:18 +00:00 committed by Lorenz Meier
parent 051822eee1
commit 483f0d55e0
10 changed files with 96 additions and 100 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

69
src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
View File

@ -9,14 +9,14 @@
orb_advert_t mavlink_log_pub = nullptr;
// required standard deviation of estimate for estimator to publish data
static const uint32_t EST_STDDEV_XY_VALID = 2.0; // 2.0 m
static const uint32_t EST_STDDEV_Z_VALID = 2.0; // 2.0 m
static const uint32_t EST_STDDEV_TZ_VALID = 2.0; // 2.0 m
static const uint32_t EST_STDDEV_XY_VALID = 2.0; // 2.0 m
static const uint32_t EST_STDDEV_Z_VALID = 2.0; // 2.0 m
static const uint32_t EST_STDDEV_TZ_VALID = 2.0; // 2.0 m
static const float P_MAX = 1.0e6f; // max allowed value in state covariance
static const float LAND_RATE = 10.0f; // rate of land detector correction
static const float P_MAX = 1.0e6f; // max allowed value in state covariance
static const float LAND_RATE = 10.0f; // rate of land detector correction
static const char *msg_label = "[lpe] "; // rate of land detector correction
static const char *msg_label = "[lpe] "; // rate of land detector correction
BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
// this block has no parent, and has name LPE
@ -192,8 +192,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
}
BlockLocalPositionEstimator::~BlockLocalPositionEstimator()
{
}
{}
Vector<float, BlockLocalPositionEstimator::n_x> BlockLocalPositionEstimator::dynamics(
float t,
@ -205,7 +204,6 @@ Vector<float, BlockLocalPositionEstimator::n_x> BlockLocalPositionEstimator::dyn
void BlockLocalPositionEstimator::update()
{
// wait for a sensor update, check for exit condition every 100 ms
int ret = px4_poll(_polls, 3, 100);
@ -224,7 +222,6 @@ void BlockLocalPositionEstimator::update()
bool armedState = _sub_armed.get().armed;
if (!armedState && (_sub_lidar == nullptr || _sub_sonar == nullptr)) {
// detect distance sensors
for (int i = 0; i < N_DIST_SUBS; i++) {
uORB::Subscription<distance_sensor_s> *s = _dist_subs[i];
@ -260,22 +257,22 @@ void BlockLocalPositionEstimator::update()
// if (!_lastArmedState && armedState) {
// // we just armed, we are at origin on the ground
// _x(X_x) = 0;
// _x(X_y) = 0;
// // reset Z or not? _x(X_z) = 0;
// // we just armed, we are at origin on the ground
// _x(X_x) = 0;
// _x(X_y) = 0;
// // reset Z or not? _x(X_z) = 0;
// // we aren't moving, all velocities are zero
// _x(X_vx) = 0;
// _x(X_vy) = 0;
// _x(X_vz) = 0;
// // we aren't moving, all velocities are zero
// _x(X_vx) = 0;
// _x(X_vy) = 0;
// _x(X_vz) = 0;
// // assume we are on the ground, so terrain alt is local alt
// _x(X_tz) = _x(X_z);
// // assume we are on the ground, so terrain alt is local alt
// _x(X_tz) = _x(X_z);
// // reset lowpass filter as well
// _xLowPass.setState(_x);
// _aglLowPass.setState(0);
// // reset lowpass filter as well
// _xLowPass.setState(_x);
// _aglLowPass.setState(0);
// }
_lastArmedState = armedState;
@ -288,7 +285,7 @@ void BlockLocalPositionEstimator::update()
int32_t baro_timestamp_relative = _sub_sensor.get().baro_timestamp_relative;
if (baro_timestamp_relative != _sub_sensor.get().RELATIVE_TIMESTAMP_INVALID) {
uint64_t baro_timestamp = _sub_sensor.get().timestamp + \
uint64_t baro_timestamp = _sub_sensor.get().timestamp + \
_sub_sensor.get().baro_timestamp_relative;
if (baro_timestamp != _timeStampLastBaro) {
@ -305,7 +302,7 @@ void BlockLocalPositionEstimator::update()
bool lidarUpdated = (_sub_lidar != nullptr) && _sub_lidar->updated();
bool sonarUpdated = (_sub_sonar != nullptr) && _sub_sonar->updated();
bool landUpdated = landed()
&& ((_timeStamp - _time_last_land) > 1.0e6f / LAND_RATE); // throttle rate
&& ((_timeStamp - _time_last_land) > 1.0e6f / LAND_RATE); // throttle rate
// get new data
updateSubscriptions();
@ -319,7 +316,7 @@ void BlockLocalPositionEstimator::update()
// is xy valid?
bool vxy_stddev_ok = false;
if (math::max(_P(X_vx, X_vx), _P(X_vy, X_vy)) < _vxy_pub_thresh.get()*_vxy_pub_thresh.get()) {
if (math::max(_P(X_vx, X_vx), _P(X_vy, X_vy)) < _vxy_pub_thresh.get() * _vxy_pub_thresh.get()) {
vxy_stddev_ok = true;
}
@ -386,7 +383,6 @@ void BlockLocalPositionEstimator::update()
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] global origin init (parameter) : lat %6.2f lon %6.2f alt %5.1f m",
double(_init_origin_lat.get()), double(_init_origin_lon.get()), double(_altOrigin));
}
// reinitialize x if necessary
@ -599,19 +595,19 @@ void BlockLocalPositionEstimator::publishLocalPos()
_pub_lpos.get().z_valid = _estimatorInitialized & EST_Z;
_pub_lpos.get().v_xy_valid = _estimatorInitialized & EST_XY;
_pub_lpos.get().v_z_valid = _estimatorInitialized & EST_Z;
_pub_lpos.get().x = xLP(X_x); // north
_pub_lpos.get().y = xLP(X_y); // east
_pub_lpos.get().x = xLP(X_x); // north
_pub_lpos.get().y = xLP(X_y); // east
if (_fusion.get() & FUSE_PUB_AGL_Z) {
_pub_lpos.get().z = -_aglLowPass.getState(); // agl
_pub_lpos.get().z = -_aglLowPass.getState(); // agl
} else {
_pub_lpos.get().z = xLP(X_z); // down
_pub_lpos.get().z = xLP(X_z); // down
}
_pub_lpos.get().vx = xLP(X_vx); // north
_pub_lpos.get().vy = xLP(X_vy); // east
_pub_lpos.get().vz = xLP(X_vz); // down
_pub_lpos.get().vx = xLP(X_vx); // north
_pub_lpos.get().vy = xLP(X_vy); // east
_pub_lpos.get().vz = xLP(X_vz); // down
// this estimator does not provide a separate vertical position time derivative estimate, so use the vertical velocity
_pub_lpos.get().z_deriv = xLP(X_vz);
@ -624,7 +620,7 @@ void BlockLocalPositionEstimator::publishLocalPos()
_pub_lpos.get().ref_lon = _map_ref.lon_rad * 180 / M_PI;
_pub_lpos.get().ref_alt = _altOrigin;
_pub_lpos.get().dist_bottom = _aglLowPass.getState();
_pub_lpos.get().dist_bottom_rate = - xLP(X_vz);
_pub_lpos.get().dist_bottom_rate = -xLP(X_vz);
// we estimate agl even when we don't have terrain info
// if you are in terrain following mode this is important
// so that if terrain estimation fails there isn't a
@ -801,7 +797,6 @@ void BlockLocalPositionEstimator::updateSSParams()
_pn_t_noise_density.get() +
(_t_max_grade.get() / 100.0f) * sqrtf(_x(X_vx) * _x(X_vx) + _x(X_vy) * _x(X_vy));
_Q(X_tz, X_tz) = pn_t_noise_density * pn_t_noise_density;
}
void BlockLocalPositionEstimator::predict()
@ -813,7 +808,7 @@ void BlockLocalPositionEstimator::predict()
Vector3f a(_sub_sensor.get().accelerometer_m_s2);
// note, bias is removed in dynamics function
_u = _R_att * a;
_u(U_az) += 9.81f; // add g
_u(U_az) += 9.81f; // add g
// update state space based on new states
updateSSStates();

53
src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
View File

@ -32,10 +32,10 @@
using namespace matrix;
using namespace control;
static const float DELAY_MAX = 0.5f; // seconds
static const float HIST_STEP = 0.05f; // 20 hz
static const float DELAY_MAX = 0.5f; // seconds
static const float HIST_STEP = 0.05f; // 20 hz
static const float BIAS_MAX = 1e-1f;
static const size_t HIST_LEN = 10; // DELAY_MAX / HIST_STEP;
static const size_t HIST_LEN = 10; // DELAY_MAX / HIST_STEP;
static const size_t N_DIST_SUBS = 4;
// for fault detection
@ -73,31 +73,31 @@ class BlockLocalPositionEstimator : public control::SuperBlock
//
//
// input:
// ax, ay, az (acceleration NED)
// ax, ay, az (acceleration NED)
//
// states:
// px, py, pz , ( position NED, m)
// vx, vy, vz ( vel NED, m/s),
// bx, by, bz ( accel bias, m/s^2)
// tz (terrain altitude, ASL, m)
// px, py, pz , ( position NED, m)
// vx, vy, vz ( vel NED, m/s),
// bx, by, bz ( accel bias, m/s^2)
// tz (terrain altitude, ASL, m)
//
// measurements:
//
// sonar: pz (measured d*cos(phi)*cos(theta))
// sonar: pz (measured d*cos(phi)*cos(theta))
//
// baro: pz
// baro: pz
//
// flow: vx, vy (flow is in body x, y frame)
// flow: vx, vy (flow is in body x, y frame)
//
// gps: px, py, pz, vx, vy, vz (flow is in body x, y frame)
// gps: px, py, pz, vx, vy, vz (flow is in body x, y frame)
//
// lidar: pz (actual measured d*cos(phi)*cos(theta))
// lidar: pz (actual measured d*cos(phi)*cos(theta))
//
// vision: px, py, pz, vx, vy, vz
// vision: px, py, pz, vx, vy, vz
//
// mocap: px, py, pz
// mocap: px, py, pz
//
// land (detects when landed)): pz (always measures agl = 0)
// land (detects when landed)): pz (always measures agl = 0)
//
public:
@ -220,7 +220,10 @@ private:
void checkTimeouts();
// misc
inline float agl() { return _x(X_tz) - _x(X_z); }
inline float agl()
{
return _x(X_tz) - _x(X_z);
}
bool landed();
int getDelayPeriods(float delay, uint8_t *periods);
@ -317,7 +320,7 @@ private:
BlockParamFloat _t_max_grade;
// init origin
BlockParamInt _fake_origin;
BlockParamInt _fake_origin;
BlockParamFloat _init_origin_lat;
BlockParamFloat _init_origin_lon;
@ -375,15 +378,15 @@ private:
uint8_t _estimatorInitialized;
// state space
Vector<float, n_x> _x; // state vector
Vector<float, n_u> _u; // input vector
Matrix<float, n_x, n_x> _P; // state covariance matrix
Vector<float, n_x> _x; // state vector
Vector<float, n_u> _u; // input vector
Matrix<float, n_x, n_x> _P; // state covariance matrix
matrix::Dcm<float> _R_att;
Vector3f _eul;
Matrix<float, n_x, n_x> _A; // dynamics matrix
Matrix<float, n_x, n_u> _B; // input matrix
Matrix<float, n_u, n_u> _R; // input covariance
Matrix<float, n_x, n_x> _Q; // process noise covariance
Matrix<float, n_x, n_x> _A; // dynamics matrix
Matrix<float, n_x, n_u> _B; // input matrix
Matrix<float, n_u, n_u> _R; // input covariance
Matrix<float, n_x, n_x> _Q; // process noise covariance
};

6
src/modules/local_position_estimator/sensors/baro.cpp
View File

@ -6,8 +6,8 @@ extern orb_advert_t mavlink_log_pub;
// required number of samples for sensor
// to initialize
static const uint32_t REQ_BARO_INIT_COUNT = 100;
static const uint32_t BARO_TIMEOUT = 100000; // 0.1 s
static const uint32_t REQ_BARO_INIT_COUNT = 100;
static const uint32_t BARO_TIMEOUT = 100000; // 0.1 s
void BlockLocalPositionEstimator::baroInit()
{
@ -59,7 +59,7 @@ void BlockLocalPositionEstimator::baroCorrect()
// baro measurement matrix
Matrix<float, n_y_baro, n_x> C;
C.setZero();
C(Y_baro_z, X_z) = -1; // measured altitude, negative down dir.
C(Y_baro_z, X_z) = -1; // measured altitude, negative down dir.
Matrix<float, n_y_baro, n_y_baro> R;
R.setZero();

10
src/modules/local_position_estimator/sensors/flow.cpp
View File

@ -7,8 +7,8 @@ extern orb_advert_t mavlink_log_pub;
// required number of samples for sensor
// to initialize
static const uint32_t REQ_FLOW_INIT_COUNT = 10;
static const uint32_t FLOW_TIMEOUT = 1000000; // 1 s
static const uint32_t REQ_FLOW_INIT_COUNT = 10;
static const uint32_t FLOW_TIMEOUT = 1000000; // 1 s
// minimum flow altitude
static const float flow_min_agl = 0.3;
@ -89,8 +89,8 @@ int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
// compute velocities in body frame using ground distance
// note that the integral rates in the optical_flow uORB topic are RH rotations about body axes
Vector3f delta_b(
+(flow_y_rad - gyro_y_rad)*d,
-(flow_x_rad - gyro_x_rad)*d,
+(flow_y_rad - gyro_y_rad) * d,
-(flow_x_rad - gyro_x_rad) * d,
0);
// rotation of flow from body to nav frame
@ -201,9 +201,7 @@ void BlockLocalPositionEstimator::flowCorrect()
Vector<float, n_x> dx = K * r;
_x += dx;
_P -= K * C * _P;
}
}
void BlockLocalPositionEstimator::flowCheckTimeout()

10
src/modules/local_position_estimator/sensors/gps.cpp
View File

@ -6,8 +6,8 @@ extern orb_advert_t mavlink_log_pub;
// required number of samples for sensor
// to initialize
static const uint32_t REQ_GPS_INIT_COUNT = 10;
static const uint32_t GPS_TIMEOUT = 1000000; // 1.0 s
static const uint32_t REQ_GPS_INIT_COUNT = 10;
static const uint32_t GPS_TIMEOUT = 1000000; // 1.0 s
void BlockLocalPositionEstimator::gpsInit()
{
@ -188,7 +188,7 @@ void BlockLocalPositionEstimator::gpsCorrect()
Matrix<float, n_y_gps, n_y_gps> S = C * _P * C.transpose() + R;
// publish innovations
for (int i = 0; i < 6; i ++) {
for (int i = 0; i < 6; i++) {
_pub_innov.get().vel_pos_innov[i] = r(i);
_pub_innov.get().vel_pos_innov_var[i] = S(i, i);
}
@ -205,8 +205,8 @@ void BlockLocalPositionEstimator::gpsCorrect()
if (beta / BETA_TABLE[n_y_gps] > beta_thresh) {
if (!(_sensorFault & SENSOR_GPS)) {
mavlink_log_critical(&mavlink_log_pub, "[lpe] gps fault %3g %3g %3g %3g %3g %3g",
double(r(0)*r(0) / S_I(0, 0)), double(r(1)*r(1) / S_I(1, 1)), double(r(2)*r(2) / S_I(2, 2)),
double(r(3)*r(3) / S_I(3, 3)), double(r(4)*r(4) / S_I(4, 4)), double(r(5)*r(5) / S_I(5, 5)));
double(r(0) * r(0) / S_I(0, 0)), double(r(1) * r(1) / S_I(1, 1)), double(r(2) * r(2) / S_I(2, 2)),
double(r(3) * r(3) / S_I(3, 3)), double(r(4) * r(4) / S_I(4, 4)), double(r(5) * r(5) / S_I(5, 5)));
_sensorFault |= SENSOR_GPS;
}

9
src/modules/local_position_estimator/sensors/land.cpp
View File

@ -7,8 +7,8 @@ extern orb_advert_t mavlink_log_pub;
// required number of samples for sensor
// to initialize
//
static const uint32_t REQ_LAND_INIT_COUNT = 1;
static const uint32_t LAND_TIMEOUT = 1000000; // 1.0 s
static const uint32_t REQ_LAND_INIT_COUNT = 1;
static const uint32_t LAND_TIMEOUT = 1000000; // 1.0 s
void BlockLocalPositionEstimator::landInit()
{
@ -48,8 +48,8 @@ void BlockLocalPositionEstimator::landCorrect()
// y = -(z - tz)
C(Y_land_vx, X_vx) = 1;
C(Y_land_vy, X_vy) = 1;
C(Y_land_agl, X_z) = -1; // measured altitude, negative down dir.
C(Y_land_agl, X_tz) = 1; // measured altitude, negative down dir.
C(Y_land_agl, X_z) = -1;// measured altitude, negative down dir.
C(Y_land_agl, X_tz) = 1;// measured altitude, negative down dir.
// use parameter covariance
SquareMatrix<float, n_y_land> R;
@ -101,4 +101,3 @@ void BlockLocalPositionEstimator::landCheckTimeout()
}
}
}

10
src/modules/local_position_estimator/sensors/lidar.cpp
View File

@ -7,8 +7,8 @@ extern orb_advert_t mavlink_log_pub;
// required number of samples for sensor
// to initialize
//
static const uint32_t REQ_LIDAR_INIT_COUNT = 10;
static const uint32_t LIDAR_TIMEOUT = 1000000; // 1.0 s
static const uint32_t REQ_LIDAR_INIT_COUNT = 10;
static const uint32_t LIDAR_TIMEOUT = 1000000; // 1.0 s
void BlockLocalPositionEstimator::lidarInit()
{
@ -34,7 +34,7 @@ int BlockLocalPositionEstimator::lidarMeasure(Vector<float, n_y_lidar> &y)
{
// measure
float d = _sub_lidar->get().current_distance;
float eps = 0.01f; // 1 cm
float eps = 0.01f; // 1 cm
float min_dist = _sub_lidar->get().min_distance + eps;
float max_dist = _sub_lidar->get().max_distance - eps;
@ -70,8 +70,8 @@ void BlockLocalPositionEstimator::lidarCorrect()
C.setZero();
// y = -(z - tz)
// TODO could add trig to make this an EKF correction
C(Y_lidar_z, X_z) = -1; // measured altitude, negative down dir.
C(Y_lidar_z, X_tz) = 1; // measured altitude, negative down dir.
C(Y_lidar_z, X_z) = -1; // measured altitude, negative down dir.
C(Y_lidar_z, X_tz) = 1; // measured altitude, negative down dir.
// use parameter covariance unless sensor provides reasonable value
SquareMatrix<float, n_y_lidar> R;

11
src/modules/local_position_estimator/sensors/mocap.cpp
View File

@ -6,8 +6,8 @@ extern orb_advert_t mavlink_log_pub;
// required number of samples for sensor
// to initialize
static const uint32_t REQ_MOCAP_INIT_COUNT = 20;
static const uint32_t MOCAP_TIMEOUT = 200000; // 0.2 s
static const uint32_t REQ_MOCAP_INIT_COUNT = 20;
static const uint32_t MOCAP_TIMEOUT = 200000; // 0.2 s
void BlockLocalPositionEstimator::mocapInit()
{
@ -67,7 +67,7 @@ void BlockLocalPositionEstimator::mocapCorrect()
// noise matrix
Matrix<float, n_y_mocap, n_y_mocap> R;
R.setZero();
float mocap_p_var = _mocap_p_stddev.get()* \
float mocap_p_var = _mocap_p_stddev.get() * \
_mocap_p_stddev.get();
R(Y_mocap_x, Y_mocap_x) = mocap_p_var;
R(Y_mocap_y, Y_mocap_y) = mocap_p_var;
@ -79,11 +79,12 @@ void BlockLocalPositionEstimator::mocapCorrect()
Matrix<float, n_y_mocap, n_y_mocap> S = C * _P * C.transpose() + R;
// publish innovations
for (int i = 0; i < 3; i ++) {
for (int i = 0; i < 3; i++) {
_pub_innov.get().vel_pos_innov[i] = r(i);
_pub_innov.get().vel_pos_innov_var[i] = S(i, i);
}
for (int i = 3; i < 6; i ++) {
for (int i = 3; i < 6; i++) {
_pub_innov.get().vel_pos_innov[i] = 0;
_pub_innov.get().vel_pos_innov_var[i] = 1;
}

12
src/modules/local_position_estimator/sensors/sonar.cpp
View File

@ -6,9 +6,9 @@ extern orb_advert_t mavlink_log_pub;
// required number of samples for sensor
// to initialize
static const int REQ_SONAR_INIT_COUNT = 10;
static const uint32_t SONAR_TIMEOUT = 5000000; // 2.0 s
static const float SONAR_MAX_INIT_STD = 0.3f; // meters
static const int REQ_SONAR_INIT_COUNT = 10;
static const uint32_t SONAR_TIMEOUT = 5000000; // 2.0 s
static const float SONAR_MAX_INIT_STD = 0.3f; // meters
void BlockLocalPositionEstimator::sonarInit()
{
@ -48,7 +48,7 @@ int BlockLocalPositionEstimator::sonarMeasure(Vector<float, n_y_sonar> &y)
{
// measure
float d = _sub_sonar->get().current_distance;
float eps = 0.01f; // 1 cm
float eps = 0.01f; // 1 cm
float min_dist = _sub_sonar->get().min_distance + eps;
float max_dist = _sub_sonar->get().max_distance - eps;
@ -95,8 +95,8 @@ void BlockLocalPositionEstimator::sonarCorrect()
C.setZero();
// y = -(z - tz)
// TODO could add trig to make this an EKF correction
C(Y_sonar_z, X_z) = -1; // measured altitude, negative down dir.
C(Y_sonar_z, X_tz) = 1; // measured altitude, negative down dir.
C(Y_sonar_z, X_z) = -1; // measured altitude, negative down dir.
C(Y_sonar_z, X_tz) = 1; // measured altitude, negative down dir.
// covariance matrix
SquareMatrix<float, n_y_sonar> R;

6
src/modules/local_position_estimator/sensors/vision.cpp
View File

@ -7,12 +7,12 @@ extern orb_advert_t mavlink_log_pub;
// required number of samples for sensor to initialize.
// This is a vision based position measurement so we assume
// as soon as we get one measurement it is initialized.
static const uint32_t REQ_VISION_INIT_COUNT = 1;
static const uint32_t REQ_VISION_INIT_COUNT = 1;
// We don't want to deinitialize it because
// this will throw away a correction before it starts using the data so we
// set the timeout to 0.5 seconds
static const uint32_t VISION_TIMEOUT = 500000; // 0.5 s
static const uint32_t VISION_TIMEOUT = 500000; // 0.5 s
void BlockLocalPositionEstimator::visionInit()
{
@ -102,7 +102,7 @@ void BlockLocalPositionEstimator::visionCorrect()
// vision delayed x
uint8_t i_hist = 0;
float vision_delay = (_timeStamp - _sub_vision_pos.get().timestamp) * 1e-6f; // measurement delay in seconds
float vision_delay = (_timeStamp - _sub_vision_pos.get().timestamp) * 1e-6f; // measurement delay in seconds
if (vision_delay < 0.0f) { vision_delay = 0.0f; }