aboutsummaryrefslogtreecommitdiff
path: root/src/lib
diff options
context:
space:
mode:
Diffstat (limited to 'src/lib')
-rw-r--r--src/lib/geo/geo.c215
-rw-r--r--src/lib/geo/geo.h40
-rw-r--r--src/lib/geo/geo_mag_declination.c136
-rw-r--r--src/lib/geo/geo_mag_declination.h47
-rw-r--r--src/lib/geo/module.mk3
-rw-r--r--src/lib/launchdetection/CatapultLaunchMethod.cpp28
-rw-r--r--src/lib/launchdetection/CatapultLaunchMethod.h17
-rw-r--r--src/lib/launchdetection/LaunchDetector.cpp25
-rw-r--r--src/lib/launchdetection/LaunchDetector.h15
-rw-r--r--src/lib/launchdetection/LaunchMethod.h13
-rw-r--r--src/lib/launchdetection/launchdetection_params.c5
-rw-r--r--src/lib/launchdetection/module.mk2
-rw-r--r--src/lib/mathlib/CMSIS/Include/arm_math.h2
-rw-r--r--src/lib/mathlib/math/filter/LowPassFilter2p.cpp7
-rw-r--r--src/lib/mathlib/math/filter/LowPassFilter2p.hpp20
-rw-r--r--src/lib/version/version.h4
16 files changed, 374 insertions, 205 deletions
diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c
index 9b3e202e6..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;
+ double lat_rad;
+ double lon_rad;
- 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));
-
- } 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;
}
@@ -197,7 +129,6 @@ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, doub
double lat_next_rad = lat_next * M_DEG_TO_RAD;
double lon_next_rad = lon_next * M_DEG_TO_RAD;
- double d_lat = lat_next_rad - lat_now_rad;
double d_lon = lon_next_rad - lon_now_rad;
/* conscious mix of double and float trig function to maximize speed and efficiency */
@@ -208,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;
@@ -223,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;
@@ -249,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
@@ -266,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);
@@ -297,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
@@ -317,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) {
@@ -395,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;
@@ -420,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;
@@ -441,17 +372,21 @@ __EXPORT float _wrap_pi(float bearing)
}
int c = 0;
- while (bearing > M_PI_F) {
+ while (bearing >= M_PI_F) {
bearing -= M_TWOPI_F;
- if (c++ > 3)
+
+ if (c++ > 3) {
return NAN;
+ }
}
c = 0;
- while (bearing <= -M_PI_F) {
+ while (bearing < -M_PI_F) {
bearing += M_TWOPI_F;
- if (c++ > 3)
+
+ if (c++ > 3) {
return NAN;
+ }
}
return bearing;
@@ -465,17 +400,21 @@ __EXPORT float _wrap_2pi(float bearing)
}
int c = 0;
- while (bearing > M_TWOPI_F) {
+ while (bearing >= M_TWOPI_F) {
bearing -= M_TWOPI_F;
- if (c++ > 3)
+
+ if (c++ > 3) {
return NAN;
+ }
}
c = 0;
- while (bearing <= 0.0f) {
+ while (bearing < 0.0f) {
bearing += M_TWOPI_F;
- if (c++ > 3)
+
+ if (c++ > 3) {
return NAN;
+ }
}
return bearing;
@@ -489,17 +428,21 @@ __EXPORT float _wrap_180(float bearing)
}
int c = 0;
- while (bearing > 180.0f) {
+ while (bearing >= 180.0f) {
bearing -= 360.0f;
- if (c++ > 3)
+
+ if (c++ > 3) {
return NAN;
+ }
}
c = 0;
- while (bearing <= -180.0f) {
+ while (bearing < -180.0f) {
bearing += 360.0f;
- if (c++ > 3)
+
+ if (c++ > 3) {
return NAN;
+ }
}
return bearing;
@@ -513,17 +456,21 @@ __EXPORT float _wrap_360(float bearing)
}
int c = 0;
- while (bearing > 360.0f) {
+ while (bearing >= 360.0f) {
bearing -= 360.0f;
- if (c++ > 3)
+
+ if (c++ > 3) {
return NAN;
+ }
}
c = 0;
- while (bearing <= 0.0f) {
+ 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..e2f3da6f8 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>
*/
@@ -52,6 +50,8 @@
__BEGIN_DECLS
+#include "geo/geo_mag_declination.h"
+
#include <stdbool.h>
#define CONSTANTS_ONE_G 9.80665f /* m/s^2 */
@@ -67,6 +67,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 +82,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 +91,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 +101,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 +123,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/lib/geo/geo_mag_declination.c b/src/lib/geo/geo_mag_declination.c
new file mode 100644
index 000000000..09eac38f4
--- /dev/null
+++ b/src/lib/geo/geo_mag_declination.c
@@ -0,0 +1,136 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2014 MAV GEO Library (MAVGEO). All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name MAVGEO nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+* @file geo_mag_declination.c
+*
+* Calculation / lookup table for earth magnetic field declination.
+*
+* Lookup table from Scott Ferguson <scottfromscott@gmail.com>
+*
+* XXX Lookup table currently too coarse in resolution (only full degrees)
+* and lat/lon res - needs extension medium term.
+*
+*/
+
+#include <geo/geo.h>
+
+/** set this always to the sampling in degrees for the table below */
+#define SAMPLING_RES 10.0f
+#define SAMPLING_MIN_LAT -60.0f
+#define SAMPLING_MAX_LAT 60.0f
+#define SAMPLING_MIN_LON -180.0f
+#define SAMPLING_MAX_LON 180.0f
+
+static const int8_t declination_table[13][37] = \
+{
+ 46, 45, 44, 42, 41, 40, 38, 36, 33, 28, 23, 16, 10, 4, -1, -5, -9, -14, -19, -26, -33, -40, -48, -55, -61, \
+ -66, -71, -74, -75, -72, -61, -25, 22, 40, 45, 47, 46, 30, 30, 30, 30, 29, 29, 29, 29, 27, 24, 18, 11, 3, \
+ -3, -9, -12, -15, -17, -21, -26, -32, -39, -45, -51, -55, -57, -56, -53, -44, -31, -14, 0, 13, 21, 26, \
+ 29, 30, 21, 22, 22, 22, 22, 22, 22, 22, 21, 18, 13, 5, -3, -11, -17, -20, -21, -22, -23, -25, -29, -35, \
+ -40, -44, -45, -44, -40, -32, -22, -12, -3, 3, 9, 14, 18, 20, 21, 16, 17, 17, 17, 17, 17, 16, 16, 16, 13, \
+ 8, 0, -9, -16, -21, -24, -25, -25, -23, -20, -21, -24, -28, -31, -31, -29, -24, -17, -9, -3, 0, 4, 7, \
+ 10, 13, 15, 16, 12, 13, 13, 13, 13, 13, 12, 12, 11, 9, 3, -4, -12, -19, -23, -24, -24, -22, -17, -12, -9, \
+ -10, -13, -17, -18, -16, -13, -8, -3, 0, 1, 3, 6, 8, 10, 12, 12, 10, 10, 10, 10, 10, 10, 10, 9, 9, 6, 0, -6, \
+ -14, -20, -22, -22, -19, -15, -10, -6, -2, -2, -4, -7, -8, -8, -7, -4, 0, 1, 1, 2, 4, 6, 8, 10, 10, 9, 9, 9, \
+ 9, 9, 9, 8, 8, 7, 4, -1, -8, -15, -19, -20, -18, -14, -9, -5, -2, 0, 1, 0, -2, -3, -4, -3, -2, 0, 0, 0, 1, 3, 5, \
+ 7, 8, 9, 8, 8, 8, 9, 9, 9, 8, 8, 6, 2, -3, -9, -15, -18, -17, -14, -10, -6, -2, 0, 1, 2, 2, 0, -1, -1, -2, -1, 0, \
+ 0, 0, 0, 1, 3, 5, 7, 8, 8, 9, 9, 10, 10, 10, 10, 8, 5, 0, -5, -11, -15, -16, -15, -12, -8, -4, -1, 0, 2, 3, 2, 1, 0, \
+ 0, 0, 0, 0, -1, -2, -2, -1, 0, 3, 6, 8, 6, 9, 10, 11, 12, 12, 11, 9, 5, 0, -7, -12, -15, -15, -13, -10, -7, -3, \
+ 0, 1, 2, 3, 3, 3, 2, 1, 0, 0, -1, -3, -4, -5, -5, -2, 0, 3, 6, 5, 8, 11, 13, 15, 15, 14, 11, 5, -1, -9, -14, -17, \
+ -16, -14, -11, -7, -3, 0, 1, 3, 4, 5, 5, 5, 4, 3, 1, -1, -4, -7, -8, -8, -6, -2, 1, 5, 4, 8, 12, 15, 17, 18, 16, \
+ 12, 5, -3, -12, -18, -20, -19, -16, -13, -8, -4, -1, 1, 4, 6, 8, 9, 9, 9, 7, 3, -1, -6, -10, -12, -11, -9, -5, \
+ 0, 4, 3, 9, 14, 17, 20, 21, 19, 14, 4, -8, -19, -25, -26, -25, -21, -17, -12, -7, -2, 1, 5, 9, 13, 15, 16, 16, \
+ 13, 7, 0, -7, -12, -15, -14, -11, -6, -1, 3
+};
+
+static float get_lookup_table_val(unsigned lat, unsigned lon);
+
+__EXPORT float get_mag_declination(float lat, float lon)
+{
+ /*
+ * If the values exceed valid ranges, return zero as default
+ * as we have no way of knowing what the closest real value
+ * would be.
+ */
+ if (lat < -90.0f || lat > 90.0f ||
+ lon < -180.0f || lon > 180.0f) {
+ return 0.0f;
+ }
+
+ /* round down to nearest sampling resolution */
+ int min_lat = (int)(lat / SAMPLING_RES) * SAMPLING_RES;
+ int min_lon = (int)(lon / SAMPLING_RES) * SAMPLING_RES;
+
+ /* for the rare case of hitting the bounds exactly
+ * the rounding logic wouldn't fit, so enforce it.
+ */
+
+ /* limit to table bounds - required for maxima even when table spans full globe range */
+ if (lat <= SAMPLING_MIN_LAT) {
+ min_lat = SAMPLING_MIN_LAT;
+ }
+
+ if (lat >= SAMPLING_MAX_LAT) {
+ min_lat = (int)(lat / SAMPLING_RES) * SAMPLING_RES - SAMPLING_RES;
+ }
+
+ if (lon <= SAMPLING_MIN_LON) {
+ min_lon = SAMPLING_MIN_LON;
+ }
+
+ if (lon >= SAMPLING_MAX_LON) {
+ min_lon = (int)(lon / SAMPLING_RES) * SAMPLING_RES - SAMPLING_RES;
+ }
+
+ /* find index of nearest low sampling point */
+ unsigned min_lat_index = (-(SAMPLING_MIN_LAT) + min_lat) / SAMPLING_RES;
+ unsigned min_lon_index = (-(SAMPLING_MIN_LON) + min_lon) / SAMPLING_RES;
+
+ float declination_sw = get_lookup_table_val(min_lat_index, min_lon_index);
+ float declination_se = get_lookup_table_val(min_lat_index, min_lon_index + 1);
+ float declination_ne = get_lookup_table_val(min_lat_index + 1, min_lon_index + 1);
+ float declination_nw = get_lookup_table_val(min_lat_index + 1, min_lon_index);
+
+ /* perform bilinear interpolation on the four grid corners */
+
+ float declination_min = ((lon - min_lon) / SAMPLING_RES) * (declination_se - declination_sw) + declination_sw;
+ float declination_max = ((lon - min_lon) / SAMPLING_RES) * (declination_ne - declination_nw) + declination_nw;
+
+ return ((lat - min_lat) / SAMPLING_RES) * (declination_max - declination_min) + declination_min;
+}
+
+float get_lookup_table_val(unsigned lat_index, unsigned lon_index)
+{
+ return declination_table[lat_index][lon_index];
+} \ No newline at end of file
diff --git a/src/lib/geo/geo_mag_declination.h b/src/lib/geo/geo_mag_declination.h
new file mode 100644
index 000000000..0ac062d6d
--- /dev/null
+++ b/src/lib/geo/geo_mag_declination.h
@@ -0,0 +1,47 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2014 MAV GEO Library (MAVGEO). All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name MAVGEO nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+* @file geo_mag_declination.h
+*
+* Calculation / lookup table for earth magnetic field declination.
+*
+*/
+
+#pragma once
+
+__BEGIN_DECLS
+
+__EXPORT float get_mag_declination(float lat, float lon);
+
+__END_DECLS
diff --git a/src/lib/geo/module.mk b/src/lib/geo/module.mk
index 30a2dc99f..9500a2bcc 100644
--- a/src/lib/geo/module.mk
+++ b/src/lib/geo/module.mk
@@ -35,4 +35,5 @@
# Geo library
#
-SRCS = geo.c
+SRCS = geo.c \
+ geo_mag_declination.c
diff --git a/src/lib/launchdetection/CatapultLaunchMethod.cpp b/src/lib/launchdetection/CatapultLaunchMethod.cpp
index 1039b4a09..c555a0a69 100644
--- a/src/lib/launchdetection/CatapultLaunchMethod.cpp
+++ b/src/lib/launchdetection/CatapultLaunchMethod.cpp
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
+ * 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
@@ -10,9 +10,9 @@
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
- * the documentation4 and/or other materials provided with the
+ * the documentation and/or other materials provided with the
* distribution.
- * 3. Neither the name ECL nor the names of its contributors may be
+ * 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
@@ -33,20 +33,25 @@
/**
* @file CatapultLaunchMethod.cpp
- * Catpult Launch detection
+ * Catapult Launch detection
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
*
- * Authors and acknowledgements in header.
*/
#include "CatapultLaunchMethod.h"
#include <systemlib/err.h>
-CatapultLaunchMethod::CatapultLaunchMethod() :
+namespace launchdetection
+{
+
+CatapultLaunchMethod::CatapultLaunchMethod(SuperBlock *parent) :
+ SuperBlock(parent, "CAT"),
last_timestamp(hrt_absolute_time()),
integrator(0.0f),
launchDetected(false),
- threshold_accel(NULL, "LAUN_CAT_A", false),
- threshold_time(NULL, "LAUN_CAT_T", false)
+ threshold_accel(this, "A"),
+ threshold_time(this, "T")
{
}
@@ -83,14 +88,11 @@ bool CatapultLaunchMethod::getLaunchDetected()
return launchDetected;
}
-void CatapultLaunchMethod::updateParams()
-{
- threshold_accel.update();
- threshold_time.update();
-}
void CatapultLaunchMethod::reset()
{
integrator = 0.0f;
launchDetected = false;
}
+
+}
diff --git a/src/lib/launchdetection/CatapultLaunchMethod.h b/src/lib/launchdetection/CatapultLaunchMethod.h
index b8476b4c8..23757f6f3 100644
--- a/src/lib/launchdetection/CatapultLaunchMethod.h
+++ b/src/lib/launchdetection/CatapultLaunchMethod.h
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
+ * 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
@@ -10,9 +10,9 @@
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
- * the documentation4 and/or other materials provided with the
+ * the documentation and/or other materials provided with the
* distribution.
- * 3. Neither the name ECL nor the names of its contributors may be
+ * 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
@@ -44,17 +44,20 @@
#include "LaunchMethod.h"
#include <drivers/drv_hrt.h>
+#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
-class CatapultLaunchMethod : public LaunchMethod
+namespace launchdetection
+{
+
+class CatapultLaunchMethod : public LaunchMethod, public control::SuperBlock
{
public:
- CatapultLaunchMethod();
+ CatapultLaunchMethod(SuperBlock *parent);
~CatapultLaunchMethod();
void update(float accel_x);
bool getLaunchDetected();
- void updateParams();
void reset();
private:
@@ -68,3 +71,5 @@ private:
};
#endif /* CATAPULTLAUNCHMETHOD_H_ */
+
+}
diff --git a/src/lib/launchdetection/LaunchDetector.cpp b/src/lib/launchdetection/LaunchDetector.cpp
index 4109a90ba..3bf47bbb0 100644
--- a/src/lib/launchdetection/LaunchDetector.cpp
+++ b/src/lib/launchdetection/LaunchDetector.cpp
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
+ * 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
@@ -12,7 +12,7 @@
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
- * 3. Neither the name ECL nor the names of its contributors may be
+ * 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
@@ -30,24 +30,27 @@
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
-
/**
* @file launchDetection.cpp
* Auto Detection for different launch methods (e.g. catapult)
*
- * Authors and acknowledgements in header.
+ * @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include "LaunchDetector.h"
#include "CatapultLaunchMethod.h"
#include <systemlib/err.h>
+namespace launchdetection
+{
+
LaunchDetector::LaunchDetector() :
- launchdetection_on(NULL, "LAUN_ALL_ON", false),
- throttlePreTakeoff(NULL, "LAUN_THR_PRE", false)
+ SuperBlock(NULL, "LAUN"),
+ launchdetection_on(this, "ALL_ON"),
+ throttlePreTakeoff(this, "THR_PRE")
{
/* init all detectors */
- launchMethods[0] = new CatapultLaunchMethod();
+ launchMethods[0] = new CatapultLaunchMethod(this);
/* update all parameters of all detectors */
@@ -87,12 +90,4 @@ bool LaunchDetector::getLaunchDetected()
return false;
}
-void LaunchDetector::updateParams() {
-
- launchdetection_on.update();
- throttlePreTakeoff.update();
-
- for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) {
- launchMethods[i]->updateParams();
- }
}
diff --git a/src/lib/launchdetection/LaunchDetector.h b/src/lib/launchdetection/LaunchDetector.h
index 05708c526..1a214b66e 100644
--- a/src/lib/launchdetection/LaunchDetector.h
+++ b/src/lib/launchdetection/LaunchDetector.h
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
+ * 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
@@ -10,9 +10,9 @@
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
- * the documentation4 and/or other materials provided with the
+ * the documentation and/or other materials provided with the
* distribution.
- * 3. Neither the name ECL nor the names of its contributors may be
+ * 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
@@ -45,10 +45,13 @@
#include <stdint.h>
#include "LaunchMethod.h"
-
+#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
-class __EXPORT LaunchDetector
+namespace launchdetection
+{
+
+class __EXPORT LaunchDetector : public control::SuperBlock
{
public:
LaunchDetector();
@@ -57,7 +60,6 @@ public:
void update(float accel_x);
bool getLaunchDetected();
- void updateParams();
bool launchDetectionEnabled() { return (bool)launchdetection_on.get(); };
float getThrottlePreTakeoff() {return throttlePreTakeoff.get(); }
@@ -72,5 +74,6 @@ private:
};
+}
#endif // LAUNCHDETECTOR_H
diff --git a/src/lib/launchdetection/LaunchMethod.h b/src/lib/launchdetection/LaunchMethod.h
index 0cfbab3e0..e04467f6a 100644
--- a/src/lib/launchdetection/LaunchMethod.h
+++ b/src/lib/launchdetection/LaunchMethod.h
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
+ * 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
@@ -10,9 +10,9 @@
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
- * the documentation4 and/or other materials provided with the
+ * the documentation and/or other materials provided with the
* distribution.
- * 3. Neither the name ECL nor the names of its contributors may be
+ * 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
@@ -41,15 +41,20 @@
#ifndef LAUNCHMETHOD_H_
#define LAUNCHMETHOD_H_
+namespace launchdetection
+{
+
class LaunchMethod
{
public:
virtual void update(float accel_x) = 0;
virtual bool getLaunchDetected() = 0;
- virtual void updateParams() = 0;
virtual void reset() = 0;
+
protected:
private:
};
+}
+
#endif /* LAUNCHMETHOD_H_ */
diff --git a/src/lib/launchdetection/launchdetection_params.c b/src/lib/launchdetection/launchdetection_params.c
index 45d7957f1..8df8c696c 100644
--- a/src/lib/launchdetection/launchdetection_params.c
+++ b/src/lib/launchdetection/launchdetection_params.c
@@ -1,7 +1,6 @@
/****************************************************************************
*
- * Copyright (c) 2013 PX4 Development Team. All rights reserved.
- * Author: Lorenz Meier <lm@inf.ethz.ch>
+ * 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
@@ -18,7 +17,7 @@
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * AS IS AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
diff --git a/src/lib/launchdetection/module.mk b/src/lib/launchdetection/module.mk
index 13648b74c..de0174e37 100644
--- a/src/lib/launchdetection/module.mk
+++ b/src/lib/launchdetection/module.mk
@@ -1,6 +1,6 @@
############################################################################
#
-# Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
+# Copyright (c) 2012, 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
diff --git a/src/lib/mathlib/CMSIS/Include/arm_math.h b/src/lib/mathlib/CMSIS/Include/arm_math.h
index 6f66f9ee3..61d3a3b61 100644
--- a/src/lib/mathlib/CMSIS/Include/arm_math.h
+++ b/src/lib/mathlib/CMSIS/Include/arm_math.h
@@ -5193,7 +5193,7 @@ void arm_rfft_fast_f32(
*pIa = Ialpha;
/* Calculating pIb from Ialpha and Ibeta by equation pIb = -(1/2) * Ialpha + (sqrt(3)/2) * Ibeta */
- *pIb = -0.5 * Ialpha + (float32_t) 0.8660254039 *Ibeta;
+ *pIb = (float32_t)-0.5 * Ialpha + (float32_t) 0.8660254039 *Ibeta;
}
diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp
index 3699d9bce..6f640c9f9 100644
--- a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp
+++ b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp
@@ -69,7 +69,7 @@ float LowPassFilter2p::apply(float sample)
// do the filtering
float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2;
if (isnan(delay_element_0) || isinf(delay_element_0)) {
- // don't allow bad values to propogate via the filter
+ // don't allow bad values to propagate via the filter
delay_element_0 = sample;
}
float output = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2;
@@ -81,5 +81,10 @@ float LowPassFilter2p::apply(float sample)
return output;
}
+float LowPassFilter2p::reset(float sample) {
+ _delay_element_1 = _delay_element_2 = sample;
+ return apply(sample);
+}
+
} // namespace math
diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.hpp b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp
index 208ec98d4..74cd5d78c 100644
--- a/src/lib/mathlib/math/filter/LowPassFilter2p.hpp
+++ b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp
@@ -52,18 +52,30 @@ public:
_delay_element_1 = _delay_element_2 = 0;
}
- // change parameters
+ /**
+ * Change filter parameters
+ */
void set_cutoff_frequency(float sample_freq, float cutoff_freq);
- // apply - Add a new raw value to the filter
- // and retrieve the filtered result
+ /**
+ * Add a new raw value to the filter
+ *
+ * @return retrieve the filtered result
+ */
float apply(float sample);
- // return the cutoff frequency
+ /**
+ * Return the cutoff frequency
+ */
float get_cutoff_freq(void) const {
return _cutoff_freq;
}
+ /**
+ * Reset the filter state to this value
+ */
+ float reset(float sample);
+
private:
float _cutoff_freq;
float _a1;
diff --git a/src/lib/version/version.h b/src/lib/version/version.h
index af733aaf0..d8ccb6774 100644
--- a/src/lib/version/version.h
+++ b/src/lib/version/version.h
@@ -59,4 +59,8 @@
#define HW_ARCH "PX4FMU_V2"
#endif
+#ifdef CONFIG_ARCH_BOARD_AEROCORE
+#define HW_ARCH "AEROCORE"
+#endif
+
#endif /* VERSION_H_ */
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-16 16:23:52 +0000