fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-29 16:00:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

INAV: Added vision position estimate input / topic

Browse Source
This commit is contained in:
Lorenz Meier
2014-06-05 16:51:09 +02:00
parent 7a776eacb1
commit 57a7e76406
3 changed files with 68 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/position_estimator_inav/position_estimator_inav_main.c
+61 -1
View File
@@ -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);
src/modules/position_estimator_inav/position_estimator_inav_params.c
+3
View File
@@ -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;
}
src/modules/position_estimator_inav/position_estimator_inav_params.h
+4
View File
@@ -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
*