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

EKF: code style updates

This commit is contained in:
Paul Riseborough 2016-03-13 20:14:16 +11:00
parent c58ab3e256
commit d2407c3463
3 changed files with 9 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
EKF/control.cpp
View File

@ -125,6 +125,7 @@ void Ekf::controlFusionModes()
if (_control_status.flags.yaw_align) {
_control_status.flags.gps = true;
_time_last_gps = _time_last_imu;
// if we are not already aiding with optical flow, then we need to reset the position and velocity
if (!_control_status.flags.opt_flow) {
_control_status.flags.gps = resetPosition();
@ -248,9 +249,9 @@ void Ekf::controlFusionModes()
// Control the soure of height measurements for the main filter
if (_params.vdist_sensor_type == VDIST_SENSOR_BARO) {
_control_status.flags.baro_hgt = true;
_control_status.flags.rng_hgt = false;
_control_status.flags.gps_hgt = false;
_control_status.flags.baro_hgt = true;
_control_status.flags.rng_hgt = false;
_control_status.flags.gps_hgt = false;
} else if (_params.vdist_sensor_type == VDIST_SENSOR_RANGE) {
_control_status.flags.baro_hgt = false;

4
EKF/gps_checks.cpp
View File

@ -56,7 +56,7 @@
bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps)
{
// If we have defined the WGS-84 position of the NED origin, run gps quality checks until they pass, then define the origins WGS-84 position using the last GPS fix
if (!_NED_origin_initialised ) {
if (!_NED_origin_initialised) {
// we have good GPS data so can now set the origin's WGS-84 position
if (gps_is_good(gps) && !_NED_origin_initialised) {
printf("gps is good - setting EKF origin\n");
@ -64,12 +64,14 @@ bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps)
double lat = gps->lat / 1.0e7;
double lon = gps->lon / 1.0e7;
map_projection_init_timestamped(&_pos_ref, lat, lon, _time_last_imu);
// if we are already doing aiding, corect for the change in posiiton since the EKF started navigating
if (_control_status.flags.opt_flow || _control_status.flags.gps) {
double est_lat, est_lon;
map_projection_reproject(&_pos_ref, -_state.pos(0), -_state.pos(1), &est_lat, &est_lon);
map_projection_init_timestamped(&_pos_ref, est_lat, est_lon, _time_last_imu);
}
// Take the current GPS height and subtract the filter height above origin to estimate the GPS height of the origin
_gps_alt_ref = 1e-3f * (float)gps->alt + _state.pos(2);
_NED_origin_initialised = true;

3
EKF/terrain_estimator.cpp
View File

@ -78,7 +78,8 @@ void Ekf::predictHagl()
_terrain_var += sq(_imu_sample_delayed.delta_vel_dt * _params.terrain_p_noise);
// process noise due to terrain gradient
_terrain_var += sq(_imu_sample_delayed.delta_vel_dt * _params.terrain_gradient) * (sq(_state.vel(0)) + sq(_state.vel(1)));
_terrain_var += sq(_imu_sample_delayed.delta_vel_dt * _params.terrain_gradient) * (sq(_state.vel(0)) + sq(_state.vel(
1)));
// limit the variance to prevent it becoming badly conditioned
_terrain_var = math::constrain(_terrain_var, 0.0f, 1e4f);