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

lpe: style fix

Browse Source
This commit is contained in:
TSC21
2017-11-03 23:45:43 +01:00
committed by Lorenz Meier
parent 051822eee1
commit 483f0d55e0
10 changed files with 96 additions and 100 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/local_position_estimator/sensors/gps.cpp
+5 -5
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;
}