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

InertialNav: Removed land detector from position estimator

This commit is contained in:
Johan Jansen 2015-01-06 12:31:05 +01:00
parent 28adc88500
commit 98ab83142c
2 changed files with 10 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -246,9 +246,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float baro_offset = 0.0f; // baro offset for reference altitude, initialized on start, then adjusted float baro_offset = 0.0f; // baro offset for reference altitude, initialized on start, then adjusted
float surface_offset = 0.0f; // ground level offset from reference altitude float surface_offset = 0.0f; // ground level offset from reference altitude
float surface_offset_rate = 0.0f; // surface offset change rate float surface_offset_rate = 0.0f; // surface offset change rate
float alt_avg = 0.0f; //float alt_avg = 0.0f;
bool landed = true; //bool landed = true;
hrt_abstime landed_time = 0; //hrt_abstime landed_time = 0;
hrt_abstime accel_timestamp = 0; hrt_abstime accel_timestamp = 0;
hrt_abstime baro_timestamp = 0; hrt_abstime baro_timestamp = 0;
@ -1068,12 +1068,14 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
inertial_filter_correct(-y_est[1], dt, y_est, 1, params.w_xy_res_v); inertial_filter_correct(-y_est[1], dt, y_est, 1, params.w_xy_res_v);
} }
/* detect land */
/*
// detect land
alt_avg += (- z_est[0] - alt_avg) * dt / params.land_t; alt_avg += (- z_est[0] - alt_avg) * dt / params.land_t;
float alt_disp2 = - z_est[0] - alt_avg; float alt_disp2 = - z_est[0] - alt_avg;
alt_disp2 = alt_disp2 * alt_disp2; alt_disp2 = alt_disp2 * alt_disp2;
float land_disp2 = params.land_disp * params.land_disp; float land_disp2 = params.land_disp * params.land_disp;
/* get actual thrust output */ // get actual thrust output
float thrust = armed.armed ? actuator.control[3] : 0.0f; float thrust = armed.armed ? actuator.control[3] : 0.0f;
if (landed) { if (landed) {
@ -1097,6 +1099,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
landed_time = 0; landed_time = 0;
} }
} }
*/
if (verbose_mode) { if (verbose_mode) {
/* print updates rate */ /* print updates rate */
@ -1148,7 +1151,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
local_pos.vy = y_est[1]; local_pos.vy = y_est[1];
local_pos.z = z_est[0]; local_pos.z = z_est[0];
local_pos.vz = z_est[1]; local_pos.vz = z_est[1];
local_pos.landed = landed; local_pos.landed = false;
local_pos.yaw = att.yaw; local_pos.yaw = att.yaw;
local_pos.dist_bottom_valid = dist_bottom_valid; local_pos.dist_bottom_valid = dist_bottom_valid;
local_pos.eph = eph; local_pos.eph = eph;

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

@ -77,7 +77,7 @@ struct vehicle_local_position_s {
double ref_lat; /**< Reference point latitude in degrees */ double ref_lat; /**< Reference point latitude in degrees */
double ref_lon; /**< Reference point longitude in degrees */ double ref_lon; /**< Reference point longitude in degrees */
float ref_alt; /**< Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level */ float ref_alt; /**< Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level */
bool landed; /**< true if vehicle is landed */ bool landed; /**< true if vehicle is landed TODO: deprecated, use vehicle_land_detected instead (remove this line?)*/
/* Distance to surface */ /* Distance to surface */
float dist_bottom; /**< Distance to bottom surface (ground) */ float dist_bottom; /**< Distance to bottom surface (ground) */
float dist_bottom_rate; /**< Distance to bottom surface (ground) change rate */ float dist_bottom_rate; /**< Distance to bottom surface (ground) change rate */