diff options
Diffstat (limited to 'src/modules/position_estimator_inav')
3 files changed, 206 insertions, 24 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 05eae047c..296919c04 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -59,6 +59,7 @@ #include <uORB/topics/vehicle_local_position.h> #include <uORB/topics/vehicle_global_position.h> #include <uORB/topics/vehicle_gps_position.h> +#include <uORB/topics/vision_position_estimate.h> #include <uORB/topics/home_position.h> #include <uORB/topics/optical_flow.h> #include <mavlink/mavlink_log.h> @@ -80,6 +81,7 @@ static bool thread_running = false; /**< Deamon status flag */ static int position_estimator_inav_task; /**< Handle of deamon task / thread */ static bool verbose_mode = false; +static const hrt_abstime vision_topic_timeout = 500000; // Vision topic timeout = 0.5s static const hrt_abstime gps_topic_timeout = 500000; // GPS topic timeout = 0.5s static const hrt_abstime flow_topic_timeout = 1000000; // optical flow topic timeout = 1s static const hrt_abstime sonar_timeout = 150000; // sonar timeout = 150ms @@ -159,7 +161,7 @@ int position_estimator_inav_main(int argc, char *argv[]) thread_should_exit = true; } else { - warnx("app not started"); + warnx("not started"); } exit(0); @@ -167,10 +169,10 @@ int position_estimator_inav_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { - warnx("app is running"); + warnx("is running"); } else { - warnx("app not started"); + warnx("not started"); } exit(0); @@ -208,10 +210,8 @@ static void write_debug_log(const char *msg, float dt, float x_est[2], float y_e ****************************************************************************/ int position_estimator_inav_thread_main(int argc, char *argv[]) { - warnx("started"); int mavlink_fd; mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - mavlink_log_info(mavlink_fd, "[inav] started"); float x_est[2] = { 0.0f, 0.0f }; // pos, vel float y_est[2] = { 0.0f, 0.0f }; // pos, vel @@ -233,6 +233,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float eph_flow = 1.0f; + float eph_vision = 0.5f; + float epv_vision = 0.5f; + float x_est_prev[2], y_est_prev[2], z_est_prev[2]; memset(x_est_prev, 0, sizeof(x_est_prev)); memset(y_est_prev, 0, sizeof(y_est_prev)); @@ -279,6 +282,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; @@ -286,7 +296,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float w_flow = 0.0f; float sonar_prev = 0.0f; - hrt_abstime flow_prev = 0; // time of last flow measurement + //hrt_abstime flow_prev = 0; // time of last flow measurement hrt_abstime sonar_time = 0; // time of last sonar measurement (not filtered) hrt_abstime sonar_valid_time = 0; // time of last sonar measurement used for correction (filtered) @@ -294,6 +304,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) bool sonar_valid = false; // sonar is valid bool flow_valid = false; // flow is valid bool flow_accurate = false; // flow should be accurate (this flag not updated if flow_valid == false) + bool vision_valid = false; /* declare and safely initialize all structs */ struct actuator_controls_s actuator; @@ -312,6 +323,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) memset(&local_pos, 0, sizeof(local_pos)); struct optical_flow_s flow; memset(&flow, 0, sizeof(flow)); + struct vision_position_estimate vision; + memset(&vision, 0, sizeof(vision)); struct vehicle_global_position_s global_pos; memset(&global_pos, 0, sizeof(global_pos)); @@ -323,6 +336,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow)); int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + int vision_position_estimate_sub = orb_subscribe(ORB_ID(vision_position_estimate)); int home_position_sub = orb_subscribe(ORB_ID(home_position)); /* advertise */ @@ -373,8 +387,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } else { wait_baro = false; baro_offset /= (float) baro_init_cnt; - warnx("baro offs: %.2f", (double)baro_offset); - mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", (double)baro_offset); + warnx("baro offs: %d", (int)baro_offset); + mavlink_log_info(mavlink_fd, "[inav] baro offs: %d", (int)baro_offset); local_pos.z_valid = true; local_pos.v_z_valid = true; } @@ -475,8 +489,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow); /* calculate time from previous update */ - float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f; - flow_prev = flow.flow_timestamp; +// float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f; +// flow_prev = flow.flow_timestamp; if ((flow.ground_distance_m > 0.31f) && (flow.ground_distance_m < 4.0f) && @@ -504,7 +518,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) sonar_valid_time = t; sonar_valid = true; local_pos.surface_bottom_timestamp = t; - mavlink_log_info(mavlink_fd, "[inav] new surface level: %.2f", (double)surface_offset); + mavlink_log_info(mavlink_fd, "[inav] new surface level: %d", (int)surface_offset); } } else { @@ -534,8 +548,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* convert raw flow to angular flow (rad/s) */ float flow_ang[2]; - flow_ang[0] = flow.flow_raw_x * params.flow_k / 1000.0f / flow_dt; - flow_ang[1] = flow.flow_raw_y * params.flow_k / 1000.0f / flow_dt; + //todo check direction of x und y axis + flow_ang[0] = flow.pixel_flow_x_integral/(float)flow.integration_timespan*1000000.0f;//flow.flow_raw_x * params.flow_k / 1000.0f / flow_dt; + flow_ang[1] = flow.pixel_flow_y_integral/(float)flow.integration_timespan*1000000.0f;//flow.flow_raw_y * params.flow_k / 1000.0f / flow_dt; /* flow measurements vector */ float flow_m[3]; flow_m[0] = -flow_ang[0] * flow_dist; @@ -616,6 +631,46 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } + + /* check no vision circuit breaker is set */ + if (params.no_vision != CBRK_NO_VISION_KEY) { + /* vehicle vision position */ + orb_check(vision_position_estimate_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vision_position_estimate), vision_position_estimate_sub, &vision); + + /* reset position estimate on first vision update */ + if (!vision_valid) { + x_est[0] = vision.x; + 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 */ + 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"); + } + + /* calculate correction for position */ + corr_vision[0][0] = vision.x - x_est[0]; + corr_vision[1][0] = vision.y - y_est[0]; + corr_vision[2][0] = vision.z - z_est[0]; + + /* calculate correction for velocity */ + corr_vision[0][1] = vision.vx - x_est[1]; + corr_vision[1][1] = vision.vy - y_est[1]; + corr_vision[2][1] = vision.vz - z_est[1]; + + } + } + /* vehicle GPS position */ orb_check(vehicle_gps_position_sub, &updated); @@ -665,8 +720,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* initialize projection */ map_projection_init(&ref, lat, lon); - warnx("init ref: lat=%.7f, lon=%.7f, alt=%.2f", (double)lat, (double)lon, (double)alt); - mavlink_log_info(mavlink_fd, "[inav] init ref: %.7f, %.7f, %.2f", (double)lat, (double)lon, (double)alt); + // XXX replace this print + warnx("init ref: lat=%.7f, lon=%.7f, alt=%8.4f", (double)lat, (double)lon, (double)alt); + mavlink_log_info(mavlink_fd, "[inav] init ref: %.7f, %.7f, %8.4f", (double)lat, (double)lon, (double)alt); } } @@ -732,14 +788,21 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } /* check for timeout on GPS topic */ - if (gps_valid && t > gps.timestamp_position + gps_topic_timeout) { + if (gps_valid && (t > (gps.timestamp_position + gps_topic_timeout))) { gps_valid = false; warnx("GPS timeout"); mavlink_log_info(mavlink_fd, "[inav] GPS timeout"); } + /* check for timeout on vision topic */ + if (vision_valid && (t > (vision.timestamp_boot + vision_topic_timeout))) { + vision_valid = false; + warnx("VISION timeout"); + mavlink_log_info(mavlink_fd, "[inav] VISION timeout"); + } + /* check for sonar measurement timeout */ - if (sonar_valid && t > sonar_time + sonar_timeout) { + if (sonar_valid && (t > (sonar_time + sonar_timeout))) { corr_sonar = 0.0f; sonar_valid = false; } @@ -759,10 +822,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* use GPS if it's valid and reference position initialized */ bool use_gps_xy = ref_inited && gps_valid && params.w_xy_gps_p > MIN_VALID_W; bool use_gps_z = ref_inited && gps_valid && params.w_z_gps_p > MIN_VALID_W; + /* use VISION if it's valid and has a valid weight parameter */ + bool use_vision_xy = vision_valid && params.w_xy_vision_p > MIN_VALID_W; + bool use_vision_z = vision_valid && params.w_z_vision_p > MIN_VALID_W; /* 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; + bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow || use_vision_xy; bool dist_bottom_valid = (t < sonar_valid_time + sonar_valid_timeout); @@ -781,6 +847,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float w_xy_gps_v = params.w_xy_gps_v * w_gps_xy; float w_z_gps_p = params.w_z_gps_p * w_gps_z; + float w_xy_vision_p = params.w_xy_vision_p; + float w_xy_vision_v = params.w_xy_vision_v; + float w_z_vision_p = params.w_z_vision_p; + /* reduce GPS weight if optical flow is good */ if (use_flow && flow_accurate) { w_xy_gps_p *= params.w_gps_flow; @@ -821,6 +891,35 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } + /* accelerometer bias correction for VISION (use buffered rotation matrix) */ + accel_bias_corr[0] = 0.0f; + accel_bias_corr[1] = 0.0f; + accel_bias_corr[2] = 0.0f; + + if (use_vision_xy) { + accel_bias_corr[0] -= corr_vision[0][0] * w_xy_vision_p * w_xy_vision_p; + accel_bias_corr[0] -= corr_vision[0][1] * w_xy_vision_v; + accel_bias_corr[1] -= corr_vision[1][0] * w_xy_vision_p * w_xy_vision_p; + accel_bias_corr[1] -= corr_vision[1][1] * w_xy_vision_v; + } + + if (use_vision_z) { + accel_bias_corr[2] -= corr_vision[2][0] * w_z_vision_p * w_z_vision_p; + } + + /* transform error vector from NED frame to body frame */ + for (int i = 0; i < 3; i++) { + float c = 0.0f; + + for (int j = 0; j < 3; j++) { + c += att.R[j][i] * accel_bias_corr[j]; + } + + if (isfinite(c)) { + acc_bias[i] += c * params.w_acc_bias * dt; + } + } + /* accelerometer bias correction for flow and baro (assume that there is no delay) */ accel_bias_corr[0] = 0.0f; accel_bias_corr[1] = 0.0f; @@ -863,10 +962,16 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p); } + if (use_vision_z) { + epv = fminf(epv, epv_vision); + inertial_filter_correct(corr_vision[2][0], dt, z_est, 0, w_z_vision_p); + } + if (!(isfinite(z_est[0]) && 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, acc, corr_gps, w_xy_gps_p, w_xy_gps_v); memcpy(z_est, z_est_prev, sizeof(z_est)); memset(corr_gps, 0, sizeof(corr_gps)); + memset(corr_vision, 0, sizeof(corr_vision)); corr_baro = 0; } else { @@ -904,11 +1009,24 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } + if (use_vision_xy) { + eph = fminf(eph, eph_vision); + + inertial_filter_correct(corr_vision[0][0], dt, x_est, 0, w_xy_vision_p); + inertial_filter_correct(corr_vision[1][0], dt, y_est, 0, w_xy_vision_p); + + if (w_xy_vision_v > MIN_VALID_W) { + inertial_filter_correct(corr_vision[0][1], dt, x_est, 1, w_xy_vision_v); + inertial_filter_correct(corr_vision[1][1], dt, y_est, 1, w_xy_vision_v); + } + } + if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(y_est[0]) && 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, acc, corr_gps, w_xy_gps_p, w_xy_gps_v); memcpy(x_est, x_est_prev, sizeof(x_est)); memcpy(y_est, y_est_prev, sizeof(y_est)); memset(corr_gps, 0, sizeof(corr_gps)); + memset(corr_vision, 0, sizeof(corr_vision)); memset(corr_flow, 0, sizeof(corr_flow)); } else { 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 98b31d17b..cc0fb4155 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 Anton Babushkin. All rights reserved. - * Author: Anton Babushkin <rk3dov@gmail.com> + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,6 +34,8 @@ /* * @file position_estimator_inav_params.c * + * @author Anton Babushkin <rk3dov@gmail.com> + * * Parameters for position_estimator_inav */ @@ -63,6 +64,17 @@ PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 0.5f); PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.005f); /** + * Z axis weight for vision + * + * Weight (cutoff frequency) for vision altitude measurements. vision altitude data is very noisy and should be used only as slow correction for baro offset. + * + * @min 0.0 + * @max 10.0 + * @group Position Estimator INAV + */ +PARAM_DEFINE_FLOAT(INAV_W_Z_VIS_P, 0.5f); + +/** * Z axis weight for sonar * * Weight (cutoff frequency) for sonar measurements. @@ -96,6 +108,28 @@ PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_P, 1.0f); PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f); /** + * XY axis weight for vision position + * + * Weight (cutoff frequency) for vision position measurements. + * + * @min 0.0 + * @max 10.0 + * @group Position Estimator INAV + */ +PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_P, 5.0f); + +/** + * XY axis weight for vision velocity + * + * Weight (cutoff frequency) for vision velocity measurements. + * + * @min 0.0 + * @max 10.0 + * @group Position Estimator INAV + */ +PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_V, 0.0f); + +/** * XY axis weight for optical flow * * Weight (cutoff frequency) for optical flow (velocity) measurements. @@ -232,13 +266,27 @@ PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.2f); */ PARAM_DEFINE_FLOAT(INAV_DELAY_GPS, 0.2f); +/** + * Disable vision input + * + * Set to the appropriate key (328754) to disable vision input. + * + * @min 0.0 + * @max 1.0 + * @group Position Estimator INAV + */ +PARAM_DEFINE_INT32(CBRK_NO_VISION, 0); + int parameters_init(struct position_estimator_inav_param_handles *h) { h->w_z_baro = param_find("INAV_W_Z_BARO"); h->w_z_gps_p = param_find("INAV_W_Z_GPS_P"); + h->w_z_vision_p = param_find("INAV_W_Z_VIS_P"); h->w_z_sonar = param_find("INAV_W_Z_SONAR"); h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P"); h->w_xy_gps_v = param_find("INAV_W_XY_GPS_V"); + h->w_xy_vision_p = param_find("INAV_W_XY_VIS_P"); + h->w_xy_vision_v = param_find("INAV_W_XY_VIS_V"); h->w_xy_flow = param_find("INAV_W_XY_FLOW"); h->w_xy_res_v = param_find("INAV_W_XY_RES_V"); h->w_gps_flow = param_find("INAV_W_GPS_FLOW"); @@ -250,6 +298,7 @@ int parameters_init(struct position_estimator_inav_param_handles *h) h->land_t = param_find("INAV_LAND_T"); h->land_disp = param_find("INAV_LAND_DISP"); h->land_thr = param_find("INAV_LAND_THR"); + h->no_vision = param_find("CBRK_NO_VISION"); h->delay_gps = param_find("INAV_DELAY_GPS"); return OK; @@ -259,9 +308,12 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str { 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_vision_p, &(p->w_z_vision_p)); param_get(h->w_z_sonar, &(p->w_z_sonar)); param_get(h->w_xy_gps_p, &(p->w_xy_gps_p)); param_get(h->w_xy_gps_v, &(p->w_xy_gps_v)); + param_get(h->w_xy_vision_p, &(p->w_xy_vision_p)); + param_get(h->w_xy_vision_v, &(p->w_xy_vision_v)); param_get(h->w_xy_flow, &(p->w_xy_flow)); param_get(h->w_xy_res_v, &(p->w_xy_res_v)); param_get(h->w_gps_flow, &(p->w_gps_flow)); @@ -273,6 +325,7 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str param_get(h->land_t, &(p->land_t)); param_get(h->land_disp, &(p->land_disp)); param_get(h->land_thr, &(p->land_thr)); + param_get(h->no_vision, &(p->no_vision)); param_get(h->delay_gps, &(p->delay_gps)); return OK; 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 423f0d879..d0a65e42e 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 Anton Babushkin. All rights reserved. - * Author: Anton Babushkin <rk3dov@gmail.com> + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -33,9 +32,11 @@ ****************************************************************************/ /* - * @file position_estimator_inav_params.h + * @file position_estimator_inav_params.c * - * Parameters for Position Estimator + * @author Anton Babushkin <rk3dov@gmail.com> + * + * Parameters definition for position_estimator_inav */ #include <systemlib/param/param.h> @@ -43,9 +44,12 @@ struct position_estimator_inav_params { float w_z_baro; float w_z_gps_p; + float w_z_vision_p; float w_z_sonar; float w_xy_gps_p; float w_xy_gps_v; + float w_xy_vision_p; + float w_xy_vision_v; float w_xy_flow; float w_xy_res_v; float w_gps_flow; @@ -57,15 +61,19 @@ struct position_estimator_inav_params { float land_t; float land_disp; float land_thr; + int32_t no_vision; float delay_gps; }; struct position_estimator_inav_param_handles { param_t w_z_baro; param_t w_z_gps_p; + param_t w_z_vision_p; param_t w_z_sonar; param_t w_xy_gps_p; param_t w_xy_gps_v; + param_t w_xy_vision_p; + param_t w_xy_vision_v; param_t w_xy_flow; param_t w_xy_res_v; param_t w_gps_flow; @@ -77,9 +85,12 @@ struct position_estimator_inav_param_handles { param_t land_t; param_t land_disp; param_t land_thr; + param_t no_vision; param_t delay_gps; }; +#define CBRK_NO_VISION_KEY 328754 + /** * Initialize all parameter handles and values * |