aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--src/drivers/frsky_telemetry/frsky_data.c2
-rw-r--r--src/lib/geo/geo.c198
-rw-r--r--src/lib/geo/geo.h38
-rw-r--r--src/modules/att_pos_estimator_ekf/KalmanNav.cpp15
-rw-r--r--src/modules/att_pos_estimator_ekf/KalmanNav.hpp2
-rwxr-xr-xsrc/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp2
-rw-r--r--src/modules/commander/commander.cpp148
-rw-r--r--src/modules/mavlink/mavlink_receiver.cpp15
-rw-r--r--src/modules/mavlink/mavlink_receiver.h1
-rw-r--r--src/modules/mc_pos_control/mc_pos_control_main.cpp260
-rw-r--r--src/modules/navigator/navigator_main.cpp17
-rw-r--r--src/modules/position_estimator_inav/position_estimator_inav_main.c161
-rw-r--r--src/modules/position_estimator_inav/position_estimator_inav_params.c2
-rw-r--r--src/modules/sdlog2/sdlog2.c8
-rw-r--r--src/modules/sdlog2/sdlog2_messages.h6
-rw-r--r--src/modules/uORB/topics/optical_flow.h1
-rw-r--r--src/modules/uORB/topics/vehicle_global_position.h7
-rw-r--r--src/modules/uORB/topics/vehicle_local_position.h8
18 files changed, 487 insertions, 404 deletions
diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c
index cfcf91e3f..57a03bc84 100644
--- a/src/drivers/frsky_telemetry/frsky_data.c
+++ b/src/drivers/frsky_telemetry/frsky_data.c
@@ -225,7 +225,7 @@ void frsky_send_frame2(int uart)
float course = 0, lat = 0, lon = 0, speed = 0, alt = 0;
char lat_ns = 0, lon_ew = 0;
int sec = 0;
- if (global_pos.global_valid) {
+ if (global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) {
time_t time_gps = global_pos.time_gps_usec / 1000000;
struct tm *tm_gps = gmtime(&time_gps);
diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c
index f72dc607c..9a24ff50e 100644
--- a/src/lib/geo/geo.c
+++ b/src/lib/geo/geo.c
@@ -1,9 +1,6 @@
/****************************************************************************
*
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
- * Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- * Julian Oes <joes@student.ethz.ch>
- * Lorenz Meier <lm@inf.ethz.ch>
+ * Copyright (C) 2012, 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -42,6 +39,7 @@
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <geo/geo.h>
@@ -52,124 +50,58 @@
#include <math.h>
#include <stdbool.h>
+/*
+ * Azimuthal Equidistant Projection
+ * formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html
+ */
-/* values for map projection */
-static double phi_1;
-static double sin_phi_1;
-static double cos_phi_1;
-static double lambda_0;
-static double scale;
-
-__EXPORT void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
+__EXPORT void map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
{
- /* notation and formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */
- phi_1 = lat_0 / 180.0 * M_PI;
- lambda_0 = lon_0 / 180.0 * M_PI;
-
- sin_phi_1 = sin(phi_1);
- cos_phi_1 = cos(phi_1);
-
- /* calculate local scale by using the relation of true distance and the distance on plane */ //TODO: this is a quick solution, there are probably easier ways to determine the scale
-
- /* 1) calculate true distance d on sphere to a point: http://www.movable-type.co.uk/scripts/latlong.html */
-
- double lat1 = phi_1;
- double lon1 = lambda_0;
-
- double lat2 = phi_1 + 0.5 / 180 * M_PI;
- double lon2 = lambda_0 + 0.5 / 180 * M_PI;
- double sin_lat_2 = sin(lat2);
- double cos_lat_2 = cos(lat2);
- double d = acos(sin(lat1) * sin_lat_2 + cos(lat1) * cos_lat_2 * cos(lon2 - lon1)) * CONSTANTS_RADIUS_OF_EARTH;
-
- /* 2) calculate distance rho on plane */
- double k_bar = 0;
- double c = acos(sin_phi_1 * sin_lat_2 + cos_phi_1 * cos_lat_2 * cos(lon2 - lambda_0));
-
- if (0 != c)
- k_bar = c / sin(c);
-
- double x2 = k_bar * (cos_lat_2 * sin(lon2 - lambda_0)); //Projection of point 2 on plane
- double y2 = k_bar * ((cos_phi_1 * sin_lat_2 - sin_phi_1 * cos_lat_2 * cos(lon2 - lambda_0)));
- double rho = sqrt(pow(x2, 2) + pow(y2, 2));
-
- scale = d / rho;
+ ref->lat = lat_0 / 180.0 * M_PI;
+ ref->lon = lon_0 / 180.0 * M_PI;
+ ref->sin_lat = sin(ref->lat);
+ ref->cos_lat = cos(ref->lat);
}
-__EXPORT void map_projection_project(double lat, double lon, float *x, float *y)
+__EXPORT void map_projection_project(struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y)
{
- /* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */
- double phi = lat / 180.0 * M_PI;
- double lambda = lon / 180.0 * M_PI;
-
- double sin_phi = sin(phi);
- double cos_phi = cos(phi);
-
- double k_bar = 0;
- /* using small angle approximation (formula in comment is without aproximation) */
- double c = acos(sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * (1 - pow((lambda - lambda_0), 2) / 2)); //double c = acos( sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * cos(lambda - lambda_0) );
+ double lat_rad = lat / 180.0 * M_PI;
+ double lon_rad = lon / 180.0 * M_PI;
- if (0 != c)
- k_bar = c / sin(c);
+ double sin_lat = sin(lat_rad);
+ double cos_lat = cos(lat_rad);
+ double cos_d_lon = cos(lon_rad - ref->lon);
- /* using small angle approximation (formula in comment is without aproximation) */
- *y = k_bar * (cos_phi * (lambda - lambda_0)) * scale;//*y = k_bar * (cos_phi * sin(lambda - lambda_0)) * scale;
- *x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * (1 - pow((lambda - lambda_0), 2) / 2))) * scale; // *x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * cos(lambda - lambda_0))) * scale;
+ double c = acos(ref->sin_lat * sin_lat + ref->cos_lat * cos_lat * cos_d_lon);
+ double k = (c == 0.0) ? 1.0 : (c / sin(c));
-// printf("%phi_1=%.10f, lambda_0 =%.10f\n", phi_1, lambda_0);
+ *x = k * (ref->cos_lat * sin_lat - ref->sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH;
+ *y = k * cos_lat * sin(lon_rad - ref->lon) * CONSTANTS_RADIUS_OF_EARTH;
}
-__EXPORT void map_projection_reproject(float x, float y, double *lat, double *lon)
+__EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon)
{
- /* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */
-
- double x_descaled = x / scale;
- double y_descaled = y / scale;
-
- double c = sqrt(pow(x_descaled, 2) + pow(y_descaled, 2));
+ float x_rad = x / CONSTANTS_RADIUS_OF_EARTH;
+ float y_rad = y / CONSTANTS_RADIUS_OF_EARTH;
+ double c = sqrtf(x_rad * x_rad + y_rad * y_rad);
double sin_c = sin(c);
double cos_c = cos(c);
- double lat_sphere = 0;
-
- if (c != 0)
- lat_sphere = asin(cos_c * sin_phi_1 + (x_descaled * sin_c * cos_phi_1) / c);
- else
- lat_sphere = asin(cos_c * sin_phi_1);
-
-// printf("lat_sphere = %.10f\n",lat_sphere);
-
- double lon_sphere = 0;
-
- if (phi_1 == M_PI / 2) {
- //using small angle approximation (formula in comment is without aproximation)
- lon_sphere = (lambda_0 - y_descaled / x_descaled); //lon_sphere = (lambda_0 + atan2(-y_descaled, x_descaled));
+ double lat_rad;
+ double lon_rad;
- } else if (phi_1 == -M_PI / 2) {
- //using small angle approximation (formula in comment is without aproximation)
- lon_sphere = (lambda_0 + y_descaled / x_descaled); //lon_sphere = (lambda_0 + atan2(y_descaled, x_descaled));
+ if (c != 0.0) {
+ lat_rad = asin(cos_c * ref->sin_lat + (x_rad * sin_c * ref->cos_lat) / c);
+ lon_rad = (ref->lon + atan2(y_rad * sin_c, c * ref->cos_lat * cos_c - x_rad * ref->sin_lat * sin_c));
} else {
-
- lon_sphere = (lambda_0 + atan2(y_descaled * sin_c , c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c));
- //using small angle approximation
-// double denominator = (c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c);
-// if(denominator != 0)
-// {
-// lon_sphere = (lambda_0 + (y_descaled * sin_c) / denominator);
-// }
-// else
-// {
-// ...
-// }
+ lat_rad = ref->lat;
+ lon_rad = ref->lon;
}
-// printf("lon_sphere = %.10f\n",lon_sphere);
-
- *lat = lat_sphere * 180.0 / M_PI;
- *lon = lon_sphere * 180.0 / M_PI;
-
+ *lat = lat_rad * 180.0 / M_PI;
+ *lon = lon_rad * 180.0 / M_PI;
}
@@ -207,7 +139,7 @@ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, doub
return theta;
}
-__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* v_n, float* v_e)
+__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, float *v_e)
{
double lat_now_rad = lat_now * M_DEG_TO_RAD;
double lon_now_rad = lon_now * M_DEG_TO_RAD;
@@ -222,7 +154,7 @@ __EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double
*v_e = CONSTANTS_RADIUS_OF_EARTH * sin(d_lon) * cos(lat_next_rad);
}
-__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* v_n, float* v_e)
+__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, float *v_e)
{
double lat_now_rad = lat_now * M_DEG_TO_RAD;
double lon_now_rad = lon_now * M_DEG_TO_RAD;
@@ -248,7 +180,7 @@ __EXPORT void add_vector_to_global_position(double lat_now, double lon_now, floa
// Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu>
-__EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end)
+__EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end)
{
// This function returns the distance to the nearest point on the track line. Distance is positive if current
// position is right of the track and negative if left of the track as seen from a point on the track line
@@ -265,7 +197,7 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error,
crosstrack_error->bearing = 0.0f;
// Return error if arguments are bad
- if (lat_now == 0.0d || lon_now == 0.0d || lat_start == 0.0d || lon_start == 0.0d || lat_end == 0.0d || lon_end == 0.0d) return return_value;
+ if (lat_now == 0.0d || lon_now == 0.0d || lat_start == 0.0d || lon_start == 0.0d || lat_end == 0.0d || lon_end == 0.0d) { return return_value; }
bearing_end = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
bearing_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end);
@@ -296,8 +228,8 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error,
}
-__EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center,
- float radius, float arc_start_bearing, float arc_sweep)
+__EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center,
+ float radius, float arc_start_bearing, float arc_sweep)
{
// This function returns the distance to the nearest point on the track arc. Distance is positive if current
// position is right of the arc and negative if left of the arc as seen from the closest point on the arc and
@@ -316,29 +248,29 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, d
crosstrack_error->bearing = 0.0f;
// Return error if arguments are bad
- if (lat_now == 0.0d || lon_now == 0.0d || lat_center == 0.0d || lon_center == 0.0d || radius == 0.0d) return return_value;
+ if (lat_now == 0.0d || lon_now == 0.0d || lat_center == 0.0d || lon_center == 0.0d || radius == 0.0d) { return return_value; }
if (arc_sweep >= 0) {
bearing_sector_start = arc_start_bearing;
bearing_sector_end = arc_start_bearing + arc_sweep;
- if (bearing_sector_end > 2.0f * M_PI_F) bearing_sector_end -= M_TWOPI_F;
+ if (bearing_sector_end > 2.0f * M_PI_F) { bearing_sector_end -= M_TWOPI_F; }
} else {
bearing_sector_end = arc_start_bearing;
bearing_sector_start = arc_start_bearing - arc_sweep;
- if (bearing_sector_start < 0.0f) bearing_sector_start += M_TWOPI_F;
+ if (bearing_sector_start < 0.0f) { bearing_sector_start += M_TWOPI_F; }
}
in_sector = false;
// Case where sector does not span zero
- if (bearing_sector_end >= bearing_sector_start && bearing_now >= bearing_sector_start && bearing_now <= bearing_sector_end) in_sector = true;
+ if (bearing_sector_end >= bearing_sector_start && bearing_now >= bearing_sector_start && bearing_now <= bearing_sector_end) { in_sector = true; }
// Case where sector does span zero
- if (bearing_sector_end < bearing_sector_start && (bearing_now > bearing_sector_start || bearing_now < bearing_sector_end)) in_sector = true;
+ if (bearing_sector_end < bearing_sector_start && (bearing_now > bearing_sector_start || bearing_now < bearing_sector_end)) { in_sector = true; }
// If in the sector then calculate distance and bearing to closest point
if (in_sector) {
@@ -394,8 +326,8 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, d
}
__EXPORT float get_distance_to_point_global_wgs84(double lat_now, double lon_now, float alt_now,
- double lat_next, double lon_next, float alt_next,
- float *dist_xy, float *dist_z)
+ double lat_next, double lon_next, float alt_next,
+ float *dist_xy, float *dist_z)
{
double current_x_rad = lat_next / 180.0 * M_PI;
double current_y_rad = lon_next / 180.0 * M_PI;
@@ -419,8 +351,8 @@ __EXPORT float get_distance_to_point_global_wgs84(double lat_now, double lon_now
__EXPORT float mavlink_wpm_distance_to_point_local(float x_now, float y_now, float z_now,
- float x_next, float y_next, float z_next,
- float *dist_xy, float *dist_z)
+ float x_next, float y_next, float z_next,
+ float *dist_xy, float *dist_z)
{
float dx = x_now - x_next;
float dy = y_now - y_next;
@@ -442,15 +374,19 @@ __EXPORT float _wrap_pi(float bearing)
int c = 0;
while (bearing >= M_PI_F) {
bearing -= M_TWOPI_F;
- if (c++ > 3)
+
+ if (c++ > 3) {
return NAN;
+ }
}
c = 0;
while (bearing < -M_PI_F) {
bearing += M_TWOPI_F;
- if (c++ > 3)
+
+ if (c++ > 3) {
return NAN;
+ }
}
return bearing;
@@ -466,15 +402,19 @@ __EXPORT float _wrap_2pi(float bearing)
int c = 0;
while (bearing >= M_TWOPI_F) {
bearing -= M_TWOPI_F;
- if (c++ > 3)
+
+ if (c++ > 3) {
return NAN;
+ }
}
c = 0;
while (bearing < 0.0f) {
bearing += M_TWOPI_F;
- if (c++ > 3)
+
+ if (c++ > 3) {
return NAN;
+ }
}
return bearing;
@@ -490,15 +430,19 @@ __EXPORT float _wrap_180(float bearing)
int c = 0;
while (bearing >= 180.0f) {
bearing -= 360.0f;
- if (c++ > 3)
+
+ if (c++ > 3) {
return NAN;
+ }
}
c = 0;
while (bearing < -180.0f) {
bearing += 360.0f;
- if (c++ > 3)
+
+ if (c++ > 3) {
return NAN;
+ }
}
return bearing;
@@ -514,15 +458,19 @@ __EXPORT float _wrap_360(float bearing)
int c = 0;
while (bearing >= 360.0f) {
bearing -= 360.0f;
- if (c++ > 3)
+
+ if (c++ > 3) {
return NAN;
+ }
}
c = 0;
while (bearing < 0.0f) {
bearing += 360.0f;
- if (c++ > 3)
+
+ if (c++ > 3) {
return NAN;
+ }
}
return bearing;
diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h
index 94afb4df0..0a3f85d97 100644
--- a/src/lib/geo/geo.h
+++ b/src/lib/geo/geo.h
@@ -1,9 +1,6 @@
/****************************************************************************
*
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
- * Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- * Julian Oes <joes@student.ethz.ch>
- * Lorenz Meier <lm@inf.ethz.ch>
+ * Copyright (C) 2012, 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -42,6 +39,7 @@
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
* Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu>
*/
@@ -67,6 +65,14 @@ struct crosstrack_error_s {
float bearing; // Bearing in radians to closest point on line/arc
} ;
+/* lat/lon are in radians */
+struct map_projection_reference_s {
+ double lat;
+ double lon;
+ double sin_lat;
+ double cos_lat;
+};
+
/**
* Initializes the map transformation.
*
@@ -74,7 +80,7 @@ struct crosstrack_error_s {
* @param lat in degrees (47.1234567°, not 471234567°)
* @param lon in degrees (8.1234567°, not 81234567°)
*/
-__EXPORT void map_projection_init(double lat_0, double lon_0);
+__EXPORT void map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0);
/**
* Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane
@@ -83,7 +89,7 @@ __EXPORT void map_projection_init(double lat_0, double lon_0);
* @param lat in degrees (47.1234567°, not 471234567°)
* @param lon in degrees (8.1234567°, not 81234567°)
*/
-__EXPORT void map_projection_project(double lat, double lon, float *x, float *y);
+__EXPORT void map_projection_project(struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y);
/**
* Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system
@@ -93,7 +99,7 @@ __EXPORT void map_projection_project(double lat, double lon, float *x, float *y)
* @param lat in degrees (47.1234567°, not 471234567°)
* @param lon in degrees (8.1234567°, not 81234567°)
*/
-__EXPORT void map_projection_reproject(float x, float y, double *lat, double *lon);
+__EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon);
/**
* Returns the distance to the next waypoint in meters.
@@ -115,30 +121,30 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou
*/
__EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
-__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* v_n, float* v_e);
+__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, float *v_e);
-__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* v_n, float* v_e);
+__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, float *v_e);
__EXPORT void add_vector_to_global_position(double lat_now, double lon_now, float v_n, float v_e, double *lat_res, double *lon_res);
-__EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end);
+__EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end);
-__EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center,
- float radius, float arc_start_bearing, float arc_sweep);
+__EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center,
+ float radius, float arc_start_bearing, float arc_sweep);
/*
* Calculate distance in global frame
*/
__EXPORT float get_distance_to_point_global_wgs84(double lat_now, double lon_now, float alt_now,
- double lat_next, double lon_next, float alt_next,
- float *dist_xy, float *dist_z);
+ double lat_next, double lon_next, float alt_next,
+ float *dist_xy, float *dist_z);
/*
* Calculate distance in local frame (NED)
*/
__EXPORT float mavlink_wpm_distance_to_point_local(float x_now, float y_now, float z_now,
- float x_next, float y_next, float z_next,
- float *dist_xy, float *dist_z);
+ float x_next, float y_next, float z_next,
+ float *dist_xy, float *dist_z);
__EXPORT float _wrap_180(float bearing);
__EXPORT float _wrap_360(float bearing);
diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
index 668bac5d9..5cf84542b 100644
--- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
+++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
@@ -97,6 +97,8 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
{
using namespace math;
+ memset(&ref, 0, sizeof(ref));
+
F.zero();
G.zero();
V.zero();
@@ -247,11 +249,7 @@ void KalmanNav::update()
lat0 = lat;
lon0 = lon;
alt0 = alt;
- // XXX map_projection has internal global
- // states that multiple things could change,
- // should make map_projection take reference
- // lat/lon and not have init
- map_projection_init(lat0, lon0);
+ map_projection_init(&ref, lat0, lon0);
_positionInitialized = true;
warnx("initialized EKF state with GPS");
warnx("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f",
@@ -314,7 +312,6 @@ void KalmanNav::updatePublications()
// global position publication
_pos.timestamp = _pubTimeStamp;
_pos.time_gps_usec = _gps.timestamp_position;
- _pos.global_valid = true;
_pos.lat = lat * M_RAD_TO_DEG;
_pos.lon = lon * M_RAD_TO_DEG;
_pos.alt = float(alt);
@@ -327,7 +324,7 @@ void KalmanNav::updatePublications()
float x;
float y;
bool landed = alt < (alt0 + 0.1); // XXX improve?
- map_projection_project(lat, lon, &x, &y);
+ map_projection_project(&ref, lat, lon, &x, &y);
_localPos.timestamp = _pubTimeStamp;
_localPos.xy_valid = true;
_localPos.z_valid = true;
@@ -343,8 +340,8 @@ void KalmanNav::updatePublications()
_localPos.xy_global = true;
_localPos.z_global = true;
_localPos.ref_timestamp = _pubTimeStamp;
- _localPos.ref_lat = getLatDegE7();
- _localPos.ref_lon = getLonDegE7();
+ _localPos.ref_lat = lat * M_RAD_TO_DEG;
+ _localPos.ref_lon = lon * M_RAD_TO_DEG;
_localPos.ref_alt = 0;
_localPos.landed = landed;
diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp
index caf93bc78..24df153cb 100644
--- a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp
+++ b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp
@@ -49,6 +49,7 @@
#include <controllib/block/BlockParam.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/Publication.hpp>
+#include <lib/geo/geo.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_global_position.h>
@@ -164,6 +165,7 @@ protected:
// parameters
float alt; /**< altitude, meters */
double lat0, lon0; /**< reference latitude and longitude */
+ struct map_projection_reference_s ref; /**< local projection reference */
float alt0; /**< refeerence altitude (ground height) */
control::BlockParamFloat _vGyro; /**< gyro process noise */
control::BlockParamFloat _vAccel; /**< accelerometer process noise */
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
index 10a6cd2c5..c61b6ff3f 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
@@ -407,7 +407,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
vel(2) = gps.vel_d_m_s;
}
- } else if (ekf_params.acc_comp == 2 && global_pos.global_valid && hrt_absolute_time() < global_pos.timestamp + 500000) {
+ } else if (ekf_params.acc_comp == 2 && gps.eph_m < 5.0f && global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) {
vel_valid = true;
if (global_pos_updated) {
vel_t = global_pos.timestamp;
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index cf7ba757e..7257cb4b3 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -117,7 +117,7 @@ extern struct system_load_s system_load;
#define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000
#define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
-#define POSITION_TIMEOUT 1000000 /**< consider the local or global position estimate invalid after 1s */
+#define POSITION_TIMEOUT 30000 /**< consider the local or global position estimate invalid after 30ms */
#define RC_TIMEOUT 100000
#define RC_TIMEOUT_HIL 500000
#define DIFFPRESS_TIMEOUT 2000000
@@ -195,7 +195,7 @@ void usage(const char *reason);
/**
* React to commands that are sent e.g. from the mavlink module.
*/
-bool handle_command(struct vehicle_status_s *status, struct vehicle_command_s *cmd, struct actuator_armed_s *armed);
+bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub);
/**
* Mainloop of commander.
@@ -390,7 +390,7 @@ int disarm()
}
}
-bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed)
+bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub)
{
/* result of the command */
enum VEHICLE_CMD_RESULT result = VEHICLE_CMD_RESULT_UNSUPPORTED;
@@ -580,6 +580,51 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
}
break;
+ case VEHICLE_CMD_DO_SET_HOME: {
+ bool use_current = cmd->param1 > 0.5f;
+ if (use_current) {
+ /* use current position */
+ if (status->condition_global_position_valid) {
+ home->lat = global_pos->lat;
+ home->lon = global_pos->lon;
+ home->alt = global_pos->alt;
+
+ home->timestamp = hrt_absolute_time();
+
+ result = VEHICLE_CMD_RESULT_ACCEPTED;
+
+ } else {
+ result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+ }
+
+ } else {
+ /* use specified position */
+ home->lat = cmd->param5;
+ home->lon = cmd->param6;
+ home->alt = cmd->param7;
+
+ home->timestamp = hrt_absolute_time();
+
+ result = VEHICLE_CMD_RESULT_ACCEPTED;
+ }
+
+ if (result == VEHICLE_CMD_RESULT_ACCEPTED) {
+ warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home->lat, home->lon, (double)home->alt);
+ mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home->lat, home->lon, (double)home->alt);
+
+ /* announce new home position */
+ if (*home_pub > 0) {
+ orb_publish(ORB_ID(home_position), *home_pub, &home);
+
+ } else {
+ *home_pub = orb_advertise(ORB_ID(home_position), &home);
+ }
+
+ /* mark home position as set */
+ status->condition_home_position_valid = true;
+ }
+ }
+ break;
case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
case VEHICLE_CMD_PREFLIGHT_CALIBRATION:
case VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS:
@@ -612,6 +657,7 @@ int commander_thread_main(int argc, char *argv[])
commander_initialized = false;
bool arm_tune_played = false;
+ bool was_armed = false;
/* set parameters */
param_t _param_sys_type = param_find("MAV_TYPE");
@@ -920,7 +966,35 @@ int commander_thread_main(int argc, char *argv[])
}
/* update condition_global_position_valid */
- check_valid(global_position.timestamp, POSITION_TIMEOUT, global_position.global_valid, &(status.condition_global_position_valid), &status_changed);
+ check_valid(global_position.timestamp, POSITION_TIMEOUT, true, &(status.condition_global_position_valid), &status_changed);
+
+ /* check if GPS fix is ok */
+ static float hdop_threshold_m = 4.0f;
+ static float vdop_threshold_m = 8.0f;
+
+ /* update home position */
+ if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed &&
+ (global_position.eph < hdop_threshold_m) && (global_position.epv < vdop_threshold_m)) {
+
+ home.lat = global_position.lat;
+ home.lon = global_position.lon;
+ home.alt = global_position.alt;
+
+ warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt);
+ mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt);
+
+ /* announce new home position */
+ if (home_pub > 0) {
+ orb_publish(ORB_ID(home_position), home_pub, &home);
+
+ } else {
+ home_pub = orb_advertise(ORB_ID(home_position), &home);
+ }
+
+ /* mark home position as set */
+ status.condition_home_position_valid = true;
+ tune_positive(true);
+ }
/* update local position estimate */
orb_check(local_position_sub, &updated);
@@ -1068,45 +1142,6 @@ int commander_thread_main(int argc, char *argv[])
if (updated) {
orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position);
- /* check if GPS fix is ok */
- float hdop_threshold_m = 4.0f;
- float vdop_threshold_m = 8.0f;
-
- /*
- * If horizontal dilution of precision (hdop / eph)
- * and vertical diluation of precision (vdop / epv)
- * are below a certain threshold (e.g. 4 m), AND
- * home position is not yet set AND the last GPS
- * GPS measurement is not older than two seconds AND
- * the system is currently not armed, set home
- * position to the current position.
- */
-
- if (!status.condition_home_position_valid && gps_position.fix_type >= 3 &&
- (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) &&
- (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed
- && global_position.global_valid) {
-
- /* copy position data to uORB home message, store it locally as well */
- home.lat = global_position.lat;
- home.lon = global_position.lon;
- home.alt = global_position.alt;
-
- warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.alt);
- mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.alt);
-
- /* announce new home position */
- if (home_pub > 0) {
- orb_publish(ORB_ID(home_position), home_pub, &home);
-
- } else {
- home_pub = orb_advertise(ORB_ID(home_position), &home);
- }
-
- /* mark home position as set */
- status.condition_home_position_valid = true;
- tune_positive(true);
- }
}
@@ -1278,7 +1313,7 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
/* handle it */
- if (handle_command(&status, &safety, &cmd, &armed))
+ if (handle_command(&status, &safety, &cmd, &armed, &home, &global_position, &home_pub))
status_changed = true;
}
@@ -1293,7 +1328,32 @@ int commander_thread_main(int argc, char *argv[])
if (arming_state_changed) {
status_changed = true;
mavlink_log_info(mavlink_fd, "[cmd] arming state: %s", arming_states_str[status.arming_state]);
+
+ /* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */
+ if (armed.armed && !was_armed && hrt_absolute_time() > start_time + 2000000 && status.condition_global_position_valid &&
+ (global_position.eph < hdop_threshold_m) && (global_position.epv < vdop_threshold_m)) {
+
+ // TODO remove code duplication
+ home.lat = global_position.lat;
+ home.lon = global_position.lon;
+ home.alt = global_position.alt;
+
+ warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt);
+ mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt);
+
+ /* announce new home position */
+ if (home_pub > 0) {
+ orb_publish(ORB_ID(home_position), home_pub, &home);
+
+ } else {
+ home_pub = orb_advertise(ORB_ID(home_position), &home);
+ }
+
+ /* mark home position as set */
+ status.condition_home_position_valid = true;
+ }
}
+ was_armed = armed.armed;
if (main_state_changed) {
status_changed = true;
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index d7e300670..dc6236a51 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -245,6 +245,7 @@ MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg)
memset(&f, 0, sizeof(f));
f.timestamp = hrt_absolute_time();
+ f.flow_timestamp = flow.time_usec;
f.flow_raw_x = flow.flow_x;
f.flow_raw_y = flow.flow_y;
f.flow_comp_x_m = flow.flow_comp_m_x;
@@ -766,7 +767,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
memset(&hil_global_pos, 0, sizeof(hil_global_pos));
hil_global_pos.timestamp = timestamp;
- hil_global_pos.global_valid = true;
hil_global_pos.lat = hil_state.lat;
hil_global_pos.lon = hil_state.lon;
hil_global_pos.alt = hil_state.alt / 1000.0f;
@@ -774,6 +774,8 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
hil_global_pos.vel_e = hil_state.vy / 100.0f;
hil_global_pos.vel_d = hil_state.vz / 100.0f;
hil_global_pos.yaw = hil_attitude.yaw;
+ hil_global_pos.eph = 2.0f;
+ hil_global_pos.epv = 4.0f;
if (_global_pos_pub > 0) {
orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &hil_global_pos);
@@ -785,19 +787,22 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
/* local position */
{
+ double lat = hil_state.lat * 1e-7;
+ double lon = hil_state.lon * 1e-7;
+
if (!_hil_local_proj_inited) {
_hil_local_proj_inited = true;
_hil_local_alt0 = hil_state.alt / 1000.0f;
- map_projection_init(hil_state.lat, hil_state.lon);
+ map_projection_init(&_hil_local_proj_ref, hil_state.lat, hil_state.lon);
hil_local_pos.ref_timestamp = timestamp;
- hil_local_pos.ref_lat = hil_state.lat;
- hil_local_pos.ref_lon = hil_state.lon;
+ hil_local_pos.ref_lat = lat;
+ hil_local_pos.ref_lon = lon;
hil_local_pos.ref_alt = _hil_local_alt0;
}
float x;
float y;
- map_projection_project(hil_state.lat * 1e-7, hil_state.lon * 1e-7, &x, &y);
+ map_projection_project(&_hil_local_proj_ref, lat, lon, &x, &y);
hil_local_pos.timestamp = timestamp;
hil_local_pos.xy_valid = true;
hil_local_pos.z_valid = true;
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 0a5a1b5c7..796c09722 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -143,4 +143,5 @@ private:
uint64_t _old_timestamp;
bool _hil_local_proj_inited;
float _hil_local_alt0;
+ struct map_projection_reference_s _hil_local_proj_ref;
};
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index 78d06ba5b..21070e1e9 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -1,7 +1,6 @@
/****************************************************************************
*
- * Copyright (c) 2013 PX4 Development Team. All rights reserved.
- * Author: @author Anton Babushkin <anton.babushkin@me.com>
+ * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -40,6 +39,8 @@
* Output of velocity controller is thrust vector that splitted to thrust direction
* (i.e. rotation matrix for multicopter orientation) and thrust module (i.e. multicopter thrust itself).
* Controller doesn't use Euler angles for work, they generated only for more human-friendly control and logging.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <nuttx/config.h>
@@ -62,9 +63,10 @@
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/parameter_update.h>
-#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_global_velocity_setpoint.h>
+#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
@@ -114,20 +116,21 @@ private:
int _params_sub; /**< notification of parameter updates */
int _manual_sub; /**< notification of manual control updates */
int _arming_sub; /**< arming status of outputs */
- int _global_pos_sub; /**< vehicle local position */
+ int _local_pos_sub; /**< vehicle local position */
int _pos_sp_triplet_sub; /**< position setpoint triplet */
orb_advert_t _att_sp_pub; /**< attitude setpoint publication */
- orb_advert_t _pos_sp_triplet_pub; /**< position setpoint triplet publication */
- orb_advert_t _global_vel_sp_pub; /**< vehicle global velocity setpoint */
+ orb_advert_t _local_pos_sp_pub; /**< vehicle local position setpoint publication */
+ orb_advert_t _global_vel_sp_pub; /**< vehicle global velocity setpoint publication */
struct vehicle_attitude_s _att; /**< vehicle attitude */
struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */
struct manual_control_setpoint_s _manual; /**< r/c channel data */
struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */
struct actuator_armed_s _arming; /**< actuator arming status */
- struct vehicle_global_position_s _global_pos; /**< vehicle global position */
+ struct vehicle_local_position_s _local_pos; /**< vehicle local position */
struct position_setpoint_triplet_s _pos_sp_triplet; /**< vehicle global position setpoint triplet */
+ struct vehicle_local_position_setpoint_s _local_pos_sp; /**< vehicle local position setpoint */
struct vehicle_global_velocity_setpoint_s _global_vel_sp; /**< vehicle global velocity setpoint */
struct {
@@ -172,14 +175,15 @@ private:
math::Vector<3> sp_offs_max;
} _params;
- double _lat_sp;
- double _lon_sp;
- float _alt_sp;
+ struct map_projection_reference_s _ref_pos;
+ float _ref_alt;
+ hrt_abstime _ref_timestamp;
- bool _reset_lat_lon_sp;
+ bool _reset_pos_sp;
bool _reset_alt_sp;
- bool _use_global_alt; /**< switch between global (AMSL) and barometric altitudes */
+ math::Vector<3> _pos;
+ math::Vector<3> _pos_sp;
math::Vector<3> _vel;
math::Vector<3> _vel_sp;
math::Vector<3> _vel_prev; /**< velocity on previous step */
@@ -202,9 +206,13 @@ private:
static float scale_control(float ctl, float end, float dz);
/**
- * Reset lat/lon to current position
+ * Update reference for local position projection
+ */
+ void update_ref();
+ /**
+ * Reset position setpoint to current position
*/
- void reset_lat_lon_sp();
+ void reset_pos_sp();
/**
* Reset altitude setpoint to current altitude
@@ -252,31 +260,32 @@ MulticopterPositionControl::MulticopterPositionControl() :
_params_sub(-1),
_manual_sub(-1),
_arming_sub(-1),
- _global_pos_sub(-1),
+ _local_pos_sub(-1),
_pos_sp_triplet_sub(-1),
/* publications */
_att_sp_pub(-1),
- _pos_sp_triplet_pub(-1),
+ _local_pos_sp_pub(-1),
_global_vel_sp_pub(-1),
- _lat_sp(0.0),
- _lon_sp(0.0),
- _alt_sp(0.0f),
+ _ref_alt(0.0f),
+ _ref_timestamp(0),
- _reset_lat_lon_sp(true),
- _reset_alt_sp(true),
- _use_global_alt(false)
+ _reset_pos_sp(true),
+ _reset_alt_sp(true)
{
memset(&_att, 0, sizeof(_att));
memset(&_att_sp, 0, sizeof(_att_sp));
memset(&_manual, 0, sizeof(_manual));
memset(&_control_mode, 0, sizeof(_control_mode));
memset(&_arming, 0, sizeof(_arming));
- memset(&_global_pos, 0, sizeof(_global_pos));
+ memset(&_local_pos, 0, sizeof(_local_pos));
memset(&_pos_sp_triplet, 0, sizeof(_pos_sp_triplet));
+ memset(&_local_pos_sp, 0, sizeof(_local_pos_sp));
memset(&_global_vel_sp, 0, sizeof(_global_vel_sp));
+ memset(&_ref_pos, 0, sizeof(_ref_pos));
+
_params.pos_p.zero();
_params.vel_p.zero();
_params.vel_i.zero();
@@ -285,6 +294,8 @@ MulticopterPositionControl::MulticopterPositionControl() :
_params.vel_ff.zero();
_params.sp_offs_max.zero();
+ _pos.zero();
+ _pos_sp.zero();
_vel.zero();
_vel_sp.zero();
_vel_prev.zero();
@@ -345,8 +356,9 @@ MulticopterPositionControl::parameters_update(bool force)
orb_check(_params_sub, &updated);
- if (updated)
+ if (updated) {
orb_copy(ORB_ID(parameter_update), _params_sub, &param_upd);
+ }
if (updated || force) {
param_get(_params_handles.thr_min, &_params.thr_min);
@@ -402,33 +414,39 @@ MulticopterPositionControl::poll_subscriptions()
orb_check(_att_sub, &updated);
- if (updated)
+ if (updated) {
orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
+ }
orb_check(_att_sp_sub, &updated);
- if (updated)
+ if (updated) {
orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp);
+ }
orb_check(_control_mode_sub, &updated);
- if (updated)
+ if (updated) {
orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
+ }
orb_check(_manual_sub, &updated);
- if (updated)
+ if (updated) {
orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &_manual);
+ }
orb_check(_arming_sub, &updated);
- if (updated)
+ if (updated) {
orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming);
+ }
- orb_check(_global_pos_sub, &updated);
+ orb_check(_local_pos_sub, &updated);
- if (updated)
- orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
+ if (updated) {
+ orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos);
+ }
}
float
@@ -452,40 +470,42 @@ MulticopterPositionControl::task_main_trampoline(int argc, char *argv[])
}
void
-MulticopterPositionControl::reset_lat_lon_sp()
+MulticopterPositionControl::update_ref()
{
- if (_reset_lat_lon_sp) {
- _reset_lat_lon_sp = false;
- _lat_sp = _global_pos.lat;
- _lon_sp = _global_pos.lon;
- mavlink_log_info(_mavlink_fd, "[mpc] reset lat/lon sp: %.7f, %.7f", _lat_sp, _lon_sp);
+ if (_local_pos.ref_timestamp != _ref_timestamp) {
+ if (_ref_timestamp != 0) {
+ /* reproject local position setpoint to new reference */
+ float dx, dy;
+ map_projection_project(&_ref_pos, _local_pos.ref_lat, _local_pos.ref_lon, &dx, &dy);
+ _pos_sp(0) -= dx;
+ _pos_sp(1) -= dy;
+ }
+
+ _ref_timestamp = _local_pos.ref_timestamp;
+
+ map_projection_init(&_ref_pos, _local_pos.ref_lat, _local_pos.ref_lon);
+ _ref_alt = _local_pos.ref_alt;
}
}
void
-MulticopterPositionControl::reset_alt_sp()
+MulticopterPositionControl::reset_pos_sp()
{
- if (_reset_alt_sp) {
- _reset_alt_sp = false;
- _alt_sp = _use_global_alt ? _global_pos.alt : _global_pos.baro_alt;
- mavlink_log_info(_mavlink_fd, "[mpc] reset alt (%s) sp: %.2f", _use_global_alt ? "AMSL" : "baro", (double)_alt_sp);
+ if (_reset_pos_sp) {
+ _reset_pos_sp = false;
+ _pos_sp(0) = _pos(0);
+ _pos_sp(1) = _pos(1);
+ mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1));
}
}
void
-MulticopterPositionControl::select_alt(bool global)
+MulticopterPositionControl::reset_alt_sp()
{
- if (global != _use_global_alt) {
- _use_global_alt = global;
-
- if (global) {
- /* switch from barometric to global altitude */
- _alt_sp += _global_pos.alt - _global_pos.baro_alt;
-
- } else {
- /* switch from global to barometric altitude */
- _alt_sp += _global_pos.baro_alt - _global_pos.alt;
- }
+ if (_reset_alt_sp) {
+ _reset_alt_sp = false;
+ _pos_sp(2) = _pos(2);
+ mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %.2f", -(double)_pos_sp(2));
}
}
@@ -506,7 +526,7 @@ MulticopterPositionControl::task_main()
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_arming_sub = orb_subscribe(ORB_ID(actuator_armed));
- _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
+ _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
parameters_update(true);
@@ -537,8 +557,7 @@ MulticopterPositionControl::task_main()
/* wakeup source */
struct pollfd fds[1];
- /* Setup of loop */
- fds[0].fd = _global_pos_sub;
+ fds[0].fd = _local_pos_sub;
fds[0].events = POLLIN;
while (!_task_should_exit) {
@@ -546,8 +565,9 @@ MulticopterPositionControl::task_main()
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500);
/* timed out - periodic check for _task_should_exit */
- if (pret == 0)
+ if (pret == 0) {
continue;
+ }
/* this is undesirable but not much we can do */
if (pret < 0) {
@@ -564,7 +584,7 @@ MulticopterPositionControl::task_main()
if (_control_mode.flag_armed && !was_armed) {
/* reset setpoints and integrals on arming */
- _reset_lat_lon_sp = true;
+ _reset_pos_sp = true;
_reset_alt_sp = true;
reset_int_z = true;
reset_int_xy = true;
@@ -572,28 +592,25 @@ MulticopterPositionControl::task_main()
was_armed = _control_mode.flag_armed;
+ update_ref();
+
if (_control_mode.flag_control_altitude_enabled ||
_control_mode.flag_control_position_enabled ||
_control_mode.flag_control_climb_rate_enabled ||
_control_mode.flag_control_velocity_enabled) {
- _vel(0) = _global_pos.vel_n;
- _vel(1) = _global_pos.vel_e;
- _vel(2) = _global_pos.vel_d;
+ _pos(0) = _local_pos.x;
+ _pos(1) = _local_pos.y;
+ _pos(2) = _local_pos.z;
- sp_move_rate.zero();
+ _vel(0) = _local_pos.vx;
+ _vel(1) = _local_pos.vy;
+ _vel(2) = _local_pos.vz;
- float alt = _global_pos.alt;
+ sp_move_rate.zero();
/* select control source */
if (_control_mode.flag_control_manual_enabled) {
- /* select altitude source and update setpoint */
- select_alt(_global_pos.global_valid);
-
- if (!_use_global_alt) {
- alt = _global_pos.baro_alt;
- }
-
/* manual control */
if (_control_mode.flag_control_altitude_enabled) {
/* reset alt setpoint to current altitude if needed */
@@ -604,8 +621,8 @@ MulticopterPositionControl::task_main()
}
if (_control_mode.flag_control_position_enabled) {
- /* reset lat/lon setpoint to current position if needed */
- reset_lat_lon_sp();
+ /* reset position setpoint to current position if needed */
+ reset_pos_sp();
/* move position setpoint with roll/pitch stick */
sp_move_rate(0) = scale_control(-_manual.pitch / _params.rc_scale_pitch, 1.0f, pos_ctl_dz);
@@ -625,74 +642,47 @@ MulticopterPositionControl::task_main()
sp_move_rate = R_yaw_sp * sp_move_rate.emult(_params.vel_max);
/* move position setpoint */
- add_vector_to_global_position(_lat_sp, _lon_sp, sp_move_rate(0) * dt, sp_move_rate(1) * dt, &_lat_sp, &_lon_sp);
- _alt_sp -= sp_move_rate(2) * dt;
+ _pos_sp += sp_move_rate * dt;
/* check if position setpoint is too far from actual position */
math::Vector<3> pos_sp_offs;
pos_sp_offs.zero();
if (_control_mode.flag_control_position_enabled) {
- get_vector_to_next_waypoint_fast(_global_pos.lat, _global_pos.lon, _lat_sp, _lon_sp, &pos_sp_offs.data[0], &pos_sp_offs.data[1]);
- pos_sp_offs(0) /= _params.sp_offs_max(0);
- pos_sp_offs(1) /= _params.sp_offs_max(1);
+ pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0);
+ pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1);
}
if (_control_mode.flag_control_altitude_enabled) {
- pos_sp_offs(2) = -(_alt_sp - alt) / _params.sp_offs_max(2);
+ pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2);
}
float pos_sp_offs_norm = pos_sp_offs.length();
if (pos_sp_offs_norm > 1.0f) {
pos_sp_offs /= pos_sp_offs_norm;
- add_vector_to_global_position(_global_pos.lat, _global_pos.lon, pos_sp_offs(0) * _params.sp_offs_max(0), pos_sp_offs(1) * _params.sp_offs_max(1), &_lat_sp, &_lon_sp);
- _alt_sp = alt - pos_sp_offs(2) * _params.sp_offs_max(2);
- }
-
- /* fill position setpoint triplet */
- _pos_sp_triplet.previous.valid = true;
- _pos_sp_triplet.current.valid = true;
- _pos_sp_triplet.next.valid = true;
-
- _pos_sp_triplet.nav_state = NAV_STATE_NONE;
- _pos_sp_triplet.current.type = SETPOINT_TYPE_NORMAL;
- _pos_sp_triplet.current.lat = _lat_sp;
- _pos_sp_triplet.current.lon = _lon_sp;
- _pos_sp_triplet.current.alt = _alt_sp;
- _pos_sp_triplet.current.yaw = _att_sp.yaw_body;
- _pos_sp_triplet.current.loiter_radius = 0.0f;
- _pos_sp_triplet.current.loiter_direction = 1.0f;
- _pos_sp_triplet.current.pitch_min = 0.0f;
-
- /* publish position setpoint triplet */
- if (_pos_sp_triplet_pub > 0) {
- orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, &_pos_sp_triplet);
-
- } else {
- _pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), &_pos_sp_triplet);
+ _pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max);
}
} else {
- /* always use AMSL altitude for AUTO */
- select_alt(true);
-
/* AUTO */
bool updated;
orb_check(_pos_sp_triplet_sub, &updated);
- if (updated)
+ if (updated) {
orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
+ }
if (_pos_sp_triplet.current.valid) {
/* in case of interrupted mission don't go to waypoint but stay at current position */
- _reset_lat_lon_sp = true;
+ _reset_pos_sp = true;
_reset_alt_sp = true;
- /* update position setpoint */
- _lat_sp = _pos_sp_triplet.current.lat;
- _lon_sp = _pos_sp_triplet.current.lon;
- _alt_sp = _pos_sp_triplet.current.alt;
+ /* project setpoint to local frame */
+ map_projection_project(&_ref_pos,
+ _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon,
+ &_pos_sp.data[0], &_pos_sp.data[1]);
+ _pos_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt);
/* update yaw setpoint if needed */
if (isfinite(_pos_sp_triplet.current.yaw)) {
@@ -701,11 +691,25 @@ MulticopterPositionControl::task_main()
} else {
/* no waypoint, loiter, reset position setpoint if needed */
- reset_lat_lon_sp();
+ reset_pos_sp();
reset_alt_sp();
}
}
+ /* fill local position setpoint */
+ _local_pos_sp.x = _pos_sp(0);
+ _local_pos_sp.y = _pos_sp(1);
+ _local_pos_sp.z = _pos_sp(2);
+ _local_pos_sp.yaw = _att_sp.yaw_body;
+
+ /* publish local position setpoint */
+ if (_local_pos_sp_pub > 0) {
+ orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &_local_pos_sp);
+
+ } else {
+ _local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp);
+ }
+
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_IDLE) {
/* idle state, don't run controller and set zero thrust */
R.identity();
@@ -729,9 +733,7 @@ MulticopterPositionControl::task_main()
} else {
/* run position & altitude controllers, calculate velocity setpoint */
- math::Vector<3> pos_err;
- get_vector_to_next_waypoint_fast(_global_pos.lat, _global_pos.lon, _lat_sp, _lon_sp, &pos_err.data[0], &pos_err.data[1]);
- pos_err(2) = -(_alt_sp - alt);
+ math::Vector<3> pos_err = _pos_sp - _pos;
_vel_sp = pos_err.emult(_params.pos_p) + sp_move_rate.emult(_params.vel_ff);
@@ -741,7 +743,7 @@ MulticopterPositionControl::task_main()
}
if (!_control_mode.flag_control_position_enabled) {
- _reset_lat_lon_sp = true;
+ _reset_pos_sp = true;
_vel_sp(0) = 0.0f;
_vel_sp(1) = 0.0f;
}
@@ -847,8 +849,9 @@ MulticopterPositionControl::task_main()
/* limit max tilt and min lift when landing */
tilt_max = _params.land_tilt_max;
- if (thr_min < 0.0f)
+ if (thr_min < 0.0f) {
thr_min = 0.0f;
+ }
}
/* limit min lift */
@@ -939,8 +942,9 @@ MulticopterPositionControl::task_main()
thrust_int(2) += vel_err(2) * _params.vel_i(2) * dt;
/* protection against flipping on ground when landing */
- if (thrust_int(2) > 0.0f)
+ if (thrust_int(2) > 0.0f) {
thrust_int(2) = 0.0f;
+ }
}
/* calculate attitude setpoint from thrust vector */
@@ -1021,7 +1025,7 @@ MulticopterPositionControl::task_main()
} else {
/* position controller disabled, reset setpoints */
_reset_alt_sp = true;
- _reset_lat_lon_sp = true;
+ _reset_pos_sp = true;
reset_int_z = true;
reset_int_xy = true;
}
@@ -1060,18 +1064,21 @@ MulticopterPositionControl::start()
int mc_pos_control_main(int argc, char *argv[])
{
- if (argc < 1)
+ if (argc < 1) {
errx(1, "usage: mc_pos_control {start|stop|status}");
+ }
if (!strcmp(argv[1], "start")) {
- if (pos_control::g_control != nullptr)
+ if (pos_control::g_control != nullptr) {
errx(1, "already running");
+ }
pos_control::g_control = new MulticopterPositionControl;
- if (pos_control::g_control == nullptr)
+ if (pos_control::g_control == nullptr) {
errx(1, "alloc failed");
+ }
if (OK != pos_control::g_control->start()) {
delete pos_control::g_control;
@@ -1083,8 +1090,9 @@ int mc_pos_control_main(int argc, char *argv[])
}
if (!strcmp(argv[1], "stop")) {
- if (pos_control::g_control == nullptr)
+ if (pos_control::g_control == nullptr) {
errx(1, "not running");
+ }
delete pos_control::g_control;
pos_control::g_control = nullptr;
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index c45cafc1b..ef7201790 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -177,7 +177,7 @@ private:
class Mission _mission;
bool _mission_item_valid; /**< current mission item valid */
- bool _global_pos_valid; /**< track changes of global_position.global_valid flag */
+ bool _global_pos_valid; /**< track changes of global_position */
bool _reset_loiter_pos; /**< if true then loiter position should be set to current position */
bool _waypoint_position_reached;
bool _waypoint_yaw_reached;
@@ -817,13 +817,11 @@ Navigator::task_main()
if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) {
_pos_sp_triplet_updated = true;
- if (myState == NAV_STATE_LAND && _global_pos.global_valid && !_global_pos_valid) {
+ if (myState == NAV_STATE_LAND && !_global_pos_valid) {
/* got global position when landing, update setpoint */
start_land();
}
- _global_pos_valid = _global_pos.global_valid;
-
/* check if waypoint has been reached in MISSION, RTL and LAND modes */
if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL || myState == NAV_STATE_LAND) {
if (check_mission_item_reached()) {
@@ -846,8 +844,15 @@ Navigator::task_main()
/* Reset the _geofence_violation_warning_sent field */
_geofence_violation_warning_sent = false;
}
+
+ _global_pos_valid = true;
+
+ } else {
+ /* assume that global position is valid if updated in last 20ms */
+ _global_pos_valid = _global_pos.timestamp != 0 && hrt_abstime() < _global_pos.timestamp + 20000;
}
+
/* publish position setpoint triplet if updated */
if (_pos_sp_triplet_updated) {
_pos_sp_triplet_updated = false;
@@ -893,9 +898,9 @@ Navigator::start()
void
Navigator::status()
{
- warnx("Global position is %svalid", _global_pos.global_valid ? "" : "in");
+ warnx("Global position: %svalid", _global_pos_valid ? "" : "in");
- if (_global_pos.global_valid) {
+ if (_global_pos_valid) {
warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon, _global_pos.lat);
warnx("Altitude %5.5f meters, altitude above home %5.5f meters",
(double)_global_pos.alt, (double)(_global_pos.alt - _home_pos.alt));
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c
index 368fa6ee2..3f1a5d39b 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -1,7 +1,6 @@
/****************************************************************************
*
- * Copyright (C) 2013 Anton Babushkin. All rights reserved.
- * Author: Anton Babushkin <rk3dov@gmail.com>
+ * Copyright (C) 2013, 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -35,6 +34,8 @@
/**
* @file position_estimator_inav_main.c
* Model-identification based position estimator for multirotors
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <unistd.h>
@@ -57,6 +58,7 @@
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/home_position.h>
#include <uORB/topics/optical_flow.h>
#include <mavlink/mavlink_log.h>
#include <poll.h>
@@ -95,8 +97,9 @@ static void usage(const char *reason);
*/
static void usage(const char *reason)
{
- if (reason)
+ if (reason) {
fprintf(stderr, "%s\n", reason);
+ }
fprintf(stderr, "usage: position_estimator_inav {start|stop|status} [-v]\n\n");
exit(1);
@@ -112,8 +115,9 @@ static void usage(const char *reason)
*/
int position_estimator_inav_main(int argc, char *argv[])
{
- if (argc < 1)
+ if (argc < 1) {
usage("missing command");
+ }
if (!strcmp(argv[1], "start")) {
if (thread_running) {
@@ -125,8 +129,9 @@ int position_estimator_inav_main(int argc, char *argv[])
verbose_mode = false;
if (argc > 1)
- if (!strcmp(argv[2], "-v"))
+ if (!strcmp(argv[2], "-v")) {
verbose_mode = true;
+ }
thread_should_exit = false;
position_estimator_inav_task = task_spawn_cmd("position_estimator_inav",
@@ -163,8 +168,10 @@ int position_estimator_inav_main(int argc, char *argv[])
exit(1);
}
-void write_debug_log(const char *msg, float dt, float x_est[3], float y_est[3], float z_est[3], float corr_acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v) {
+void write_debug_log(const char *msg, float dt, float x_est[3], float y_est[3], float z_est[3], float corr_acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v)
+{
FILE *f = fopen("/fs/microsd/inav.log", "a");
+
if (f) {
char *s = malloc(256);
unsigned n = snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f %.5f] y_est=[%.5f %.5f %.5f] z_est=[%.5f %.5f %.5f]\n", hrt_absolute_time(), msg, dt, x_est[0], x_est[1], x_est[2], y_est[0], y_est[1], y_est[2], z_est[0], z_est[1], z_est[2]);
@@ -173,6 +180,7 @@ void write_debug_log(const char *msg, float dt, float x_est[3], float y_est[3],
fwrite(s, 1, n, f);
free(s);
}
+
fsync(fileno(f));
fclose(f);
}
@@ -206,6 +214,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
bool ref_inited = false;
hrt_abstime ref_init_start = 0;
const hrt_abstime ref_init_delay = 1000000; // wait for 1s after 3D fix
+ struct map_projection_reference_s ref;
+ memset(&ref, 0, sizeof(ref));
+ hrt_abstime home_timestamp = 0;
uint16_t accel_updates = 0;
uint16_t baro_updates = 0;
@@ -239,6 +250,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float w_flow = 0.0f;
float sonar_prev = 0.0f;
+ hrt_abstime flow_prev = 0; // time of last flow measurement
hrt_abstime sonar_time = 0; // time of last sonar measurement (not filtered)
hrt_abstime sonar_valid_time = 0; // time of last sonar measurement used for correction (filtered)
hrt_abstime xy_src_time = 0; // time of last available position data
@@ -257,6 +269,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
memset(&sensor, 0, sizeof(sensor));
struct vehicle_gps_position_s gps;
memset(&gps, 0, sizeof(gps));
+ struct home_position_s home;
+ memset(&home, 0, sizeof(home));
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
struct vehicle_local_position_s local_pos;
@@ -274,10 +288,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));
int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
+ int home_position_sub = orb_subscribe(ORB_ID(home_position));
/* advertise */
orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos);
- orb_advert_t vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);
+ orb_advert_t vehicle_global_position_pub = -1;
struct position_estimator_inav_params params;
struct position_estimator_inav_param_handles pos_inav_param_handles;
@@ -325,7 +340,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", baro_offset);
local_pos.z_valid = true;
local_pos.v_z_valid = true;
- global_pos.baro_valid = true;
}
}
}
@@ -425,6 +439,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (updated) {
orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow);
+ /* calculate time from previous update */
+ float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f;
+ flow_prev = flow.flow_timestamp;
+
if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && att.R[2][2] > 0.7 && flow.ground_distance_m != sonar_prev) {
sonar_time = t;
sonar_prev = flow.ground_distance_m;
@@ -475,10 +493,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
flow_accurate = fabsf(body_v_est[1] / flow_dist - att.rollspeed) < max_flow &&
fabsf(body_v_est[0] / flow_dist + att.pitchspeed) < max_flow;
- /* convert raw flow to angular flow */
+ /* convert raw flow to angular flow (rad/s) */
float flow_ang[2];
- flow_ang[0] = flow.flow_raw_x * params.flow_k;
- flow_ang[1] = flow.flow_raw_y * params.flow_k;
+ flow_ang[0] = flow.flow_raw_x * params.flow_k / 1000.0f / flow_dt;
+ flow_ang[1] = flow.flow_raw_y * params.flow_k / 1000.0f / flow_dt;
/* flow measurements vector */
float flow_m[3];
flow_m[0] = -flow_ang[0] * flow_dist;
@@ -503,8 +521,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* if flow is not accurate, reduce weight for it */
// TODO make this more fuzzy
- if (!flow_accurate)
+ if (!flow_accurate) {
w_flow *= 0.05f;
+ }
flow_valid = true;
@@ -516,29 +535,56 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
flow_updates++;
}
+ /* home position */
+ orb_check(home_position_sub, &updated);
+
+ if (updated) {
+ orb_copy(ORB_ID(home_position), home_position_sub, &home);
+
+ if (home.timestamp != home_timestamp) {
+ home_timestamp = home.timestamp;
+ if (ref_inited) {
+ ref_inited = true;
+
+ /* reproject position estimate to new reference */
+ float dx, dy;
+ map_projection_project(&ref, home.lat, home.lon, &dx, &dy);
+ x_est[0] -= dx;
+ y_est[0] -= dx;
+ z_est[0] += home.alt - local_pos.ref_alt;
+ }
+
+ /* update baro offset */
+ baro_offset -= home.alt - local_pos.ref_alt;
+
+ /* update reference */
+ map_projection_init(&ref, home.lat, home.lon);
+
+ local_pos.ref_lat = home.lat;
+ local_pos.ref_lon = home.lon;
+ local_pos.ref_alt = home.alt;
+ local_pos.ref_timestamp = home.timestamp;
+ }
+ }
+
/* vehicle GPS position */
orb_check(vehicle_gps_position_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps);
- if (gps.fix_type >= 3) {
- /* hysteresis for GPS quality */
- if (gps_valid) {
- if (gps.eph_m > 10.0f || gps.epv_m > 20.0f) {
- gps_valid = false;
- mavlink_log_info(mavlink_fd, "[inav] GPS signal lost");
- }
-
- } else {
- if (gps.eph_m < 5.0f && gps.epv_m < 10.0f) {
- gps_valid = true;
- mavlink_log_info(mavlink_fd, "[inav] GPS signal found");
- }
+ /* hysteresis for GPS quality */
+ if (gps_valid) {
+ if (gps.eph_m > 10.0f || gps.epv_m > 20.0f || gps.fix_type < 3) {
+ gps_valid = false;
+ mavlink_log_info(mavlink_fd, "[inav] GPS signal lost");
}
} else {
- gps_valid = false;
+ if (gps.eph_m < 5.0f && gps.epv_m < 10.0f && gps.fix_type >= 3) {
+ gps_valid = true;
+ mavlink_log_info(mavlink_fd, "[inav] GPS signal found");
+ }
}
if (gps_valid) {
@@ -554,13 +600,23 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
double lon = gps.lon * 1e-7;
float alt = gps.alt * 1e-3;
- local_pos.ref_lat = gps.lat;
- local_pos.ref_lon = gps.lon;
- local_pos.ref_alt = alt + z_est[0];
+ /* update baro offset */
+ baro_offset -= z_est[0];
+
+ /* set position estimate to (0, 0, 0), use GPS velocity for XY */
+ x_est[0] = 0.0f;
+ x_est[1] = gps.vel_n_m_s;
+ y_est[0] = 0.0f;
+ y_est[1] = gps.vel_e_m_s;
+ z_est[0] = 0.0f;
+
+ local_pos.ref_lat = lat;
+ local_pos.ref_lon = lon;
+ local_pos.ref_alt = alt;
local_pos.ref_timestamp = t;
/* initialize projection */
- map_projection_init(lat, lon);
+ map_projection_init(&ref, lat, lon);
warnx("init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt);
mavlink_log_info(mavlink_fd, "[inav] init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt);
}
@@ -569,7 +625,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (ref_inited) {
/* project GPS lat lon to plane */
float gps_proj[2];
- map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1]));
+ map_projection_project(&ref, gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1]));
/* calculate correction for position */
corr_gps[0][0] = gps_proj[0] - x_est[0];
corr_gps[1][0] = gps_proj[1] - y_est[0];
@@ -808,7 +864,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (t > pub_last + pub_interval) {
pub_last = t;
/* publish local position */
- local_pos.xy_valid = can_estimate_xy && use_gps_xy;
+ local_pos.xy_valid = can_estimate_xy;
local_pos.v_xy_valid = can_estimate_xy;
local_pos.xy_global = local_pos.xy_valid && use_gps_xy;
local_pos.z_global = local_pos.z_valid && use_gps_z;
@@ -831,40 +887,35 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos);
- /* publish global position */
- global_pos.global_valid = local_pos.xy_global;
+ if (local_pos.xy_global && local_pos.z_global) {
+ /* publish global position */
+ global_pos.timestamp = t;
+ global_pos.time_gps_usec = gps.time_gps_usec;
- if (local_pos.xy_global) {
double est_lat, est_lon;
- map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon);
+ map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon);
+
global_pos.lat = est_lat;
global_pos.lon = est_lon;
- global_pos.time_gps_usec = gps.time_gps_usec;
- }
+ global_pos.alt = local_pos.ref_alt - local_pos.z;
- /* set valid values even if position is not valid */
- if (local_pos.v_xy_valid) {
global_pos.vel_n = local_pos.vx;
global_pos.vel_e = local_pos.vy;
- }
-
- if (local_pos.z_global) {
- global_pos.alt = local_pos.ref_alt - local_pos.z;
- }
-
- if (local_pos.z_valid) {
- global_pos.baro_alt = baro_offset - local_pos.z;
- }
-
- if (local_pos.v_z_valid) {
global_pos.vel_d = local_pos.vz;
- }
- global_pos.yaw = local_pos.yaw;
+ global_pos.yaw = local_pos.yaw;
+
+ // TODO implement dead-reckoning
+ global_pos.eph = gps.eph_m;
+ global_pos.epv = gps.epv_m;
- global_pos.timestamp = t;
+ if (vehicle_global_position_pub < 0) {
+ vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);
- orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos);
+ } else {
+ orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos);
+ }
+ }
}
}
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c
index b71f9472f..6892ac496 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_params.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c
@@ -50,7 +50,7 @@ PARAM_DEFINE_FLOAT(INAV_W_XY_ACC, 20.0f);
PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f);
PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f);
PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.05f);
-PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.0165f);
+PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.15f);
PARAM_DEFINE_FLOAT(INAV_FLOW_Q_MIN, 0.5f);
PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.05f);
PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f);
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 13981ac54..bc8c0f0db 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1092,8 +1092,8 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_LPOS.vx = buf.local_pos.vx;
log_msg.body.log_LPOS.vy = buf.local_pos.vy;
log_msg.body.log_LPOS.vz = buf.local_pos.vz;
- log_msg.body.log_LPOS.ref_lat = buf.local_pos.ref_lat;
- log_msg.body.log_LPOS.ref_lon = buf.local_pos.ref_lon;
+ log_msg.body.log_LPOS.ref_lat = buf.local_pos.ref_lat * 1e7;
+ log_msg.body.log_LPOS.ref_lon = buf.local_pos.ref_lon * 1e7;
log_msg.body.log_LPOS.ref_alt = buf.local_pos.ref_alt;
log_msg.body.log_LPOS.xy_flags = (buf.local_pos.xy_valid ? 1 : 0) | (buf.local_pos.v_xy_valid ? 2 : 0) | (buf.local_pos.xy_global ? 8 : 0);
log_msg.body.log_LPOS.z_flags = (buf.local_pos.z_valid ? 1 : 0) | (buf.local_pos.v_z_valid ? 2 : 0) | (buf.local_pos.z_global ? 8 : 0);
@@ -1132,8 +1132,8 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_GPOS.vel_n = buf.global_pos.vel_n;
log_msg.body.log_GPOS.vel_e = buf.global_pos.vel_e;
log_msg.body.log_GPOS.vel_d = buf.global_pos.vel_d;
- log_msg.body.log_GPOS.baro_alt = buf.global_pos.baro_alt;
- log_msg.body.log_GPOS.flags = (buf.global_pos.baro_valid ? 1 : 0) | (buf.global_pos.global_valid ? 2 : 0);
+ log_msg.body.log_GPOS.eph = buf.global_pos.eph;
+ log_msg.body.log_GPOS.epv = buf.global_pos.epv;
LOGBUFFER_WRITE_AND_COUNT(GPOS);
}
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index 2b41755de..936f1c4d1 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -205,8 +205,8 @@ struct log_GPOS_s {
float vel_n;
float vel_e;
float vel_d;
- float baro_alt;
- uint8_t flags;
+ float eph;
+ float epv;
};
/* --- GPSP - GLOBAL POSITION SETPOINT --- */
@@ -328,7 +328,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"),
LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"),
LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"),
- LOG_FORMAT(GPOS, "LLfffffB", "Lat,Lon,Alt,VelN,VelE,VelD,BaroAlt,Flags"),
+ LOG_FORMAT(GPOS, "LLffffff", "Lat,Lon,Alt,VelN,VelE,VelD,EPH,EPV"),
LOG_FORMAT(GPSP, "BLLffBfbf", "NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"),
LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"),
diff --git a/src/modules/uORB/topics/optical_flow.h b/src/modules/uORB/topics/optical_flow.h
index 98f0e3fa2..0196ae86b 100644
--- a/src/modules/uORB/topics/optical_flow.h
+++ b/src/modules/uORB/topics/optical_flow.h
@@ -57,6 +57,7 @@ struct optical_flow_s {
uint64_t timestamp; /**< in microseconds since system start */
+ uint64_t flow_timestamp; /**< timestamp from flow sensor */
int16_t flow_raw_x; /**< flow in pixels in X direction, not rotation-compensated */
int16_t flow_raw_y; /**< flow in pixels in Y direction, not rotation-compensated */
float flow_comp_x_m; /**< speed over ground in meters, rotation-compensated */
diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h
index adcd028f0..4897ca737 100644
--- a/src/modules/uORB/topics/vehicle_global_position.h
+++ b/src/modules/uORB/topics/vehicle_global_position.h
@@ -63,9 +63,6 @@
struct vehicle_global_position_s {
uint64_t timestamp; /**< Time of this estimate, in microseconds since system start */
- bool global_valid; /**< true if position satisfies validity criteria of estimator */
- bool baro_valid; /**< true if baro_alt is valid (vel_d is also valid in this case) */
-
uint64_t time_gps_usec; /**< GPS timestamp in microseconds */
double lat; /**< Latitude in degrees */
double lon; /**< Longitude in degrees */
@@ -74,8 +71,8 @@ struct vehicle_global_position_s {
float vel_e; /**< Ground east velocity, m/s */
float vel_d; /**< Ground downside velocity, m/s */
float yaw; /**< Yaw in radians -PI..+PI. */
-
- float baro_alt; /**< Barometric altitude (not raw baro but fused with accelerometer) */
+ float eph;
+ float epv;
};
/**
diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h
index db9637cd9..a15303ea4 100644
--- a/src/modules/uORB/topics/vehicle_local_position.h
+++ b/src/modules/uORB/topics/vehicle_local_position.h
@@ -1,7 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
- * Author: @author Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -35,6 +34,9 @@
/**
* @file vehicle_local_position.h
* Definition of the local fused NED position uORB topic.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
*/
#ifndef TOPIC_VEHICLE_LOCAL_POSITION_H_
@@ -72,8 +74,8 @@ struct vehicle_local_position_s {
bool xy_global; /**< true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) */
bool z_global; /**< true if z is valid and has valid global reference (ref_alt) */
uint64_t ref_timestamp; /**< Time when reference position was set */
- int32_t ref_lat; /**< Reference point latitude in 1E7 degrees */
- int32_t ref_lon; /**< Reference point longitude in 1E7 degrees */
+ double ref_lat; /**< Reference point latitude 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 */
bool landed; /**< true if vehicle is landed */
/* Distance to surface */
generated by cgit v1.2.3 (git 2.39.1) at 2024-08-10 14:15:17 +0000