forked from Archive/PX4-Autopilot
geo std pressure constants and update usage
This commit is contained in:
Daniel Agar
Lorenz Meier
parent
02e319431b
commit
bd72f3c521
|
|
@ -228,7 +228,7 @@ struct parameters {
|
|||
float gyro_bias_p_noise{1.0e-3f}; ///< process noise for IMU rate gyro bias prediction (rad/sec**2)
|
||||
float accel_bias_p_noise{6.0e-3f}; ///< process noise for IMU accelerometer bias prediction (m/sec**3)
|
||||
float mage_p_noise{1.0e-3f}; ///< process noise for earth magnetic field prediction (Gauss/sec)
|
||||
float magb_p_noise{1.0e-4}; ///< process noise for body magnetic field prediction (Gauss/sec)
|
||||
float magb_p_noise{1.0e-4f}; ///< process noise for body magnetic field prediction (Gauss/sec)
|
||||
float wind_vel_p_noise{1.0e-1f}; ///< process noise for wind velocity prediction (m/sec**2)
|
||||
float terrain_p_noise{5.0f}; ///< process noise for terrain offset (m/sec)
|
||||
float terrain_gradient{0.5f}; ///< gradient of terrain used to estimate process noise due to changing position (m/m)
|
||||
|
|
|
|||
|
|
@ -114,7 +114,7 @@ float ECL_YawController::control_attitude_impl_openloop(const struct ECL_Control
|
|||
|
||||
if (!inverted) {
|
||||
/* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */
|
||||
_rate_setpoint = tanf(constrained_roll) * cosf(ctl_data.pitch) * 9.81f / (ctl_data.airspeed < ctl_data.airspeed_min ?
|
||||
_rate_setpoint = tanf(constrained_roll) * cosf(ctl_data.pitch) * CONSTANTS_ONE_G / (ctl_data.airspeed < ctl_data.airspeed_min ?
|
||||
ctl_data.airspeed_min : ctl_data.airspeed);
|
||||
}
|
||||
|
||||
|
|
|
|||
139
geo/geo.cpp
139
geo/geo.cpp
|
|
@ -131,27 +131,25 @@ int map_projection_project(const struct map_projection_reference_s *ref, double
|
|||
return -1;
|
||||
}
|
||||
|
||||
double lat_rad = math::radians(lat);
|
||||
double lon_rad = math::radians(lon);
|
||||
const double lat_rad = math::radians(lat);
|
||||
const double lon_rad = math::radians(lon);
|
||||
|
||||
double sin_lat = sin(lat_rad);
|
||||
double cos_lat = cos(lat_rad);
|
||||
double cos_d_lon = cos(lon_rad - ref->lon_rad);
|
||||
const double sin_lat = sin(lat_rad);
|
||||
const double cos_lat = cos(lat_rad);
|
||||
|
||||
double arg = ref->sin_lat * sin_lat + ref->cos_lat * cos_lat * cos_d_lon;
|
||||
const double cos_d_lon = cos(lon_rad - ref->lon_rad);
|
||||
|
||||
if (arg > 1.0) {
|
||||
arg = 1.0;
|
||||
const double arg = math::constrain(ref->sin_lat * sin_lat + ref->cos_lat * cos_lat * cos_d_lon, -1.0, 1.0);
|
||||
const double c = acos(arg);
|
||||
|
||||
} else if (arg < -1.0) {
|
||||
arg = -1.0;
|
||||
double k = 1.0;
|
||||
|
||||
if (fabs(c) >= DBL_EPSILON) {
|
||||
k = (c / sin(c));
|
||||
}
|
||||
|
||||
double c = acos(arg);
|
||||
double k = (fabs(c) < DBL_EPSILON) ? 1.0 : (c / sin(c));
|
||||
|
||||
*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_rad) * CONSTANTS_RADIUS_OF_EARTH;
|
||||
*x = static_cast<float>(k * (ref->cos_lat * sin_lat - ref->sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH);
|
||||
*y = static_cast<float>(k * cos_lat * sin(lon_rad - ref->lon_rad) * CONSTANTS_RADIUS_OF_EARTH);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -167,27 +165,25 @@ int map_projection_reproject(const struct map_projection_reference_s *ref, float
|
|||
return -1;
|
||||
}
|
||||
|
||||
double x_rad = (double)x / CONSTANTS_RADIUS_OF_EARTH;
|
||||
double y_rad = (double)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_rad;
|
||||
double lon_rad;
|
||||
const double x_rad = (double)x / CONSTANTS_RADIUS_OF_EARTH;
|
||||
const double y_rad = (double)y / CONSTANTS_RADIUS_OF_EARTH;
|
||||
const double c = sqrt(x_rad * x_rad + y_rad * y_rad);
|
||||
|
||||
if (fabs(c) > DBL_EPSILON) {
|
||||
lat_rad = asin(cos_c * ref->sin_lat + (x_rad * sin_c * ref->cos_lat) / c);
|
||||
lon_rad = (ref->lon_rad + atan2(y_rad * sin_c, c * ref->cos_lat * cos_c - x_rad * ref->sin_lat * sin_c));
|
||||
const double sin_c = sin(c);
|
||||
const double cos_c = cos(c);
|
||||
|
||||
const double lat_rad = asin(cos_c * ref->sin_lat + (x_rad * sin_c * ref->cos_lat) / c);
|
||||
const double lon_rad = (ref->lon_rad + atan2(y_rad * sin_c, c * ref->cos_lat * cos_c - x_rad * ref->sin_lat * sin_c));
|
||||
|
||||
*lat = math::degrees(lat_rad);
|
||||
*lon = math::degrees(lon_rad);
|
||||
|
||||
} else {
|
||||
lat_rad = ref->lat_rad;
|
||||
lon_rad = ref->lon_rad;
|
||||
*lat = math::degrees(ref->lat_rad);
|
||||
*lon = math::degrees(ref->lon_rad);
|
||||
}
|
||||
|
||||
*lat = lat_rad * 180.0 / M_PI;
|
||||
*lon = lon_rad * 180.0 / M_PI;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
|
@ -212,14 +208,13 @@ int globallocalconverter_init(double lat_0, double lon_0, float alt_0, uint64_t
|
|||
{
|
||||
gl_ref.alt = alt_0;
|
||||
|
||||
if (!map_projection_global_init(lat_0, lon_0, timestamp)) {
|
||||
if (map_projection_global_init(lat_0, lon_0, timestamp) != 0) {
|
||||
gl_ref.init_done = true;
|
||||
return 0;
|
||||
|
||||
} else {
|
||||
gl_ref.init_done = false;
|
||||
return -1;
|
||||
}
|
||||
|
||||
gl_ref.init_done = false;
|
||||
return -1;
|
||||
}
|
||||
|
||||
bool globallocalconverter_initialized()
|
||||
|
|
@ -253,7 +248,7 @@ int globallocalconverter_toglobal(float x, float y, float z, double *lat, doubl
|
|||
|
||||
int globallocalconverter_getref(double *lat_0, double *lon_0, float *alt_0)
|
||||
{
|
||||
if (!map_projection_global_initialized()) {
|
||||
if (map_projection_global_initialized() != 0) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
|
|
@ -270,24 +265,21 @@ int globallocalconverter_getref(double *lat_0, double *lon_0, float *alt_0)
|
|||
|
||||
float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
|
||||
{
|
||||
double lat_now_rad = lat_now / (double)180.0 * M_PI;
|
||||
double lon_now_rad = lon_now / (double)180.0 * M_PI;
|
||||
double lat_next_rad = lat_next / (double)180.0 * M_PI;
|
||||
double lon_next_rad = lon_next / (double)180.0 * M_PI;
|
||||
const double lat_now_rad = math::radians(lat_now);
|
||||
const double lat_next_rad = math::radians(lat_next);
|
||||
|
||||
const double d_lat = lat_next_rad - lat_now_rad;
|
||||
const double d_lon = math::radians(lon_next) - math::radians(lon_now);
|
||||
|
||||
double d_lat = lat_next_rad - lat_now_rad;
|
||||
double d_lon = lon_next_rad - lon_now_rad;
|
||||
const double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0) * cos(lat_now_rad) * cos(lat_next_rad);
|
||||
|
||||
double a = sin(d_lat / (double)2.0) * sin(d_lat / (double)2.0) + sin(d_lon / (double)2.0) * sin(d_lon /
|
||||
(double)2.0) * cos(lat_now_rad) * cos(lat_next_rad);
|
||||
double c = (double)2.0 * atan2(sqrt(a), sqrt((double)1.0 - a));
|
||||
const double c = atan2(sqrt(a), sqrt(1.0 - a));
|
||||
|
||||
return CONSTANTS_RADIUS_OF_EARTH * c;
|
||||
return static_cast<float>(CONSTANTS_RADIUS_OF_EARTH * 2.0 * c);
|
||||
}
|
||||
|
||||
void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B, double lon_B, float dist,
|
||||
double *lat_target, double *lon_target)
|
||||
double *lat_target, double *lon_target)
|
||||
{
|
||||
if (fabsf(dist) < FLT_EPSILON) {
|
||||
*lat_target = lat_A;
|
||||
|
|
@ -305,10 +297,10 @@ void create_waypoint_from_line_and_dist(double lat_A, double lon_A, double lat_B
|
|||
}
|
||||
|
||||
void waypoint_from_heading_and_distance(double lat_start, double lon_start, float bearing, float dist,
|
||||
double *lat_target, double *lon_target)
|
||||
double *lat_target, double *lon_target)
|
||||
{
|
||||
bearing = _wrap_2pi(bearing);
|
||||
double radius_ratio = fabs((double)dist) / CONSTANTS_RADIUS_OF_EARTH;
|
||||
double radius_ratio = (double)fabs((double)dist) / CONSTANTS_RADIUS_OF_EARTH;
|
||||
|
||||
double lat_start_rad = math::radians(lat_start);
|
||||
double lon_start_rad = math::radians(lon_start);
|
||||
|
|
@ -331,28 +323,23 @@ float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_ne
|
|||
double d_lon = lon_next_rad - lon_now_rad;
|
||||
|
||||
/* conscious mix of double and float trig function to maximize speed and efficiency */
|
||||
float theta = atan2f(sin(d_lon) * cos(lat_next_rad),
|
||||
cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon));
|
||||
|
||||
theta = _wrap_pi(theta);
|
||||
const float y = static_cast<float>(sin(d_lon) * cos(lat_next_rad));
|
||||
const float x = static_cast<float>(cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon));
|
||||
|
||||
return theta;
|
||||
return _wrap_pi(atan2f(y, x));
|
||||
}
|
||||
|
||||
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 = math::radians(lat_now);
|
||||
double lon_now_rad = math::radians(lon_now);
|
||||
double lat_next_rad = math::radians(lat_next);
|
||||
double lon_next_rad = math::radians(lon_next);
|
||||
|
||||
double d_lon = lon_next_rad - lon_now_rad;
|
||||
const double lat_now_rad = math::radians(lat_now);
|
||||
const double lat_next_rad = math::radians(lat_next);
|
||||
const double d_lon = math::radians(lon_next) - math::radians(lon_now);
|
||||
|
||||
/* conscious mix of double and float trig function to maximize speed and efficiency */
|
||||
*v_n = CONSTANTS_RADIUS_OF_EARTH * (cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(
|
||||
d_lon));
|
||||
*v_e = CONSTANTS_RADIUS_OF_EARTH * sin(d_lon) * cos(lat_next_rad);
|
||||
*v_n = static_cast<float>(CONSTANTS_RADIUS_OF_EARTH * (cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon)));
|
||||
*v_e = static_cast<float>(CONSTANTS_RADIUS_OF_EARTH * sin(d_lon) * cos(lat_next_rad));
|
||||
}
|
||||
|
||||
void
|
||||
|
|
@ -367,8 +354,8 @@ get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next
|
|||
double d_lon = lon_next_rad - lon_now_rad;
|
||||
|
||||
/* conscious mix of double and float trig function to maximize speed and efficiency */
|
||||
*v_n = CONSTANTS_RADIUS_OF_EARTH * d_lat;
|
||||
*v_e = CONSTANTS_RADIUS_OF_EARTH * d_lon * cos(lat_now_rad);
|
||||
*v_n = static_cast<float>(CONSTANTS_RADIUS_OF_EARTH * d_lat);
|
||||
*v_e = static_cast<float>(CONSTANTS_RADIUS_OF_EARTH * d_lon * cos(lat_now_rad));
|
||||
}
|
||||
|
||||
void add_vector_to_global_position(double lat_now, double lon_now, float v_n, float v_e, double *lat_res, double *lon_res)
|
||||
|
|
@ -383,7 +370,7 @@ void add_vector_to_global_position(double lat_now, double lon_now, float v_n, fl
|
|||
// Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu>
|
||||
|
||||
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)
|
||||
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
|
||||
|
|
@ -427,8 +414,8 @@ int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, double lat
|
|||
}
|
||||
|
||||
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)
|
||||
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
|
||||
|
|
@ -436,10 +423,9 @@ int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, double lat_
|
|||
|
||||
// Determine if the current position is inside or outside the sector between the line from the center
|
||||
// to the arc start and the line from the center to the arc end
|
||||
float bearing_sector_start;
|
||||
float bearing_sector_end;
|
||||
float bearing_sector_start = 0.0f;
|
||||
float bearing_sector_end = 0.0f;
|
||||
float bearing_now = get_bearing_to_next_waypoint(lat_now, lon_now, lat_center, lon_center);
|
||||
bool in_sector;
|
||||
|
||||
int return_value = -1; // Set error flag, cleared when valid result calculated.
|
||||
crosstrack_error->past_end = false;
|
||||
|
|
@ -464,7 +450,7 @@ int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, double lat_
|
|||
if (bearing_sector_start < 0.0f) { bearing_sector_start += (2 * M_PI_F); }
|
||||
}
|
||||
|
||||
in_sector = false;
|
||||
bool in_sector = false;
|
||||
|
||||
// Case where sector does not span zero
|
||||
if (bearing_sector_end >= bearing_sector_start && bearing_now >= bearing_sector_start
|
||||
|
|
@ -505,8 +491,8 @@ int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, double lat_
|
|||
|
||||
double start_disp_x = (double)radius * sin((double)arc_start_bearing);
|
||||
double start_disp_y = (double)radius * cos((double)arc_start_bearing);
|
||||
double end_disp_x = (double)radius * sin((double)_wrap_pi((double)(arc_start_bearing + arc_sweep)));
|
||||
double end_disp_y = (double)radius * cos((double)_wrap_pi((double)(arc_start_bearing + arc_sweep)));
|
||||
double end_disp_x = (double)radius * sin((double)_wrap_pi(arc_start_bearing + arc_sweep));
|
||||
double end_disp_y = (double)radius * cos((double)_wrap_pi(arc_start_bearing + arc_sweep));
|
||||
double lon_start = lon_now + start_disp_x / 111111.0;
|
||||
double lat_start = lat_now + start_disp_y * cos(lat_now) / 111111.0;
|
||||
double lon_end = lon_now + end_disp_x / 111111.0;
|
||||
|
|
@ -514,7 +500,6 @@ int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, double lat_
|
|||
double dist_to_start = get_distance_to_next_waypoint(lat_now, lon_now, lat_start, lon_start);
|
||||
double dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
|
||||
|
||||
|
||||
if (dist_to_start < dist_to_end) {
|
||||
crosstrack_error->distance = dist_to_start;
|
||||
crosstrack_error->bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_start, lon_start);
|
||||
|
|
@ -526,7 +511,7 @@ int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, double lat_
|
|||
}
|
||||
}
|
||||
|
||||
crosstrack_error->bearing = _wrap_pi((double)crosstrack_error->bearing);
|
||||
crosstrack_error->bearing = _wrap_pi(crosstrack_error->bearing);
|
||||
return_value = 0;
|
||||
return return_value;
|
||||
}
|
||||
|
|
@ -546,8 +531,8 @@ float get_distance_to_point_global_wgs84(double lat_now, double lon_now, float a
|
|||
double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0) * cos(current_x_rad) * cos(x_rad);
|
||||
double c = 2 * atan2(sqrt(a), sqrt(1 - a));
|
||||
|
||||
float dxy = CONSTANTS_RADIUS_OF_EARTH * c;
|
||||
float dz = alt_now - alt_next;
|
||||
const float dxy = static_cast<float>(CONSTANTS_RADIUS_OF_EARTH * c);
|
||||
const float dz = static_cast<float>(alt_now - alt_next);
|
||||
|
||||
*dist_xy = fabsf(dxy);
|
||||
*dist_z = fabsf(dz);
|
||||
|
|
|
|||
18
geo/geo.h
18
geo/geo.h
|
|
@ -48,11 +48,19 @@
|
|||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#define CONSTANTS_ONE_G 9.80665f /* m/s^2 */
|
||||
#define CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C 1.225f /* kg/m^3 */
|
||||
#define CONSTANTS_AIR_GAS_CONST 287.1f /* J/(kg * K) */
|
||||
#define CONSTANTS_ABSOLUTE_NULL_CELSIUS -273.15f /* °C */
|
||||
#define CONSTANTS_RADIUS_OF_EARTH 6371000 /* meters (m) */
|
||||
static constexpr float CONSTANTS_ONE_G = 9.80665f; // m/s^2
|
||||
|
||||
static constexpr float CONSTANTS_STD_PRESSURE_PA = 101325.0f; // pascals (Pa)
|
||||
static constexpr float CONSTANTS_STD_PRESSURE_KPA = CONSTANTS_STD_PRESSURE_PA / 1000.0f; // kilopascals (kPa)
|
||||
static constexpr float CONSTANTS_STD_PRESSURE_MBAR = CONSTANTS_STD_PRESSURE_PA / 100.0f; // Millibar (mbar) (1 mbar = 100 Pa)
|
||||
|
||||
static constexpr float CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C = 1.225f; // kg/m^3
|
||||
static constexpr float CONSTANTS_AIR_GAS_CONST = 287.1f; // J/(kg * K)
|
||||
static constexpr float CONSTANTS_ABSOLUTE_NULL_CELSIUS = -273.15f; // °C
|
||||
|
||||
static constexpr double CONSTANTS_RADIUS_OF_EARTH = 6371000; // meters (m)
|
||||
static constexpr float CONSTANTS_RADIUS_OF_EARTH_F = CONSTANTS_RADIUS_OF_EARTH; // meters (m)
|
||||
|
||||
|
||||
// XXX remove
|
||||
struct crosstrack_error_s {
|
||||
|
|
|
|||
|
|
@ -124,7 +124,7 @@ get_lookup_table_index(float *val, float min, float max)
|
|||
*val = max - SAMPLING_RES;
|
||||
}
|
||||
|
||||
return (-(min) + *val) / SAMPLING_RES;
|
||||
return static_cast<unsigned>((-(min) + *val) / SAMPLING_RES);
|
||||
}
|
||||
|
||||
static float
|
||||
|
|
|
|||
|
|
@ -67,8 +67,7 @@ ECL_L1_Pos_Controller::navigate_waypoints(const Vector2f &vector_A, const Vector
|
|||
float ltrack_vel = 0.0f;
|
||||
|
||||
/* get the direction between the last (visited) and next waypoint */
|
||||
_target_bearing = get_bearing_to_next_waypoint(vector_curr_position(0), vector_curr_position(1), vector_B(0),
|
||||
vector_B(1));
|
||||
_target_bearing = get_bearing_to_next_waypoint((double)vector_curr_position(0), (double)vector_curr_position(1), (double)vector_B(0), (double)vector_B(1));
|
||||
|
||||
/* enforce a minimum ground speed of 0.1 m/s to avoid singularities */
|
||||
float ground_speed = math::max(ground_speed_vector.length(), 0.1f);
|
||||
|
|
@ -196,8 +195,7 @@ ECL_L1_Pos_Controller::navigate_loiter(const Vector2f &vector_A, const Vector2f
|
|||
float K_velocity = 2.0f * _L1_damping * omega;
|
||||
|
||||
/* update bearing to next waypoint */
|
||||
_target_bearing = get_bearing_to_next_waypoint(vector_curr_position(0), vector_curr_position(1), vector_A(0),
|
||||
vector_A(1));
|
||||
_target_bearing = get_bearing_to_next_waypoint((double)vector_curr_position(0), (double)vector_curr_position(1), (double)vector_A(0), (double)vector_A(1));
|
||||
|
||||
/* ground speed, enforce minimum of 0.1 m/s to avoid singularities */
|
||||
float ground_speed = math::max(ground_speed_vector.length(), 0.1f);
|
||||
|
|
|
|||
Loading…
Reference in New Issue