fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-02 22:31:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

formatting

Browse Source
This commit is contained in:
ChristophTobler
2016-02-10 09:24:39 +01:00
committed by Lorenz Meier
parent f822b136dc
commit 1a6c4e123c
3 changed files with 78 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/position_estimator_inav/position_estimator_inav_main.cpp
+73 -42
View File
@@ -155,7 +155,7 @@ int position_estimator_inav_main(int argc, char *argv[])
position_estimator_inav_task = px4_task_spawn_cmd("position_estimator_inav", position_estimator_inav_task = px4_task_spawn_cmd("position_estimator_inav",
SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 5300, SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 5300,
position_estimator_inav_thread_main, position_estimator_inav_thread_main,
(argv && argc > 2) ? (char * const *) &argv[2] : (char * const *) NULL); (argv && argc > 2) ? (char *const *) &argv[2] : (char *const *) NULL);
return 0; return 0;
} }
@@ -187,27 +187,35 @@ int position_estimator_inav_main(int argc, char *argv[])
} }
#ifdef INAV_DEBUG #ifdef INAV_DEBUG
static void write_debug_log(const char *msg, float dt, float x_est[2], float y_est[2], float z_est[2], float x_est_prev[2], float y_est_prev[2], float z_est_prev[2], static void write_debug_log(const char *msg, float dt, float x_est[2], float y_est[2], float z_est[2],
float acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v, float corr_mocap[3][1], float w_mocap_p, float x_est_prev[2], float y_est_prev[2], float z_est_prev[2],
float corr_vision[3][2], float w_xy_vision_p, float w_z_vision_p, float w_xy_vision_v) float acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v, float corr_mocap[3][1], float w_mocap_p,
float corr_vision[3][2], float w_xy_vision_p, float w_z_vision_p, float w_xy_vision_v)
{ {
FILE *f = fopen(PX4_ROOTFSDIR"/fs/microsd/inav.log", "a"); FILE *f = fopen(PX4_ROOTFSDIR"/fs/microsd/inav.log", "a");
if (f) { if (f) {
char *s = malloc(256); char *s = malloc(256);
unsigned n = snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f] y_est=[%.5f %.5f] z_est=[%.5f %.5f] x_est_prev=[%.5f %.5f] y_est_prev=[%.5f %.5f] z_est_prev=[%.5f %.5f]\n", unsigned n = snprintf(s, 256,
(unsigned long long)hrt_absolute_time(), msg, (double)dt, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f] y_est=[%.5f %.5f] z_est=[%.5f %.5f] x_est_prev=[%.5f %.5f] y_est_prev=[%.5f %.5f] z_est_prev=[%.5f %.5f]\n",
(double)x_est[0], (double)x_est[1], (double)y_est[0], (double)y_est[1], (double)z_est[0], (double)z_est[1], (unsigned long long)hrt_absolute_time(), msg, (double)dt,
(double)x_est_prev[0], (double)x_est_prev[1], (double)y_est_prev[0], (double)y_est_prev[1], (double)z_est_prev[0], (double)z_est_prev[1]); (double)x_est[0], (double)x_est[1], (double)y_est[0], (double)y_est[1], (double)z_est[0], (double)z_est[1],
(double)x_est_prev[0], (double)x_est_prev[1], (double)y_est_prev[0], (double)y_est_prev[1], (double)z_est_prev[0],
(double)z_est_prev[1]);
fwrite(s, 1, n, f); fwrite(s, 1, n, f);
n = snprintf(s, 256, "\tacc=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f mocap_pos_corr=[%.5f %.5f %.5f] w_mocap_p=%.5f\n", n = snprintf(s, 256,
(double)acc[0], (double)acc[1], (double)acc[2], "\tacc=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f mocap_pos_corr=[%.5f %.5f %.5f] w_mocap_p=%.5f\n",
(double)corr_gps[0][0], (double)corr_gps[1][0], (double)corr_gps[2][0], (double)corr_gps[0][1], (double)corr_gps[1][1], (double)corr_gps[2][1], (double)acc[0], (double)acc[1], (double)acc[2],
(double)w_xy_gps_p, (double)w_xy_gps_v, (double)corr_mocap[0][0], (double)corr_mocap[1][0], (double)corr_mocap[2][0], (double)w_mocap_p); (double)corr_gps[0][0], (double)corr_gps[1][0], (double)corr_gps[2][0], (double)corr_gps[0][1], (double)corr_gps[1][1],
(double)corr_gps[2][1],
(double)w_xy_gps_p, (double)w_xy_gps_v, (double)corr_mocap[0][0], (double)corr_mocap[1][0], (double)corr_mocap[2][0],
(double)w_mocap_p);
fwrite(s, 1, n, f); fwrite(s, 1, n, f);
n = snprintf(s, 256, "\tvision_pos_corr=[%.5f %.5f %.5f] vision_vel_corr=[%.5f %.5f %.5f] w_xy_vision_p=%.5f w_z_vision_p=%.5f w_xy_vision_v=%.5f\n", n = snprintf(s, 256,
(double)corr_vision[0][0], (double)corr_vision[1][0], (double)corr_vision[2][0], (double)corr_vision[0][1], (double)corr_vision[1][1], (double)corr_vision[2][1], "\tvision_pos_corr=[%.5f %.5f %.5f] vision_vel_corr=[%.5f %.5f %.5f] w_xy_vision_p=%.5f w_z_vision_p=%.5f w_xy_vision_v=%.5f\n",
(double)w_xy_vision_p, (double)w_z_vision_p, (double)w_xy_vision_v); (double)corr_vision[0][0], (double)corr_vision[1][0], (double)corr_vision[2][0], (double)corr_vision[0][1],
(double)corr_vision[1][1], (double)corr_vision[2][1],
(double)w_xy_vision_p, (double)w_z_vision_p, (double)w_xy_vision_v);
fwrite(s, 1, n, f); fwrite(s, 1, n, f);
free(s); free(s);
} }
@@ -385,7 +393,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* first parameters read at start up */ /* first parameters read at start up */
struct parameter_update_s param_update; struct parameter_update_s param_update;
orb_copy(ORB_ID(parameter_update), parameter_update_sub, &param_update); /* read from param topic to clear updated flag */ orb_copy(ORB_ID(parameter_update), parameter_update_sub,
&param_update); /* read from param topic to clear updated flag */
/* first parameters update */ /* first parameters update */
inav_parameters_update(&pos_inav_param_handles, &params); inav_parameters_update(&pos_inav_param_handles, &params);
@@ -430,6 +439,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
} }
} }
} }
} else { } else {
PX4_WARN("INAV poll timeout"); PX4_WARN("INAV poll timeout");
} }
@@ -523,26 +533,30 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* lidar alt estimation */ /* lidar alt estimation */
orb_check(distance_sensor_sub, &updated); orb_check(distance_sensor_sub, &updated);
/* update lidar separately, needed by terrain estimator */ /* update lidar separately, needed by terrain estimator */
if (updated) { if (updated) {
orb_copy(ORB_ID(distance_sensor), distance_sensor_sub, &lidar); orb_copy(ORB_ID(distance_sensor), distance_sensor_sub, &lidar);
} }
if (updated && lidar.current_distance > 0.2f && lidar.current_distance < 10.0f && (PX4_R(att.R, 2, 2) > 0.7f)) { //check if altitude estimation for lidar is enabled and new sensor data
if (!use_lidar_prev && use_lidar) if (updated && lidar.current_distance > 0.2f && lidar.current_distance < 10.0f
&& (PX4_R(att.R, 2, 2) > 0.7f)) { //check if altitude estimation for lidar is enabled and new sensor data
if (!use_lidar_prev && use_lidar) {
lidar_first = true; lidar_first = true;
}
use_lidar_prev = use_lidar; use_lidar_prev = use_lidar;
lidar_time = t; lidar_time = t;
dist_ground = lidar.current_distance * PX4_R(att.R, 2, 2); //vertical distance dist_ground = lidar.current_distance * PX4_R(att.R, 2, 2); //vertical distance
if (lidar_first) { if (lidar_first) {
lidar_first = false; lidar_first = false;
lidar_offset = dist_ground + z_est[0]; lidar_offset = dist_ground + z_est[0];
mavlink_log_info(mavlink_fd, "[inav] LIDAR: new ground offset"); mavlink_log_info(mavlink_fd, "[inav] LIDAR: new ground offset");
warnx("[inav] LIDAR: new ground offset"); warnx("[inav] LIDAR: new ground offset");
} }
corr_lidar = lidar_offset - dist_ground - z_est[0]; corr_lidar = lidar_offset - dist_ground - z_est[0];
@@ -550,10 +564,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
corr_lidar = 0; corr_lidar = 0;
lidar_valid = false; lidar_valid = false;
lidar_offset_count++; lidar_offset_count++;
if (lidar_offset_count > 3) { //if consecutive bigger/smaller measurements -> new ground offset -> reinit if (lidar_offset_count > 3) { //if consecutive bigger/smaller measurements -> new ground offset -> reinit
lidar_first = true; lidar_first = true;
lidar_offset_count = 0; lidar_offset_count = 0;
} }
} else { } else {
corr_lidar = lidar_offset - dist_ground - z_est[0]; corr_lidar = lidar_offset - dist_ground - z_est[0];
lidar_valid = true; lidar_valid = true;
@@ -590,9 +606,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
fabsf(body_v_est[0] / flow_dist + att.pitchspeed) < max_flow; fabsf(body_v_est[0] / flow_dist + att.pitchspeed) < max_flow;
/*calculate offset of flow-gyro using already calibrated gyro from autopilot*/ /*calculate offset of flow-gyro using already calibrated gyro from autopilot*/
flow_gyrospeed[0] = flow.gyro_x_rate_integral/(float)flow.integration_timespan*1000000.0f; flow_gyrospeed[0] = flow.gyro_x_rate_integral / (float)flow.integration_timespan * 1000000.0f;
flow_gyrospeed[1] = flow.gyro_y_rate_integral/(float)flow.integration_timespan*1000000.0f; flow_gyrospeed[1] = flow.gyro_y_rate_integral / (float)flow.integration_timespan * 1000000.0f;
flow_gyrospeed[2] = flow.gyro_z_rate_integral/(float)flow.integration_timespan*1000000.0f; flow_gyrospeed[2] = flow.gyro_z_rate_integral / (float)flow.integration_timespan * 1000000.0f;
//moving average //moving average
if (n_flow >= 100) { if (n_flow >= 100) {
@@ -606,6 +622,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
att_gyrospeed_filtered[0] = 0.0f; att_gyrospeed_filtered[0] = 0.0f;
att_gyrospeed_filtered[1] = 0.0f; att_gyrospeed_filtered[1] = 0.0f;
att_gyrospeed_filtered[2] = 0.0f; att_gyrospeed_filtered[2] = 0.0f;
} else { } else {
flow_gyrospeed_filtered[0] = (flow_gyrospeed[0] + n_flow * flow_gyrospeed_filtered[0]) / (n_flow + 1); flow_gyrospeed_filtered[0] = (flow_gyrospeed[0] + n_flow * flow_gyrospeed_filtered[0]) / (n_flow + 1);
flow_gyrospeed_filtered[1] = (flow_gyrospeed[1] + n_flow * flow_gyrospeed_filtered[1]) / (n_flow + 1); flow_gyrospeed_filtered[1] = (flow_gyrospeed[1] + n_flow * flow_gyrospeed_filtered[1]) / (n_flow + 1);
@@ -625,15 +642,17 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* convert raw flow to angular flow (rad/s) */ /* convert raw flow to angular flow (rad/s) */
float flow_ang[2]; float flow_ang[2];
//calculate flow [rad/s] and compensate for rotations (and offset of flow-gyro) //calculate flow [rad/s] and compensate for rotations (and offset of flow-gyro)
flow_ang[0] = ((flow.pixel_flow_x_integral - flow.gyro_x_rate_integral)/(float)flow.integration_timespan*1000000.0f + gyro_offset_filtered[0]) * params.flow_k;//for now the flow has to be scaled (to small) flow_ang[0] = ((flow.pixel_flow_x_integral - flow.gyro_x_rate_integral) / (float)flow.integration_timespan * 1000000.0f
flow_ang[1] = ((flow.pixel_flow_y_integral - flow.gyro_y_rate_integral)/(float)flow.integration_timespan*1000000.0f + gyro_offset_filtered[1]) * params.flow_k;//for now the flow has to be scaled (to small) + gyro_offset_filtered[0]) * params.flow_k;//for now the flow has to be scaled (to small)
flow_ang[1] = ((flow.pixel_flow_y_integral - flow.gyro_y_rate_integral) / (float)flow.integration_timespan * 1000000.0f
+ gyro_offset_filtered[1]) * params.flow_k;//for now the flow has to be scaled (to small)
/* flow measurements vector */ /* flow measurements vector */
float flow_m[3]; float flow_m[3];
flow_m[0] = -flow_ang[0] * flow_dist - yaw_comp[0] * params.flow_k; flow_m[0] = -flow_ang[0] * flow_dist - yaw_comp[0] * params.flow_k;
flow_m[1] = -flow_ang[1] * flow_dist - yaw_comp[1] * params.flow_k; flow_m[1] = -flow_ang[1] * flow_dist - yaw_comp[1] * params.flow_k;
flow_m[2] = z_est[1]; flow_m[2] = z_est[1];
/* velocity in NED */ /* velocity in NED */
float flow_v[2] = { 0.0f, 0.0f }; float flow_v[2] = { 0.0f, 0.0f };
@@ -689,9 +708,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
x_est[1] = vision.vx; x_est[1] = vision.vx;
y_est[0] = vision.y; y_est[0] = vision.y;
y_est[1] = vision.vy; 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) if (params.w_z_vision_p > MIN_VALID_W) {
{
z_est[0] = vision.z; z_est[0] = vision.z;
z_est[1] = vision.vz; z_est[1] = vision.vz;
} }
@@ -729,6 +748,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
corr_vision[0][1] = vision.vx - x_est[1]; corr_vision[0][1] = vision.vx - x_est[1];
corr_vision[1][1] = vision.vy - y_est[1]; corr_vision[1][1] = vision.vy - y_est[1];
corr_vision[2][1] = vision.vz - z_est[1]; corr_vision[2][1] = vision.vz - z_est[1];
} else { } else {
/* assume zero motion */ /* assume zero motion */
corr_vision[0][1] = 0.0f - x_est[1]; corr_vision[0][1] = 0.0f - x_est[1];
@@ -838,6 +858,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* calculate index of estimated values in buffer */ /* calculate index of estimated values in buffer */
int est_i = buf_ptr - 1 - min(EST_BUF_SIZE - 1, max(0, (int)(params.delay_gps * 1000000.0f / PUB_INTERVAL))); int est_i = buf_ptr - 1 - min(EST_BUF_SIZE - 1, max(0, (int)(params.delay_gps * 1000000.0f / PUB_INTERVAL)));
if (est_i < 0) { if (est_i < 0) {
est_i += EST_BUF_SIZE; est_i += EST_BUF_SIZE;
} }
@@ -919,12 +940,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (eph < 0.000001f) { //get case where eph is 0 -> would stay 0 if (eph < 0.000001f) { //get case where eph is 0 -> would stay 0
eph = 0.001; eph = 0.001;
} }
if (eph < max_eph_epv) { if (eph < max_eph_epv) {
eph *= 1.0f + dt; eph *= 1.0f + dt;
} }
if (epv < 0.000001f) { //get case where epv is 0 -> would stay 0 if (epv < 0.000001f) { //get case where epv is 0 -> would stay 0
epv = 0.001; epv = 0.001;
} }
if (epv < max_eph_epv) { if (epv < max_eph_epv) {
epv += 0.005f * dt; // add 1m to EPV each 200s (baro drift) epv += 0.005f * dt; // add 1m to EPV each 200s (baro drift)
} }
@@ -937,9 +961,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
bool use_vision_z = vision_valid && params.w_z_vision_p > MIN_VALID_W; 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 */ /* use MOCAP if it's valid and has a valid weight parameter */
bool use_mocap = mocap_valid && params.w_mocap_p > MIN_VALID_W; bool use_mocap = mocap_valid && params.w_mocap_p > MIN_VALID_W;
if(params.disable_mocap) { //disable mocap if fake gps is used
if (params.disable_mocap) { //disable mocap if fake gps is used
use_mocap = false; use_mocap = false;
} }
/* use flow if it's valid and (accurate or no GPS available) */ /* use flow if it's valid and (accurate or no GPS available) */
bool use_flow = flow_valid && (flow_accurate || !use_gps_xy); bool use_flow = flow_valid && (flow_accurate || !use_gps_xy);
@@ -1046,14 +1072,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
accel_bias_corr[0] = 0.0f; accel_bias_corr[0] = 0.0f;
accel_bias_corr[1] = 0.0f; accel_bias_corr[1] = 0.0f;
accel_bias_corr[2] = 0.0f; accel_bias_corr[2] = 0.0f;
if (use_flow) { if (use_flow) {
accel_bias_corr[0] -= corr_flow[0] * params.w_xy_flow; accel_bias_corr[0] -= corr_flow[0] * params.w_xy_flow;
accel_bias_corr[1] -= corr_flow[1] * params.w_xy_flow; accel_bias_corr[1] -= corr_flow[1] * params.w_xy_flow;
} }
if (use_lidar) { if (use_lidar) {
accel_bias_corr[2] -= corr_lidar * params.w_z_lidar * params.w_z_lidar; accel_bias_corr[2] -= corr_lidar * params.w_z_lidar * params.w_z_lidar;
} else { } else {
accel_bias_corr[2] -= corr_baro * params.w_z_baro * params.w_z_baro; accel_bias_corr[2] -= corr_baro * params.w_z_baro * params.w_z_baro;
} }
@@ -1070,20 +1097,21 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
acc_bias[i] += c * params.w_acc_bias * dt; acc_bias[i] += c * params.w_acc_bias * dt;
} }
} }
/* inertial filter prediction for altitude */ /* inertial filter prediction for altitude */
inertial_filter_predict(dt, z_est, acc[2]); inertial_filter_predict(dt, z_est, acc[2]);
if (!(PX4_ISFINITE(z_est[0]) && PX4_ISFINITE(z_est[1]))) { if (!(PX4_ISFINITE(z_est[0]) && PX4_ISFINITE(z_est[1]))) {
write_debug_log("BAD ESTIMATE AFTER Z PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, write_debug_log("BAD ESTIMATE AFTER Z PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev,
acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p, acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p,
corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v);
memcpy(z_est, z_est_prev, sizeof(z_est)); memcpy(z_est, z_est_prev, sizeof(z_est));
} }
/* inertial filter correction for altitude */ /* inertial filter correction for altitude */
if (use_lidar) { if (use_lidar) {
inertial_filter_correct(corr_lidar, dt, z_est, 0, params.w_z_lidar); inertial_filter_correct(corr_lidar, dt, z_est, 0, params.w_z_lidar);
} else { } else {
inertial_filter_correct(corr_baro, dt, z_est, 0, params.w_z_baro); inertial_filter_correct(corr_baro, dt, z_est, 0, params.w_z_baro);
} }
@@ -1107,8 +1135,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (!(PX4_ISFINITE(z_est[0]) && PX4_ISFINITE(z_est[1]))) { if (!(PX4_ISFINITE(z_est[0]) && PX4_ISFINITE(z_est[1]))) {
write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev,
acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p, acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p,
corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v);
memcpy(z_est, z_est_prev, sizeof(z_est)); memcpy(z_est, z_est_prev, sizeof(z_est));
memset(corr_gps, 0, sizeof(corr_gps)); memset(corr_gps, 0, sizeof(corr_gps));
memset(corr_vision, 0, sizeof(corr_vision)); memset(corr_vision, 0, sizeof(corr_vision));
@@ -1126,8 +1154,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (!(PX4_ISFINITE(x_est[0]) && PX4_ISFINITE(x_est[1]) && PX4_ISFINITE(y_est[0]) && PX4_ISFINITE(y_est[1]))) { if (!(PX4_ISFINITE(x_est[0]) && PX4_ISFINITE(x_est[1]) && PX4_ISFINITE(y_est[0]) && PX4_ISFINITE(y_est[1]))) {
write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev,
acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p, acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p,
corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v);
memcpy(x_est, x_est_prev, sizeof(x_est)); memcpy(x_est, x_est_prev, sizeof(x_est));
memcpy(y_est, y_est_prev, sizeof(y_est)); memcpy(y_est, y_est_prev, sizeof(y_est));
} }
@@ -1173,8 +1201,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (!(PX4_ISFINITE(x_est[0]) && PX4_ISFINITE(x_est[1]) && PX4_ISFINITE(y_est[0]) && PX4_ISFINITE(y_est[1]))) { if (!(PX4_ISFINITE(x_est[0]) && PX4_ISFINITE(x_est[1]) && PX4_ISFINITE(y_est[0]) && PX4_ISFINITE(y_est[1]))) {
write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev,
acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p, acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p,
corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v);
memcpy(x_est, x_est_prev, sizeof(x_est)); memcpy(x_est, x_est_prev, sizeof(x_est));
memcpy(y_est, y_est_prev, sizeof(y_est)); memcpy(y_est, y_est_prev, sizeof(y_est));
memset(corr_gps, 0, sizeof(corr_gps)); memset(corr_gps, 0, sizeof(corr_gps));
@@ -1186,6 +1214,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
memcpy(x_est_prev, x_est, sizeof(x_est)); memcpy(x_est_prev, x_est, sizeof(x_est));
memcpy(y_est_prev, y_est, sizeof(y_est)); memcpy(y_est_prev, y_est, sizeof(y_est));
} }
} else { } else {
/* gradually reset xy velocity estimates */ /* gradually reset xy velocity estimates */
inertial_filter_correct(-x_est[1], dt, x_est, 1, params.w_xy_res_v); inertial_filter_correct(-x_est[1], dt, x_est, 1, params.w_xy_res_v);
@@ -1235,6 +1264,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
memcpy(R_buf[buf_ptr], att.R, sizeof(att.R)); memcpy(R_buf[buf_ptr], att.R, sizeof(att.R));
buf_ptr++; buf_ptr++;
if (buf_ptr >= EST_BUF_SIZE) { if (buf_ptr >= EST_BUF_SIZE) {
buf_ptr = 0; buf_ptr = 0;
} }
@@ -1289,6 +1319,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (terrain_estimator->is_valid()) { if (terrain_estimator->is_valid()) {
global_pos.terrain_alt = global_pos.alt - terrain_estimator->get_distance_to_ground(); global_pos.terrain_alt = global_pos.alt - terrain_estimator->get_distance_to_ground();
global_pos.terrain_alt_valid = true; global_pos.terrain_alt_valid = true;
} else { } else {
global_pos.terrain_alt_valid = false; global_pos.terrain_alt_valid = false;
} }
src/modules/position_estimator_inav/position_estimator_inav_params.cpp
+3 -2
View File
@@ -150,7 +150,7 @@ PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_V, 0.0f);
* @group Position Estimator INAV * @group Position Estimator INAV
*/ */
PARAM_DEFINE_FLOAT(INAV_W_MOC_P, 10.0f); PARAM_DEFINE_FLOAT(INAV_W_MOC_P, 10.0f);
/** /**
* XY axis weight for optical flow * XY axis weight for optical flow
@@ -379,7 +379,8 @@ int inav_parameters_init(struct position_estimator_inav_param_handles *h)
return 0; return 0;
} }
int inav_parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p) int inav_parameters_update(const struct position_estimator_inav_param_handles *h,
struct position_estimator_inav_params *p)
{ {
param_get(h->w_z_baro, &(p->w_z_baro)); param_get(h->w_z_baro, &(p->w_z_baro));
param_get(h->w_z_gps_p, &(p->w_z_gps_p)); param_get(h->w_z_gps_p, &(p->w_z_gps_p));
src/modules/position_estimator_inav/position_estimator_inav_params.h
+2 -1
View File
@@ -111,4 +111,5 @@ int inav_parameters_init(struct position_estimator_inav_param_handles *h);
* Update all parameters * Update all parameters
* *
*/ */
int inav_parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p); int inav_parameters_update(const struct position_estimator_inav_param_handles *h,
struct position_estimator_inav_params *p);