diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 0d09e8526b..023337ff04 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -298,16 +298,19 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou return CONSTANTS_RADIUS_OF_EARTH * c; } -__EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B, double lon_B, float dist, double *lat_target, double *lon_target) +__EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B, double lon_B, float dist, + double *lat_target, double *lon_target) { float heading; + if (fabsf(dist) < FLT_EPSILON) { *lat_target = lat_A; *lon_target = lon_A; - } - else if (dist >= FLT_EPSILON) { + + } else if (dist >= FLT_EPSILON) { heading = get_bearing_to_next_waypoint(lat_A, lon_A, lat_B, lon_B); waypoint_from_heading_and_distance(lat_A, lon_A, heading, dist, lat_target, lon_target); + } else { heading = get_bearing_to_next_waypoint(lat_A, lon_A, lat_B, lon_B); heading = _wrap_2pi(heading + M_PI_F); @@ -315,13 +318,15 @@ __EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, dou } } -__EXPORT void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist, double *lat_end, double *lon_end) +__EXPORT void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist, + double *lat_end, double *lon_end) { bearing = _wrap_2pi(bearing); double radius_ratio = (double)(fabs(dist) / CONSTANTS_RADIUS_OF_EARTH); *lat_end = asin(sin(lat_start) * cos(radius_ratio) + cos(lat_start) * sin(radius_ratio) * cos((double)bearing)); - *lon_end = lon_start + atan2(sin((double)bearing) * sin(radius_ratio) * cos(lat_start), cos(radius_ratio) - sin(lat_start) * sin(*lat_end)); + *lon_end = lon_start + atan2(sin((double)bearing) * sin(radius_ratio) * cos(lat_start), + cos(radius_ratio) - sin(lat_start) * sin(*lat_end)); } __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) { diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index f9250fb5a3..8dc0836e34 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -238,9 +238,11 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou // TODO put description for both functions and improve naming -__EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B, double lon_B, float dist, double *lat_target, double *lon_target); +__EXPORT void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B, double lon_B, float dist, + double *lat_target, double *lon_target); -__EXPORT void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist, double *end_lat, double *end_lon); +__EXPORT void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist, + double *end_lat, double *end_lon); /** * Returns the bearing to the next waypoint in radians. diff --git a/src/lib/runway_takeoff/RunwayTakeoff.cpp b/src/lib/runway_takeoff/RunwayTakeoff.cpp index 46df5cd744..7089889858 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.cpp +++ b/src/lib/runway_takeoff/RunwayTakeoff.cpp @@ -92,7 +92,7 @@ void RunwayTakeoff::init(float yaw, double current_lat, double current_lon) } void RunwayTakeoff::update(float airspeed, float alt_agl, - double current_lat, double current_lon, int mavlink_fd) + double current_lat, double current_lon, int mavlink_fd) { switch (_state) { diff --git a/src/lib/terrain_estimation/terrain_estimator.cpp b/src/lib/terrain_estimation/terrain_estimator.cpp index a92bbb136e..64a747f532 100644 --- a/src/lib/terrain_estimation/terrain_estimator.cpp +++ b/src/lib/terrain_estimation/terrain_estimator.cpp @@ -51,16 +51,19 @@ TerrainEstimator::TerrainEstimator() : _P.setIdentity(); } -bool TerrainEstimator::is_distance_valid(float distance) { +bool TerrainEstimator::is_distance_valid(float distance) +{ if (distance > 40.0f || distance < 0.00001f) { return false; + } else { return true; } } -void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitude, const struct sensor_combined_s *sensor, - const struct distance_sensor_s *distance) +void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitude, + const struct sensor_combined_s *sensor, + const struct distance_sensor_s *distance) { if (attitude->R_valid) { matrix::Matrix R_att(attitude->R); @@ -76,25 +79,25 @@ void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitu // dynamics matrix matrix::Matrix A; A.setZero(); - A(0,1) = 1; - A(1,2) = 1; + A(0, 1) = 1; + A(1, 2) = 1; // input matrix - matrix::Matrix B; + matrix::Matrix B; B.setZero(); - B(1,0) = 1; + B(1, 0) = 1; // input noise variance float R = 0.135f; // process noise convariance matrix::Matrix Q; - Q(0,0) = 0; - Q(1,1) = 0; + Q(0, 0) = 0; + Q(1, 1) = 0; // do prediction matrix::Vector dx = (A * _x) * dt; - dx(1) += B(1,0) * _u_z * dt; + dx(1) += B(1, 0) * _u_z * dt; // propagate state and covariance matrix _x += dx; @@ -102,8 +105,9 @@ void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitu B * R * B.transpose() + Q) * dt; } -void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicle_gps_position_s *gps, const struct distance_sensor_s *distance, - const struct vehicle_attitude_s *attitude) +void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicle_gps_position_s *gps, + const struct distance_sensor_s *distance, + const struct vehicle_attitude_s *attitude) { // terrain estimate is invalid if we have range sensor timeout if (time_ref - distance->timestamp > DISTANCE_TIMEOUT) { @@ -124,7 +128,7 @@ void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicl // residual matrix::Matrix S_I = (C * _P * C.transpose()); - S_I(0,0) += R; + S_I(0, 0) += R; S_I = matrix::inv (S_I); matrix::Vector r = y - C * _x; @@ -140,6 +144,7 @@ void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicl // estimate to be invalid if (!is_distance_valid(distance->current_distance) && !is_distance_valid(_distance_last)) { _terrain_valid = false; + } else { _terrain_valid = true; } @@ -150,7 +155,7 @@ void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicl if (gps->timestamp_position > _time_last_gps && gps->fix_type >= 3) { matrix::Matrix C; - C(0,1) = 1; + C(0, 1) = 1; float R = 0.056f; @@ -159,7 +164,7 @@ void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicl // residual matrix::Matrix S_I = (C * _P * C.transpose()); - S_I(0,0) += R; + S_I(0, 0) += R; S_I = matrix::inv(S_I); matrix::Vector r = y - C * _x; @@ -172,6 +177,7 @@ void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicl // reinitialise filter if we find bad data bool reinit = false; + for (int i = 0; i < n_x; i++) { if (!PX4_ISFINITE(_x(i))) { reinit = true; @@ -180,7 +186,7 @@ void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicl for (int i = 0; i < n_x; i++) { for (int j = 0; j < n_x; j++) { - if (!PX4_ISFINITE(_P(i,j))) { + if (!PX4_ISFINITE(_P(i, j))) { reinit = true; } } @@ -189,7 +195,7 @@ void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicl if (reinit) { memset(&_x._data[0], 0, sizeof(_x._data)); _P.setZero(); - _P(0,0) = _P(1,1) = _P(2,2) = 0.1f; + _P(0, 0) = _P(1, 1) = _P(2, 2) = 0.1f; } } diff --git a/src/lib/terrain_estimation/terrain_estimator.h b/src/lib/terrain_estimation/terrain_estimator.h index 697a053b88..25723f21d8 100644 --- a/src/lib/terrain_estimation/terrain_estimator.h +++ b/src/lib/terrain_estimation/terrain_estimator.h @@ -65,12 +65,13 @@ public: float get_velocity() {return _x(1);}; void predict(float dt, const struct vehicle_attitude_s *attitude, const struct sensor_combined_s *sensor, - const struct distance_sensor_s *distance); - void measurement_update(uint64_t time_ref, const struct vehicle_gps_position_s *gps, const struct distance_sensor_s *distance, + const struct distance_sensor_s *distance); + void measurement_update(uint64_t time_ref, const struct vehicle_gps_position_s *gps, + const struct distance_sensor_s *distance, const struct vehicle_attitude_s *attitude); private: - enum {n_x=3}; + enum {n_x = 3}; float _distance_last; bool _terrain_valid;