From 1235c1f0bdd61bd5bb60df6a8e20c28b5dd4ac39 Mon Sep 17 00:00:00 2001 From: ChristophTobler Date: Tue, 3 Nov 2015 08:30:14 +0100 Subject: [PATCH 1/4] gps re-init after loss --- .../position_estimator_inav/position_estimator_inav_main.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 04f3c4d918..d10ff2d0df 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -817,6 +817,7 @@ 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"); } From dfdf7dce4d6dac46016fdd849968e6eb8d08a461 Mon Sep 17 00:00:00 2001 From: ChristophTobler Date: Tue, 17 Nov 2015 16:38:11 +0100 Subject: [PATCH 2/4] new mode for fake gps --- ROMFS/px4fmu_common/init.d/rcS | 4 ++++ src/modules/mavlink/mavlink_main.cpp | 9 ++++++++- src/modules/mavlink/mavlink_main.h | 1 + 3 files changed, 13 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index c4b25987be..411eb0d892 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -564,6 +564,10 @@ then then mavlink start -d /dev/ttyS2 -b 57600 -m osd -r 1000 fi + if param compare SYS_COMPANION 257600 + then + mavlink start -d /dev/ttyS2 -b 57600 -m magic -r 5000 -x + fi # Sensors on the PWM interface bank # clear pins 5 and 6 if param compare SENS_EN_LL40LS 1 diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 3a2f1d583b..1b04eea1a0 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1523,6 +1523,9 @@ Mavlink::task_main(int argc, char *argv[]) } else if (strcmp(myoptarg, "osd") == 0) { _mode = MAVLINK_MODE_OSD; + } else if (strcmp(myoptarg, "magic") == 0) { + _mode = MAVLINK_MODE_MAGIC; + } else if (strcmp(myoptarg, "config") == 0) { _mode = MAVLINK_MODE_CONFIG; } @@ -1709,7 +1712,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("SYSTEM_TIME", 1.0f); configure_stream("TIMESYNC", 10.0f); configure_stream("ACTUATOR_CONTROL_TARGET0", 10.0f); - /* camera trigger is rate limited at the source, do not limit here */ + //camera trigger is rate limited at the source, do not limit here configure_stream("CAMERA_TRIGGER", 500.0f); configure_stream("EXTENDED_SYS_STATE", 2.0f); break; @@ -1729,6 +1732,10 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("EXTENDED_SYS_STATE", 1.0f); break; + case MAVLINK_MODE_MAGIC: + //stream nothing + break; + case MAVLINK_MODE_CONFIG: // Enable a number of interesting streams we want via USB configure_stream("SYS_STATUS", 1.0f); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 95e0fdb91a..37c66b5963 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -149,6 +149,7 @@ public: MAVLINK_MODE_CUSTOM, MAVLINK_MODE_ONBOARD, MAVLINK_MODE_OSD, + MAVLINK_MODE_MAGIC, MAVLINK_MODE_CONFIG }; From b0515ef07dabd5e26e84332ba8501d40a1c7f15b Mon Sep 17 00:00:00 2001 From: ChristophTobler Date: Tue, 17 Nov 2015 16:47:32 +0100 Subject: [PATCH 3/4] added a disabling flag for mocap when using fake gps --- .../position_estimator_inav/CMakeLists.txt | 2 +- .../position_estimator_inav_main.c | 23 +++++++++++++------ .../position_estimator_inav_params.c | 17 ++++++++++++-- .../position_estimator_inav_params.h | 2 ++ 4 files changed, 34 insertions(+), 10 deletions(-) diff --git a/src/modules/position_estimator_inav/CMakeLists.txt b/src/modules/position_estimator_inav/CMakeLists.txt index 2d5315b92b..60429f5d22 100644 --- a/src/modules/position_estimator_inav/CMakeLists.txt +++ b/src/modules/position_estimator_inav/CMakeLists.txt @@ -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 diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 4696d4712b..32cb74ec8b 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -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; diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index be8a136c4d..f303abaf6d 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -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; } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h index 43601568f8..07b78104bf 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -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 From 8044c5aaf660f174e878bce0116d599165ad9558 Mon Sep 17 00:00:00 2001 From: ChristophTobler Date: Tue, 17 Nov 2015 16:59:44 +0100 Subject: [PATCH 4/4] remove unnecessary lines --- .../position_estimator_inav/position_estimator_inav_main.c | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 32cb74ec8b..d8f29f1082 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -863,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 and ref when GPS becomes good */ + /* reset position estimate when GPS becomes good */ if (reset_est) { x_est[0] = gps_proj[0]; x_est[1] = gps.vel_n_m_s; @@ -877,8 +877,6 @@ 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]; @@ -982,7 +980,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) 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) {