mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-31 02:16:53 +08:00
Expand standalone mathlib double precision functions' usage
This commit is contained in:
+4
-4
@@ -524,10 +524,10 @@ float get_distance_to_point_global_wgs84(double lat_now, double lon_now, float a
|
|||||||
double lat_next, double lon_next, float alt_next,
|
double lat_next, double lon_next, float alt_next,
|
||||||
float *dist_xy, float *dist_z)
|
float *dist_xy, float *dist_z)
|
||||||
{
|
{
|
||||||
double current_x_rad = lat_next / 180.0 * M_PI;
|
double current_x_rad = math::radians(lat_next);
|
||||||
double current_y_rad = lon_next / 180.0 * M_PI;
|
double current_y_rad = math::radians(lon_next);
|
||||||
double x_rad = lat_now / 180.0 * M_PI;
|
double x_rad = math::radians(lat_now);
|
||||||
double y_rad = lon_now / 180.0 * M_PI;
|
double y_rad = math::radians(lon_now);
|
||||||
|
|
||||||
double d_lat = x_rad - current_x_rad;
|
double d_lat = x_rad - current_x_rad;
|
||||||
double d_lon = y_rad - current_y_rad;
|
double d_lon = y_rad - current_y_rad;
|
||||||
|
|||||||
+15
-5
@@ -57,19 +57,29 @@
|
|||||||
|
|
||||||
namespace math {
|
namespace math {
|
||||||
template <typename Type>
|
template <typename Type>
|
||||||
Type min(Type val1, Type val2) { return (val1 < val2) ? val1 : val2; }
|
Type min(Type val1, Type val2) {
|
||||||
|
return (val1 < val2) ? val1 : val2;
|
||||||
|
}
|
||||||
|
|
||||||
template <typename Type>
|
template <typename Type>
|
||||||
Type max(Type val1, Type val2) { return (val1 > val2) ? val1 : val2; }
|
Type max(Type val1, Type val2) {
|
||||||
|
return (val1 > val2) ? val1 : val2;
|
||||||
|
}
|
||||||
|
|
||||||
template <typename Type>
|
template <typename Type>
|
||||||
Type constrain(Type val, Type min, Type max) { return (val < min) ? min : ((val > max) ? max : val); }
|
Type constrain(Type val, Type min, Type max) {
|
||||||
|
return (val < min) ? min : ((val > max) ? max : val);
|
||||||
|
}
|
||||||
|
|
||||||
template <typename Type>
|
template <typename Type>
|
||||||
Type radians(Type degrees) { return (degrees / Type(180)) * Type(M_PI); }
|
Type radians(Type degrees) {
|
||||||
|
return (degrees / Type(180)) * Type(M_PI);
|
||||||
|
}
|
||||||
|
|
||||||
template <typename Type>
|
template <typename Type>
|
||||||
Type degrees(Type radians) { return (radians * Type(180)) / Type(M_PI); }
|
Type degrees(Type radians) {
|
||||||
|
return (radians * Type(180)) / Type(M_PI);
|
||||||
|
}
|
||||||
|
|
||||||
} // namespace math
|
} // namespace math
|
||||||
#else
|
#else
|
||||||
|
|||||||
Reference in New Issue
Block a user