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

make default apps compatible with autogenerated attitude and rc_channels message

Browse Source
This commit is contained in:
Thomas Gubler
2014-12-02 12:19:24 +01:00
parent 141dda2092
commit e28e8c11bb
7 changed files with 66 additions and 61 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/position_estimator_inav/position_estimator_inav_main.c
+14 -13
View File
@@ -68,6 +68,7 @@
#include <geo/geo.h>
#include <systemlib/systemlib.h>
#include <drivers/drv_hrt.h>
#include <px4_defines.h>
#include "position_estimator_inav_params.h"
#include "inertial_filter.h"
@@ -282,13 +283,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
};
float w_gps_xy = 1.0f;
float w_gps_z = 1.0f;
float corr_vision[3][2] = {
{ 0.0f, 0.0f }, // N (pos, vel)
{ 0.0f, 0.0f }, // E (pos, vel)
{ 0.0f, 0.0f }, // D (pos, vel)
};
float corr_sonar = 0.0f;
float corr_sonar_filtered = 0.0f;
@@ -461,7 +462,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
acc[i] = 0.0f;
for (int j = 0; j < 3; j++) {
acc[i] += att.R[i][j] * sensor.accelerometer_m_s2[j];
acc[i] += PX4_R(att.R, i, j) * sensor.accelerometer_m_s2[j];
}
}
@@ -494,7 +495,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if ((flow.ground_distance_m > 0.31f) &&
(flow.ground_distance_m < 4.0f) &&
(att.R[2][2] > 0.7f) &&
(PX4_R(att.R, 2, 2) > 0.7f) &&
(fabsf(flow.ground_distance_m - sonar_prev) > FLT_EPSILON)) {
sonar_time = t;
@@ -531,15 +532,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float flow_q = flow.quality / 255.0f;
float dist_bottom = - z_est[0] - surface_offset;
if (dist_bottom > 0.3f && flow_q > params.flow_q_min && (t < sonar_valid_time + sonar_valid_timeout) && att.R[2][2] > 0.7f) {
if (dist_bottom > 0.3f && flow_q > params.flow_q_min && (t < sonar_valid_time + sonar_valid_timeout) && PX4_R(att.R, 2, 2) > 0.7f) {
/* distance to surface */
float flow_dist = dist_bottom / att.R[2][2];
float flow_dist = dist_bottom / PX4_R(att.R, 2, 2);
/* check if flow if too large for accurate measurements */
/* calculate estimated velocity in body frame */
float body_v_est[2] = { 0.0f, 0.0f };
for (int i = 0; i < 2; i++) {
body_v_est[i] = att.R[0][i] * x_est[1] + att.R[1][i] * y_est[1] + att.R[2][i] * z_est[1];
body_v_est[i] = PX4_R(att.R, 0, i) * x_est[1] + PX4_R(att.R, 1, i) * y_est[1] + PX4_R(att.R, 2, i) * z_est[1];
}
/* set this flag if flow should be accurate according to current velocity and attitude rate estimate */
@@ -562,7 +563,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* project measurements vector to NED basis, skip Z component */
for (int i = 0; i < 2; i++) {
for (int j = 0; j < 3; j++) {
flow_v[i] += att.R[i][j] * flow_m[j];
flow_v[i] += PX4_R(att.R, i, j) * flow_m[j];
}
}
@@ -571,7 +572,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
corr_flow[1] = flow_v[1] - y_est[1];
/* adjust correction weight */
float flow_q_weight = (flow_q - params.flow_q_min) / (1.0f - params.flow_q_min);
w_flow = att.R[2][2] * flow_q_weight / fmaxf(1.0f, flow_dist);
w_flow = PX4_R(att.R, 2, 2) * flow_q_weight / fmaxf(1.0f, flow_dist);
/* if flow is not accurate, reduce weight for it */
// TODO make this more fuzzy
@@ -646,13 +647,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
x_est[1] = vision.vx;
y_est[0] = vision.y;
y_est[1] = vision.vy;
/* only reset the z estimate if the z weight parameter is not zero */
/* only reset the z estimate if the z weight parameter is not zero */
if (params.w_z_vision_p > MIN_VALID_W)
{
z_est[0] = vision.z;
z_est[1] = vision.vz;
}
vision_valid = true;
warnx("VISION estimate valid");
mavlink_log_info(mavlink_fd, "[inav] VISION estimate valid");
@@ -912,7 +913,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float c = 0.0f;
for (int j = 0; j < 3; j++) {
c += att.R[j][i] * accel_bias_corr[j];
c += PX4_R(att.R, j, i) * accel_bias_corr[j];
}
if (isfinite(c)) {
@@ -937,7 +938,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float c = 0.0f;
for (int j = 0; j < 3; j++) {
c += att.R[j][i] * accel_bias_corr[j];
c += PX4_R(att.R, j, i) * accel_bias_corr[j];
}
if (isfinite(c)) {