fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-28 01:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merged master into vision_estimate

Browse Source
This commit is contained in:
Lorenz Meier
2014-07-01 14:00:54 +02:00
parent 5c6a541128 8f957aeb5f
commit 4b9f9281f5
170 changed files with 11332 additions and 4679 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/position_estimator_inav/position_estimator_inav_main.c
+142 -67
View File
@@ -49,6 +49,7 @@
#include <sys/prctl.h>
#include <termios.h>
#include <math.h>
#include <float.h>
#include <uORB/uORB.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/actuator_controls.h>
@@ -72,20 +73,21 @@
#include "inertial_filter.h"
#define MIN_VALID_W 0.00001f
#define PUB_INTERVAL 10000 // limit publish rate to 100 Hz
#define EST_BUF_SIZE 250000 / PUB_INTERVAL // buffer size is 0.5s
static bool thread_should_exit = false; /**< Deamon exit flag */
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 gps_topic_timeout = 1000000; // GPS topic timeout = 1s
static const hrt_abstime vision_topic_timeout = 1000000; // Vision topic timeout = 1s
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
static const hrt_abstime sonar_valid_timeout = 1000000; // estimate sonar distance during this time after sonar loss
static const hrt_abstime xy_src_timeout = 2000000; // estimate position during this time after position sources loss
static const uint32_t updates_counter_len = 1000000;
static const uint32_t pub_interval = 10000; // limit publish rate to 100 Hz
static const float max_flow = 1.0f; // max flow value that can be used, rad/s
__EXPORT int position_estimator_inav_main(int argc, char *argv[]);
@@ -94,6 +96,16 @@ int position_estimator_inav_thread_main(int argc, char *argv[]);
static void usage(const char *reason);
static inline int min(int val1, int val2)
{
return (val1 < val2) ? val1 : val2;
}
static inline int max(int val1, int val2)
{
return (val1 > val2) ? val1 : val2;
}
/**
* Print the correct usage.
*/
@@ -137,7 +149,7 @@ int position_estimator_inav_main(int argc, char *argv[])
thread_should_exit = false;
position_estimator_inav_task = task_spawn_cmd("position_estimator_inav",
SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 4000,
SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 5000,
position_estimator_inav_thread_main,
(argv) ? (const char **) &argv[2] : (const char **) NULL);
exit(0);
@@ -170,15 +182,21 @@ int position_estimator_inav_main(int argc, char *argv[])
exit(1);
}
void write_debug_log(const char *msg, float dt, float x_est[2], float y_est[2], float z_est[2], float x_est_prev[2], float y_est_prev[2], float z_est_prev[2], float acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v)
static void write_debug_log(const char *msg, float dt, float x_est[2], float y_est[2], float z_est[2], float x_est_prev[2], float y_est_prev[2], float z_est_prev[2], float acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v)
{
FILE *f = fopen("/fs/microsd/inav.log", "a");
if (f) {
char *s = malloc(256);
unsigned n = snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f] y_est=[%.5f %.5f] z_est=[%.5f %.5f] x_est_prev=[%.5f %.5f] y_est_prev=[%.5f %.5f] z_est_prev=[%.5f %.5f]\n", hrt_absolute_time(), msg, dt, x_est[0], x_est[1], y_est[0], y_est[1], z_est[0], z_est[1], x_est_prev[0], x_est_prev[1], y_est_prev[0], y_est_prev[1], z_est_prev[0], z_est_prev[1]);
unsigned n = snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f] y_est=[%.5f %.5f] z_est=[%.5f %.5f] x_est_prev=[%.5f %.5f] y_est_prev=[%.5f %.5f] z_est_prev=[%.5f %.5f]\n",
hrt_absolute_time(), msg, (double)dt,
(double)x_est[0], (double)x_est[1], (double)y_est[0], (double)y_est[1], (double)z_est[0], (double)z_est[1],
(double)x_est_prev[0], (double)x_est_prev[1], (double)y_est_prev[0], (double)y_est_prev[1], (double)z_est_prev[0], (double)z_est_prev[1]);
fwrite(s, 1, n, f);
n = snprintf(s, 256, "\tacc=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f\n", acc[0], acc[1], acc[2], corr_gps[0][0], corr_gps[1][0], corr_gps[2][0], corr_gps[0][1], corr_gps[1][1], corr_gps[2][1], w_xy_gps_p, w_xy_gps_v);
n = snprintf(s, 256, "\tacc=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f\n",
(double)acc[0], (double)acc[1], (double)acc[2],
(double)corr_gps[0][0], (double)corr_gps[1][0], (double)corr_gps[2][0], (double)corr_gps[0][1], (double)corr_gps[1][1], (double)corr_gps[2][1],
(double)w_xy_gps_p, (double)w_xy_gps_v);
fwrite(s, 1, n, f);
free(s);
}
@@ -201,8 +219,21 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float y_est[2] = { 0.0f, 0.0f }; // pos, vel
float z_est[2] = { 0.0f, 0.0f }; // pos, vel
float eph = 1.0;
float epv = 1.0;
float est_buf[EST_BUF_SIZE][3][2]; // estimated position buffer
float R_buf[EST_BUF_SIZE][3][3]; // rotation matrix buffer
float R_gps[3][3]; // rotation matrix for GPS correction moment
memset(est_buf, 0, sizeof(est_buf));
memset(R_buf, 0, sizeof(R_buf));
memset(R_gps, 0, sizeof(R_gps));
int buf_ptr = 0;
static const float min_eph_epv = 2.0f; // min EPH/EPV, used for weight calculation
static const float max_eph_epv = 20.0f; // max EPH/EPV acceptable for estimation
float eph = max_eph_epv;
float epv = 1.0f;
float eph_flow = 1.0f;
float x_est_prev[2], y_est_prev[2], z_est_prev[2];
memset(x_est_prev, 0, sizeof(x_est_prev));
@@ -239,9 +270,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
hrt_abstime t_prev = 0;
/* acceleration in NED frame */
float accel_NED[3] = { 0.0f, 0.0f, -CONSTANTS_ONE_G };
/* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */
float acc[] = { 0.0f, 0.0f, 0.0f }; // N E D
float acc_bias[] = { 0.0f, 0.0f, 0.0f }; // body frame
@@ -259,14 +287,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float corr_flow[] = { 0.0f, 0.0f }; // N E
float w_flow = 0.0f;
static float min_eph_epv = 2.0f; // min EPH/EPV, used for weight calculation
static float max_eph_epv = 10.0f; // max EPH/EPV acceptable for estimation
float sonar_prev = 0.0f;
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)
hrt_abstime xy_src_time = 0; // time of last available position data
bool gps_valid = false; // GPS is valid
bool sonar_valid = false; // sonar is valid
@@ -355,8 +379,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", baro_offset);
mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", baro_offset);
warnx("baro offs: %.2f", (double)baro_offset);
mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", (double)baro_offset);
local_pos.z_valid = true;
local_pos.v_z_valid = true;
}
@@ -460,7 +484,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
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 && att.R[2][2] > 0.7 && flow.ground_distance_m != sonar_prev) {
if ((flow.ground_distance_m > 0.31f) &&
(flow.ground_distance_m < 4.0f) &&
(att.R[2][2] > 0.7f) &&
(fabsf(flow.ground_distance_m - sonar_prev) > FLT_EPSILON)) {
sonar_time = t;
sonar_prev = flow.ground_distance_m;
corr_sonar = flow.ground_distance_m + surface_offset + z_est[0];
@@ -482,7 +510,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", surface_offset);
mavlink_log_info(mavlink_fd, "[inav] new surface level: %.2f", (double)surface_offset);
}
} else {
@@ -495,7 +523,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float flow_q = flow.quality / 255.0f;
float dist_bottom = - z_est[0] - surface_offset;
if (dist_bottom > 0.3f && flow_q > params.flow_q_min && (t < sonar_valid_time + sonar_valid_timeout) && att.R[2][2] > 0.7) {
if (dist_bottom > 0.3f && flow_q > params.flow_q_min && (t < sonar_valid_time + sonar_valid_timeout) && att.R[2][2] > 0.7f) {
/* distance to surface */
float flow_dist = dist_bottom / att.R[2][2];
/* check if flow if too large for accurate measurements */
@@ -542,9 +570,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
w_flow *= 0.05f;
}
flow_valid = true;
/* under ideal conditions, on 1m distance assume EPH = 10cm */
eph_flow = 0.1f / w_flow;
eph = fminf(eph, 0.1 / att.R[2][2] / flow_q * fmaxf(1.0f, flow_dist)); // under ideal conditions, on 1m distance assume EPH = 10cm
flow_valid = true;
} else {
w_flow = 0.0f;
@@ -595,17 +624,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* check no vision circuit breaker is set */
if (params.no_vision != CBRK_NO_VISION) {
if (params.no_vision != CBRK_NO_VISION_KEY) {
/* 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);
@@ -634,8 +656,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
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);
w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, eph);
w_gps_z = min_eph_epv / fmaxf(min_eph_epv, epv);
}
}
@@ -650,13 +672,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* hysteresis for GPS quality */
if (gps_valid) {
if (gps.eph_m > max_eph_epv * 1.5f || gps.epv_m > max_eph_epv * 1.5f || gps.fix_type < 3) {
if (gps.eph > max_eph_epv || gps.epv > max_eph_epv || gps.fix_type < 3) {
gps_valid = false;
mavlink_log_info(mavlink_fd, "[inav] GPS signal lost");
}
} else {
if (gps.eph_m < max_eph_epv && gps.epv_m < max_eph_epv && gps.fix_type >= 3) {
if (gps.eph < max_eph_epv * 0.7f && gps.epv < max_eph_epv * 0.7f && gps.fix_type >= 3) {
gps_valid = true;
reset_est = true;
mavlink_log_info(mavlink_fd, "[inav] GPS signal found");
@@ -675,25 +697,22 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
} else if (t > ref_init_start + ref_init_delay) {
ref_inited = true;
/* update baro offset */
baro_offset -= z_est[0];
/* 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;
y_est[0] = 0.0f;
y_est[1] = gps.vel_e_m_s;
z_est[0] = 0.0f;
local_pos.ref_lat = lat;
local_pos.ref_lon = lon;
local_pos.ref_alt = alt;
local_pos.ref_alt = alt + z_est[0];
local_pos.ref_timestamp = t;
/* initialize projection */
map_projection_init(&ref, lat, lon);
warnx("init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt);
mavlink_log_info(mavlink_fd, "[inav] init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt);
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);
}
}
@@ -710,16 +729,22 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
y_est[1] = gps.vel_e_m_s;
}
/* calculate index of estimated values in buffer */
int est_i = buf_ptr - 1 - min(EST_BUF_SIZE - 1, max(0, (int)(params.delay_gps * 1000000.0f / PUB_INTERVAL)));
if (est_i < 0) {
est_i += EST_BUF_SIZE;
}
/* calculate correction for position */
corr_gps[0][0] = gps_proj[0] - x_est[0];
corr_gps[1][0] = gps_proj[1] - y_est[0];
corr_gps[2][0] = local_pos.ref_alt - alt - z_est[0];
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];
corr_gps[2][0] = local_pos.ref_alt - alt - est_buf[est_i][2][0];
/* calculate correction for velocity */
if (gps.vel_ned_valid) {
corr_gps[0][1] = gps.vel_n_m_s - x_est[1];
corr_gps[1][1] = gps.vel_e_m_s - y_est[1];
corr_gps[2][1] = gps.vel_d_m_s - z_est[1];
corr_gps[0][1] = gps.vel_n_m_s - est_buf[est_i][0][1];
corr_gps[1][1] = gps.vel_e_m_s - est_buf[est_i][1][1];
corr_gps[2][1] = gps.vel_d_m_s - est_buf[est_i][2][1];
} else {
corr_gps[0][1] = 0.0f;
@@ -727,11 +752,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
corr_gps[2][1] = 0.0f;
}
eph = fminf(eph, gps.eph_m);
epv = fminf(epv, gps.epv_m);
/* save rotation matrix at this moment */
memcpy(R_gps, R_buf[est_i], sizeof(R_gps));
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);
w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, gps.eph);
w_gps_z = min_eph_epv / fmaxf(min_eph_epv, gps.epv);
}
} else {
@@ -777,8 +802,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
t_prev = t;
/* increase EPH/EPV on each step */
eph *= 1.0 + dt;
epv += 0.005 * dt; // add 1m to EPV each 200s (baro drift)
if (eph < max_eph_epv) {
eph *= 1.0f + dt;
}
if (epv < max_eph_epv) {
epv += 0.005f * dt; // add 1m to EPV each 200s (baro drift)
}
/* 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) || vision_valid;
@@ -786,12 +815,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* use flow if it's valid and (accurate or no GPS available) */
bool use_flow = flow_valid && (flow_accurate || !use_gps_xy);
/* try to estimate position during some time after position sources lost */
if (use_gps_xy || use_flow) {
xy_src_time = t;
}
bool can_estimate_xy = (eph < max_eph_epv * 1.5) || vision_valid;
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);
@@ -823,7 +847,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
corr_baro += offs_corr;
}
/* accelerometer bias correction */
/* accelerometer bias correction for GPS (use buffered rotation matrix) */
float accel_bias_corr[3] = { 0.0f, 0.0f, 0.0f };
if (use_gps_xy) {
@@ -837,6 +861,24 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
accel_bias_corr[2] -= corr_gps[2][0] * w_z_gps_p * w_z_gps_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 += R_gps[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;
accel_bias_corr[2] = 0.0f;
if (use_flow) {
accel_bias_corr[0] -= corr_flow[0] * params.w_xy_flow;
accel_bias_corr[1] -= corr_flow[1] * params.w_xy_flow;
@@ -867,7 +909,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* inertial filter correction for altitude */
inertial_filter_correct(corr_baro, dt, z_est, 0, params.w_z_baro);
inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p);
if (use_gps_z) {
epv = fminf(epv, gps.epv);
inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_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);
@@ -892,11 +939,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* inertial filter correction for position */
if (use_flow) {
eph = fminf(eph, eph_flow);
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);
}
if (use_gps_xy) {
eph = fminf(eph, gps.eph);
inertial_filter_correct(corr_gps[0][0], dt, x_est, 0, w_xy_gps_p);
inertial_filter_correct(corr_gps[1][0], dt, y_est, 0, w_xy_gps_p);
@@ -917,6 +968,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
memcpy(x_est_prev, x_est, sizeof(x_est));
memcpy(y_est_prev, y_est, sizeof(y_est));
}
} else {
/* gradually reset xy velocity estimates */
inertial_filter_correct(-x_est[1], dt, x_est, 1, params.w_xy_res_v);
inertial_filter_correct(-y_est[1], dt, y_est, 1, params.w_xy_res_v);
}
/* detect land */
@@ -932,6 +987,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
landed = false;
landed_time = 0;
}
/* reset xy velocity estimates when landed */
x_est[1] = 0.0f;
y_est[1] = 0.0f;
} else {
if (alt_disp2 < land_disp2 && thrust < params.land_thr) {
@@ -956,11 +1014,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float updates_dt = (t - updates_counter_start) * 0.000001f;
warnx(
"updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s, flow = %.1f/s",
accel_updates / updates_dt,
baro_updates / updates_dt,
gps_updates / updates_dt,
attitude_updates / updates_dt,
flow_updates / updates_dt);
(double)(accel_updates / updates_dt),
(double)(baro_updates / updates_dt),
(double)(gps_updates / updates_dt),
(double)(attitude_updates / updates_dt),
(double)(flow_updates / updates_dt));
updates_counter_start = t;
accel_updates = 0;
baro_updates = 0;
@@ -970,8 +1028,25 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
}
}
if (t > pub_last + pub_interval) {
if (t > pub_last + PUB_INTERVAL) {
pub_last = t;
/* push current estimate to buffer */
est_buf[buf_ptr][0][0] = x_est[0];
est_buf[buf_ptr][0][1] = x_est[1];
est_buf[buf_ptr][1][0] = y_est[0];
est_buf[buf_ptr][1][1] = y_est[1];
est_buf[buf_ptr][2][0] = z_est[0];
est_buf[buf_ptr][2][1] = z_est[1];
/* push current rotation matrix to buffer */
memcpy(R_buf[buf_ptr], att.R, sizeof(att.R));
buf_ptr++;
if (buf_ptr >= EST_BUF_SIZE) {
buf_ptr = 0;
}
/* publish local position */
local_pos.xy_valid = can_estimate_xy;
local_pos.v_xy_valid = can_estimate_xy;
src/modules/position_estimator_inav/position_estimator_inav_params.c
+10 -3
View File
@@ -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
*/
@@ -46,6 +47,7 @@ PARAM_DEFINE_FLOAT(INAV_W_Z_SONAR, 3.0f);
PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_P, 1.0f);
PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f);
PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f);
PARAM_DEFINE_FLOAT(INAV_W_XY_RES_V, 0.5f);
PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f);
PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.05f);
PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.15f);
@@ -55,7 +57,8 @@ 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);
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)
{
@@ -65,6 +68,7 @@ int parameters_init(struct position_estimator_inav_param_handles *h)
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_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");
h->w_acc_bias = param_find("INAV_W_ACC_BIAS");
h->flow_k = param_find("INAV_FLOW_K");
@@ -75,6 +79,7 @@ int parameters_init(struct position_estimator_inav_param_handles *h)
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;
}
@@ -87,6 +92,7 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str
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_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));
param_get(h->w_acc_bias, &(p->w_acc_bias));
param_get(h->flow_k, &(p->flow_k));
@@ -97,6 +103,7 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str
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;
}
src/modules/position_estimator_inav/position_estimator_inav_params.h
+10 -5
View File
@@ -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>
@@ -47,6 +48,7 @@ struct position_estimator_inav_params {
float w_xy_gps_p;
float w_xy_gps_v;
float w_xy_flow;
float w_xy_res_v;
float w_gps_flow;
float w_acc_bias;
float flow_k;
@@ -57,6 +59,7 @@ struct position_estimator_inav_params {
float land_disp;
float land_thr;
int32_t no_vision;
float delay_gps;
};
struct position_estimator_inav_param_handles {
@@ -66,6 +69,7 @@ struct position_estimator_inav_param_handles {
param_t w_xy_gps_p;
param_t w_xy_gps_v;
param_t w_xy_flow;
param_t w_xy_res_v;
param_t w_gps_flow;
param_t w_acc_bias;
param_t flow_k;
@@ -76,9 +80,10 @@ struct position_estimator_inav_param_handles {
param_t land_disp;
param_t land_thr;
param_t no_vision;
param_t delay_gps;
};
#define CBRK_NO_VISION 328754
#define CBRK_NO_VISION_KEY 328754
/**
* Initialize all parameter handles and values