diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-06-05 16:51:09 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-06-05 16:51:09 +0200 |
commit | 57a7e764068178e365b05b7baaa39041762f2e96 (patch) | |
tree | 00c8c50340f850deb89dccc430eb1891a170e4f0 | |
parent | 7a776eacb11f30a007812449a02916c6f00d2ad0 (diff) | |
download | px4-firmware-57a7e764068178e365b05b7baaa39041762f2e96.tar.gz px4-firmware-57a7e764068178e365b05b7baaa39041762f2e96.tar.bz2 px4-firmware-57a7e764068178e365b05b7baaa39041762f2e96.zip |
INAV: Added vision position estimate input / topic
3 files changed, 68 insertions, 1 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 d7503e42d..9ab21ee38 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -58,6 +58,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> @@ -78,6 +79,7 @@ static int position_estimator_inav_task; /**< Handle of deamon task / thread */ static bool verbose_mode = false; static const hrt_abstime gps_topic_timeout = 1000000; // GPS topic timeout = 1s +static const hrt_abstime vision_topic_timeout = 1000000; // Vision topic timeout = 1s static const hrt_abstime flow_topic_timeout = 1000000; // optical flow topic timeout = 1s static const hrt_abstime sonar_timeout = 150000; // sonar timeout = 150ms static const hrt_abstime sonar_valid_timeout = 1000000; // estimate sonar distance during this time after sonar loss @@ -270,6 +272,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; @@ -288,6 +291,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)); @@ -299,6 +304,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 */ @@ -587,6 +593,53 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } + + /* check no vision circuit breaker is set */ + if (params.no_vision != CBRK_NO_VISION) { + /* vehicle vision position */ + orb_check(vision_position_estimate_sub, &updated); + + + + // XXX THIS IS EVIL UNTESTED CODE + // set CBRK_NO_VISION to 0 to disengage the circuit breaker + // and activate this section + + + 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, gps.eph_m); + w_gps_z = min_eph_epv / fmaxf(min_eph_epv, gps.epv_m); + + } + } + /* vehicle GPS position */ orb_check(vehicle_gps_position_sub, &updated); @@ -706,6 +759,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; @@ -731,7 +791,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) xy_src_time = t; } - bool can_estimate_xy = eph < max_eph_epv * 1.5; + bool can_estimate_xy = (eph < max_eph_epv * 1.5) || 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 8aa08b6f2..c7c1c070c 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -55,6 +55,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, 328754); int parameters_init(struct position_estimator_inav_param_handles *h) { @@ -73,6 +74,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"); return OK; } @@ -94,6 +96,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)); 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 08ec996a1..b7826793a 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -56,6 +56,7 @@ struct position_estimator_inav_params { float land_t; float land_disp; float land_thr; + int32_t no_vision; }; struct position_estimator_inav_param_handles { @@ -74,8 +75,11 @@ struct position_estimator_inav_param_handles { param_t land_t; param_t land_disp; param_t land_thr; + param_t no_vision; }; +#define CBRK_NO_VISION 328754 + /** * Initialize all parameter handles and values * |