diff options
Diffstat (limited to 'src/modules/position_estimator_inav')
3 files changed, 71 insertions, 9 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..71abcc170 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 @@ -294,6 +296,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 +315,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 +328,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 */ @@ -616,6 +622,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; + z_est[0] = vision.z; + z_est[1] = vision.vz; + + vision_valid = true; + } + + /* calculate correction for position */ + corr_gps[0][0] = vision.x - x_est[0]; + corr_gps[1][0] = vision.y - y_est[0]; + corr_gps[2][0] = vision.z - z_est[0]; + + /* calculate correction for velocity */ + corr_gps[0][1] = vision.vx - x_est[1]; + corr_gps[1][1] = vision.vy - y_est[1]; + corr_gps[2][1] = vision.vz - z_est[1]; + + eph = 0.05f; + epv = 0.05f; + + w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, eph); + w_gps_z = min_eph_epv / fmaxf(min_eph_epv, epv); + + } + } + /* vehicle GPS position */ orb_check(vehicle_gps_position_sub, &updated); @@ -738,6 +784,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) 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) { corr_sonar = 0.0f; @@ -757,12 +810,12 @@ 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; + bool use_gps_xy = (ref_inited && gps_valid && params.w_xy_gps_p > MIN_VALID_W) || vision_valid; + bool use_gps_z = (ref_inited && gps_valid && params.w_z_gps_p > MIN_VALID_W) || vision_valid; /* 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 || vision_valid; bool dist_bottom_valid = (t < sonar_valid_time + sonar_valid_timeout); 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 0581f8236..0d9fbef27 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 */ @@ -56,6 +57,7 @@ PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f); PARAM_DEFINE_FLOAT(INAV_LAND_T, 3.0f); PARAM_DEFINE_FLOAT(INAV_LAND_DISP, 0.7f); PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.2f); +PARAM_DEFINE_INT32(CBRK_NO_VISION, 0); PARAM_DEFINE_FLOAT(INAV_DELAY_GPS, 0.2f); int parameters_init(struct position_estimator_inav_param_handles *h) @@ -76,6 +78,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; @@ -99,6 +102,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..08098d6c3 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> @@ -57,6 +58,7 @@ struct position_estimator_inav_params { float land_t; float land_disp; float land_thr; + int32_t no_vision; float delay_gps; }; @@ -77,9 +79,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 * |