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

added a disabling flag for mocap when using fake gps

Browse Source
This commit is contained in:
ChristophTobler
2015-11-17 16:47:32 +01:00
parent dfdf7dce4d
commit b0515ef07d
4 changed files with 34 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/position_estimator_inav/CMakeLists.txt
+1 -1
View File
@@ -31,7 +31,7 @@
#
############################################################################
if(${OS} STREQUAL "nuttx")
list(APPEND MODULE_CFLAGS -Wframe-larger-than=3890)
list(APPEND MODULE_CFLAGS -Wframe-larger-than=4000)
endif()
px4_add_module(
MODULE modules__position_estimator_inav
src/modules/position_estimator_inav/position_estimator_inav_main.c
+16 -7
View File
@@ -586,8 +586,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* set this flag if flow should be accurate according to current velocity and attitude rate estimate */
flow_accurate = fabsf(body_v_est[1] / flow_dist - att.rollspeed) < max_flow &&
fabsf(body_v_est[0] / flow_dist + att.pitchspeed) < max_flow;
//this flag is not working -->
flow_accurate = true; //already checked if flow_q > 0.3
/*calculate offset of flow-gyro using already calibrated gyro from autopilot*/
@@ -817,7 +815,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (gps_valid) {
if (gps.eph > max_eph_epv || gps.epv > max_eph_epv || gps.fix_type < 3) {
gps_valid = false;
ref_inited = false; //if gps gets lost it has to init again to avoid jumps
mavlink_log_info(mavlink_fd, "[inav] GPS signal lost");
}
@@ -841,7 +838,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
} else if (t > ref_init_start + ref_init_delay) {
ref_inited = true;
/* set position estimate to (0, 0, 0), use GPS velocity for XY */
x_est[0] = 0.0f;
x_est[1] = gps.vel_n_m_s;
@@ -866,7 +863,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float gps_proj[2];
map_projection_project(&ref, lat, lon, &(gps_proj[0]), &(gps_proj[1]));
/* reset position estimate when GPS becomes good */
/* reset position estimate and ref when GPS becomes good */
if (reset_est) {
x_est[0] = gps_proj[0];
x_est[1] = gps.vel_n_m_s;
@@ -880,6 +877,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
est_i += EST_BUF_SIZE;
}
//mavlink_log_info(mavlink_fd, "est_i = %d\n", (int)est_i);
/* calculate correction for position */
corr_gps[0][0] = gps_proj[0] - est_buf[est_i][0][0];
corr_gps[1][0] = gps_proj[1] - est_buf[est_i][1][0];
@@ -954,9 +953,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
t_prev = t;
/* increase EPH/EPV on each step */
if (eph < 0.000001f) { //get case where eph is 0 -> would stay 0
eph = 0.001;
}
if (eph < max_eph_epv) {
eph *= 1.0f + dt;
}
if (epv < 0.000001f) { //get case where epv is 0 -> would stay 0
epv = 0.001;
}
if (epv < max_eph_epv) {
epv += 0.005f * dt; // add 1m to EPV each 200s (baro drift)
}
@@ -969,11 +974,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
bool use_vision_z = vision_valid && params.w_z_vision_p > MIN_VALID_W;
/* use MOCAP if it's valid and has a valid weight parameter */
bool use_mocap = mocap_valid && params.w_mocap_p > MIN_VALID_W;
if(params.disable_mocap) { //disable mocap if fake gps is used
use_mocap = false;
}
/* use flow if it's valid and (accurate or no GPS available) */
bool use_flow = flow_valid && (flow_accurate || !use_gps_xy);
bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow || use_vision_xy || use_mocap;
//mavlink_log_info(mavlink_fd, "eph = %2.6f\t gps eph = %2.4f\n", (double)eph, (double)gps.eph);
bool dist_bottom_valid = (t < lidar_valid_time + lidar_valid_timeout);
if (dist_bottom_valid) {
@@ -1167,7 +1176,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
inertial_filter_correct(corr_flow[0], dt, x_est, 1, params.w_xy_flow * w_flow);
inertial_filter_correct(corr_flow[1], dt, y_est, 1, params.w_xy_flow * w_flow);
//mavlink_log_info(mavlink_fd, "w_flow = %2.4f\t w_xy_flow = %2.4f\n", (double)w_flow, (double)params.w_xy_flow);
}
if (use_gps_xy) {
@@ -1265,6 +1273,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
buf_ptr = 0;
}
/* publish local position */
local_pos.xy_valid = can_estimate_xy;
local_pos.v_xy_valid = can_estimate_xy;
src/modules/position_estimator_inav/position_estimator_inav_params.c
+15 -2
View File
@@ -158,10 +158,10 @@ PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_V, 0.0f);
* Weight (cutoff frequency) for optical flow (velocity) measurements.
*
* @min 0.0
* @max 30.0
* @max 10.0
* @group Position Estimator INAV
*/
PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 10.0f);
PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 9.0f);
/**
* XY axis weight for resetting velocity
@@ -313,6 +313,17 @@ PARAM_DEFINE_FLOAT(INAV_FLOW_DIST_X, 0.0f);
*/
PARAM_DEFINE_FLOAT(INAV_FLOW_DIST_Y, 0.0f);
/**
* Disable mocap (set 0 if using fake gps)
*
* Disable mocap
*
* @min 0
* @max 1
* @group Position Estimator INAV
*/
PARAM_DEFINE_FLOAT(INAV_DISAB_MOCAP, 0);
/**
* Disable vision input
*
@@ -364,6 +375,7 @@ int inav_parameters_init(struct position_estimator_inav_param_handles *h)
h->delay_gps = param_find("INAV_DELAY_GPS");
h->flow_module_offset_x = param_find("INAV_FLOW_DIST_X");
h->flow_module_offset_y = param_find("INAV_FLOW_DIST_Y");
h->disable_mocap = param_find("INAV_DISAB_MOCAP");
return 0;
}
@@ -394,6 +406,7 @@ int inav_parameters_update(const struct position_estimator_inav_param_handles *h
param_get(h->delay_gps, &(p->delay_gps));
param_get(h->flow_module_offset_x, &(p->flow_module_offset_x));
param_get(h->flow_module_offset_y, &(p->flow_module_offset_y));
param_get(h->disable_mocap, &(p->disable_mocap));
return 0;
}
src/modules/position_estimator_inav/position_estimator_inav_params.h
+2
View File
@@ -67,6 +67,7 @@ struct position_estimator_inav_params {
float delay_gps;
float flow_module_offset_x;
float flow_module_offset_y;
int32_t disable_mocap;
};
struct position_estimator_inav_param_handles {
@@ -95,6 +96,7 @@ struct position_estimator_inav_param_handles {
param_t delay_gps;
param_t flow_module_offset_x;
param_t flow_module_offset_y;
param_t disable_mocap;
};
#define CBRK_NO_VISION_KEY 328754