PX4 style fixes (excluding EKF generated code)

This commit is contained in:
Daniel Agar
2018-05-03 22:25:36 -04:00
parent e5952fadaf
commit c5b2f59f57
22 changed files with 384 additions and 187 deletions
+3
View File
@@ -62,6 +62,7 @@ bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps)
// Also run checks if the vehicle is on-ground as the check data can be used by vehicle pre-flight checks
if (!_control_status.flags.in_air || !_NED_origin_initialised || !_control_status.flags.gps) {
bool gps_checks_pass = gps_is_good(gps);
if (!_NED_origin_initialised && gps_checks_pass) {
// If we have good GPS data set the origin's WGS-84 position to the last gps fix
double lat = gps->lat / 1.0e7;
@@ -93,6 +94,7 @@ bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps)
_control_status.flags.rng_hgt = false;
// zero the sensor offset
_hgt_sensor_offset = 0.0f;
} else {
ECL_INFO("EKF GPS checks passed (WGS-84 origin set)");
}
@@ -139,6 +141,7 @@ bool Ekf::gps_is_good(struct gps_message *gps)
// calculate position movement since last GPS fix
if (_gps_pos_prev.timestamp > 0) {
map_projection_project(&_gps_pos_prev, lat, lon, &delta_posN, &delta_PosE);
} else {
// no previous position has been set
map_projection_init_timestamped(&_gps_pos_prev, lat, lon, _time_last_imu);