dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

commander: don't require good EPH for local_position_valid, position_estimator_inav: estimate EPH/EPV and publish it in local position topic

This commit is contained in:
Anton Babushkin 2014-05-16 22:12:07 +02:00
parent a72015c260
commit 692e8f84a9
5 changed files with 51 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
src/modules/commander/commander.cpp
View File

@ -1012,7 +1012,26 @@ int commander_thread_main(int argc, char *argv[])
}
/* update condition_local_position_valid and condition_local_altitude_valid */
check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid && eph_epv_good, &(status.condition_local_position_valid), &status_changed);
/* hysteresis for EPH */
bool local_eph_good;
if (status.condition_global_position_valid) {
if (local_position.eph > eph_epv_threshold * 2.0f) {
local_eph_good = false;
} else {
local_eph_good = true;
}
} else {
if (local_position.eph < eph_epv_threshold) {
local_eph_good = true;
} else {
local_eph_good = false;
}
}
check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid && local_eph_good, &(status.condition_local_position_valid), &status_changed);
check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed);
static bool published_condition_landed_fw = false;

21
src/modules/position_estimator_inav/position_estimator_inav_main.c
View File

@ -199,6 +199,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float y_est[3] = { 0.0f, 0.0f, 0.0f };
float z_est[3] = { 0.0f, 0.0f, 0.0f };
float eph = 1.0;
float epv = 1.0;
float x_est_prev[3], y_est_prev[3], z_est_prev[3];
memset(x_est_prev, 0, sizeof(x_est_prev));
memset(y_est_prev, 0, sizeof(y_est_prev));
@ -535,6 +538,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
flow_valid = true;
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
} else {
w_flow = 0.0f;
flow_valid = false;
@ -673,6 +678,9 @@ 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);
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);
}
@ -712,6 +720,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
dt = fmaxf(fminf(0.02, dt), 0.002); // constrain dt from 2 to 20 ms
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)
/* 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;
@ -723,7 +735,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
xy_src_time = t;
}
bool can_estimate_xy = (t < xy_src_time + xy_src_timeout);
bool can_estimate_xy = eph < max_eph_epv * 1.5;
bool dist_bottom_valid = (t < sonar_valid_time + sonar_valid_timeout);
@ -922,6 +934,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
local_pos.landed = landed;
local_pos.yaw = att.yaw;
local_pos.dist_bottom_valid = dist_bottom_valid;
local_pos.eph = eph;
local_pos.epv = epv;
if (local_pos.dist_bottom_valid) {
local_pos.dist_bottom = -z_est[0] - surface_offset;
@ -950,9 +964,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
global_pos.yaw = local_pos.yaw;
// TODO implement dead-reckoning
global_pos.eph = gps.eph_m;
global_pos.epv = gps.epv_m;
global_pos.eph = eph;
global_pos.epv = epv;
if (vehicle_global_position_pub < 0) {
vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);

10
src/modules/sdlog2/sdlog2.c
View File

@ -1119,10 +1119,16 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_LPOS.ref_lat = buf.local_pos.ref_lat * 1e7;
log_msg.body.log_LPOS.ref_lon = buf.local_pos.ref_lon * 1e7;
log_msg.body.log_LPOS.ref_alt = buf.local_pos.ref_alt;
log_msg.body.log_LPOS.xy_flags = (buf.local_pos.xy_valid ? 1 : 0) | (buf.local_pos.v_xy_valid ? 2 : 0) | (buf.local_pos.xy_global ? 8 : 0);
log_msg.body.log_LPOS.z_flags = (buf.local_pos.z_valid ? 1 : 0) | (buf.local_pos.v_z_valid ? 2 : 0) | (buf.local_pos.z_global ? 8 : 0);
log_msg.body.log_LPOS.pos_flags = (buf.local_pos.xy_valid ? 1 : 0) |
(buf.local_pos.z_valid ? 2 : 0) |
(buf.local_pos.v_xy_valid ? 4 : 0) |
(buf.local_pos.v_z_valid ? 8 : 0) |
(buf.local_pos.xy_global ? 16 : 0) |
(buf.local_pos.z_global ? 32 : 0);
log_msg.body.log_LPOS.landed = buf.local_pos.landed;
log_msg.body.log_LPOS.ground_dist_flags = (buf.local_pos.dist_bottom_valid ? 1 : 0);
log_msg.body.log_LPOS.eph = buf.local_pos.eph;
log_msg.body.log_LPOS.epv = buf.local_pos.epv;
LOGBUFFER_WRITE_AND_COUNT(LPOS);
}

7
src/modules/sdlog2/sdlog2_messages.h
View File

@ -109,10 +109,11 @@ struct log_LPOS_s {
int32_t ref_lat;
int32_t ref_lon;
float ref_alt;
uint8_t xy_flags;
uint8_t z_flags;
uint8_t pos_flags;
uint8_t landed;
uint8_t ground_dist_flags;
float eph;
float epv;
};
/* --- LPSP - LOCAL POSITION SETPOINT --- */
@ -360,7 +361,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"),
LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
LOG_FORMAT(SENS, "fffff", "BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"),
LOG_FORMAT(LPOS, "ffffffffLLfBBBB", "X,Y,Z,dist,distR,VX,VY,VZ,RLat,RLon,RAlt,XYFlg,ZFlg,LFlg,GFlg"),
LOG_FORMAT(LPOS, "ffffffffLLfBBBff", "X,Y,Z,Dist,DistR,VX,VY,VZ,RLat,RLon,RAlt,PFlg,LFlg,GFlg,EPH,EPV"),
LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"),
LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"),
LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),

2
src/modules/uORB/topics/vehicle_local_position.h
View File

@ -83,6 +83,8 @@ struct vehicle_local_position_s {
float dist_bottom_rate; /**< Distance to bottom surface (ground) change rate */
uint64_t surface_bottom_timestamp; /**< Time when new bottom surface found */
bool dist_bottom_valid; /**< true if distance to bottom surface is valid */
float eph;
float epv;
};
/**