Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-07 09:13:32 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

fix code style

Browse Source
This commit is contained in:
Roman
2015-11-07 11:43:07 +01:00
parent 120fd9d522
commit e48cf53ce8
5 changed files with 42 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/geo/geo.c
+10 -5
View File
@@ -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)
{
src/lib/geo/geo.h
+4 -2
View File
@@ -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.
src/lib/runway_takeoff/RunwayTakeoff.cpp
+1 -1
View File
@@ -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) {
src/lib/terrain_estimation/terrain_estimator.cpp
+23 -17
View File
@@ -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<float, 3, 3> R_att(attitude->R);
@@ -76,25 +79,25 @@ void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitu
// dynamics matrix
matrix::Matrix<float, n_x, n_x> A;
A.setZero();
A(0,1) = 1;
A(1,2) = 1;
A(0, 1) = 1;
A(1, 2) = 1;
// input matrix
matrix::Matrix<float, n_x,1> B;
matrix::Matrix<float, n_x, 1> B;
B.setZero();
B(1,0) = 1;
B(1, 0) = 1;
// input noise variance
float R = 0.135f;
// process noise convariance
matrix::Matrix<float, n_x, n_x> Q;
Q(0,0) = 0;
Q(1,1) = 0;
Q(0, 0) = 0;
Q(1, 1) = 0;
// do prediction
matrix::Vector<float, n_x> 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<float, 1, 1> S_I = (C * _P * C.transpose());
S_I(0,0) += R;
S_I(0, 0) += R;
S_I = matrix::inv<float, 1> (S_I);
matrix::Vector<float, 1> 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<float, 1, n_x> 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<float, 1, 1> S_I = (C * _P * C.transpose());
S_I(0,0) += R;
S_I(0, 0) += R;
S_I = matrix::inv<float, 1>(S_I);
matrix::Vector<float, 1> 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;
}
}
src/lib/terrain_estimation/terrain_estimator.h
+4 -3
View File
@@ -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;