forked from Archive/PX4-Autopilot
geo: Moved the map_projection_* functions and state into a self-contained C++ class
This commit is contained in:
parent
fe23718e2c
commit
8db7a6225b
|
@ -60,61 +60,30 @@ using matrix::wrap_2pi;
|
|||
* formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html
|
||||
*/
|
||||
|
||||
bool map_projection_initialized(const map_projection_reference_s *ref)
|
||||
|
||||
void MapProjection::initReference(double lat_0, double lon_0, uint64_t timestamp)
|
||||
{
|
||||
return ref->init_done;
|
||||
_ref_lat = math::radians(lat_0);
|
||||
_ref_lon = math::radians(lon_0);
|
||||
_ref_sin_lat = sin(_ref_lat);
|
||||
_ref_cos_lat = cos(_ref_lat);
|
||||
|
||||
_ref_timestamp = timestamp;
|
||||
_ref_init_done = true;
|
||||
}
|
||||
|
||||
uint64_t map_projection_timestamp(const map_projection_reference_s *ref)
|
||||
|
||||
void MapProjection::project(double lat, double lon, float &x, float &y) const
|
||||
{
|
||||
return ref->timestamp;
|
||||
}
|
||||
|
||||
int map_projection_init_timestamped(map_projection_reference_s *ref, double lat_0, double lon_0, uint64_t timestamp)
|
||||
{
|
||||
ref->lat_rad = math::radians(lat_0);
|
||||
ref->lon_rad = math::radians(lon_0);
|
||||
ref->sin_lat = sin(ref->lat_rad);
|
||||
ref->cos_lat = cos(ref->lat_rad);
|
||||
|
||||
ref->timestamp = timestamp;
|
||||
ref->init_done = true;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int map_projection_init(map_projection_reference_s *ref, double lat_0, double lon_0)
|
||||
{
|
||||
return map_projection_init_timestamped(ref, lat_0, lon_0, hrt_absolute_time());
|
||||
}
|
||||
|
||||
int map_projection_reference(const map_projection_reference_s *ref, double *ref_lat_rad, double *ref_lon_rad)
|
||||
{
|
||||
if (!map_projection_initialized(ref)) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
*ref_lat_rad = ref->lat_rad;
|
||||
*ref_lon_rad = ref->lon_rad;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int map_projection_project(const map_projection_reference_s *ref, double lat, double lon, float *x, float *y)
|
||||
{
|
||||
if (!map_projection_initialized(ref)) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
const double lat_rad = math::radians(lat);
|
||||
const double lon_rad = math::radians(lon);
|
||||
|
||||
const double sin_lat = sin(lat_rad);
|
||||
const double cos_lat = cos(lat_rad);
|
||||
|
||||
const double cos_d_lon = cos(lon_rad - ref->lon_rad);
|
||||
const double cos_d_lon = cos(lon_rad - _ref_lon);
|
||||
|
||||
const double arg = math::constrain(ref->sin_lat * sin_lat + ref->cos_lat * cos_lat * cos_d_lon, -1.0, 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);
|
||||
|
||||
double k = 1.0;
|
||||
|
@ -123,18 +92,12 @@ int map_projection_project(const map_projection_reference_s *ref, double lat, do
|
|||
k = (c / sin(c));
|
||||
}
|
||||
|
||||
*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;
|
||||
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) * CONSTANTS_RADIUS_OF_EARTH);
|
||||
}
|
||||
|
||||
int map_projection_reproject(const map_projection_reference_s *ref, float x, float y, double *lat, double *lon)
|
||||
void MapProjection::reproject(float x, float y, double &lat, double &lon) const
|
||||
{
|
||||
if (!map_projection_initialized(ref)) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
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);
|
||||
|
@ -143,18 +106,17 @@ int map_projection_reproject(const map_projection_reference_s *ref, float x, flo
|
|||
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));
|
||||
const double lat_rad = asin(cos_c * _ref_sin_lat + (x_rad * sin_c * _ref_cos_lat) / c);
|
||||
const double lon_rad = (_ref_lon + 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);
|
||||
lat = math::degrees(lat_rad);
|
||||
lon = math::degrees(lon_rad);
|
||||
|
||||
} else {
|
||||
*lat = math::degrees(ref->lat_rad);
|
||||
*lon = math::degrees(ref->lon_rad);
|
||||
lat = math::degrees(_ref_lat);
|
||||
lon = math::degrees(_ref_lon);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
|
||||
|
|
|
@ -48,6 +48,10 @@
|
|||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include <matrix/Matrix.hpp>
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
static constexpr float CONSTANTS_ONE_G = 9.80665f; // m/s^2
|
||||
|
||||
static constexpr float CONSTANTS_STD_PRESSURE_PA = 101325.0f; // pascals (Pa)
|
||||
|
@ -72,76 +76,6 @@ 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 {
|
||||
uint64_t timestamp;
|
||||
double lat_rad;
|
||||
double lon_rad;
|
||||
double sin_lat;
|
||||
double cos_lat;
|
||||
bool init_done;
|
||||
};
|
||||
|
||||
/**
|
||||
* Checks if projection given as argument was initialized
|
||||
* @return true if map was initialized before, false else
|
||||
*/
|
||||
bool map_projection_initialized(const struct map_projection_reference_s *ref);
|
||||
|
||||
/**
|
||||
* Get the timestamp of the map projection given by the argument
|
||||
* @return the timestamp of the map_projection
|
||||
*/
|
||||
uint64_t map_projection_timestamp(const struct map_projection_reference_s *ref);
|
||||
|
||||
/**
|
||||
* Writes the reference values of the projection given by the argument to ref_lat and ref_lon
|
||||
* @return 0 if map_projection_init was called before, -1 else
|
||||
*/
|
||||
int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, double *ref_lon_rad);
|
||||
|
||||
/**
|
||||
* Initializes the map transformation given by the argument.
|
||||
*
|
||||
* Initializes the transformation between the geographic coordinate system and
|
||||
* the azimuthal equidistant plane
|
||||
* @param lat in degrees (47.1234567°, not 471234567°)
|
||||
* @param lon in degrees (8.1234567°, not 81234567°)
|
||||
*/
|
||||
int map_projection_init_timestamped(struct map_projection_reference_s *ref, double lat_0, double lon_0,
|
||||
uint64_t timestamp);
|
||||
|
||||
/**
|
||||
* Initializes the map transformation given by the argument and sets the timestamp to now.
|
||||
*
|
||||
* Initializes the transformation between the geographic coordinate system and
|
||||
* the azimuthal equidistant plane
|
||||
* @param lat in degrees (47.1234567°, not 471234567°)
|
||||
* @param lon in degrees (8.1234567°, not 81234567°)
|
||||
*/
|
||||
int 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 using the projection given by the argument
|
||||
* @param x north
|
||||
* @param y east
|
||||
* @param lat in degrees (47.1234567°, not 471234567°)
|
||||
* @param lon in degrees (8.1234567°, not 81234567°)
|
||||
* @return 0 if map_projection_init was called before, -1 else
|
||||
*/
|
||||
int map_projection_project(const 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 using the projection given by the argument
|
||||
*
|
||||
* @param x north
|
||||
* @param y east
|
||||
* @param lat in degrees (47.1234567°, not 471234567°)
|
||||
* @param lon in degrees (8.1234567°, not 81234567°)
|
||||
* @return 0 if map_projection_init was called before, -1 else
|
||||
*/
|
||||
int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon);
|
||||
|
||||
/**
|
||||
* Returns the distance to the next waypoint in meters.
|
||||
|
@ -221,3 +155,134 @@ float get_distance_to_point_global_wgs84(double lat_now, double lon_now, float a
|
|||
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);
|
||||
|
||||
|
||||
/**
|
||||
* @brief C++ class for mapping lat/lon coordinates to local coordinated using a reference position
|
||||
*/
|
||||
class MapProjection final
|
||||
{
|
||||
private:
|
||||
uint64_t _ref_timestamp{0};
|
||||
double _ref_lat{0.f};
|
||||
double _ref_lon{0.f};
|
||||
double _ref_sin_lat{0.f};
|
||||
double _ref_cos_lat{0.f};
|
||||
bool _ref_init_done {false};
|
||||
|
||||
public:
|
||||
/**
|
||||
* @brief Construct a new Map Projection object
|
||||
* The generated object will be uninitialized.
|
||||
* To initialize, use the `initReference` function
|
||||
*/
|
||||
MapProjection() = default;
|
||||
|
||||
/**
|
||||
* @brief Construct and initializes a new Map Projection object
|
||||
*/
|
||||
MapProjection(double lat_0, double lon_0)
|
||||
{
|
||||
initReference(lat_0, lon_0);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Construct and initializes a new Map Projection object
|
||||
*/
|
||||
MapProjection(double lat_0, double lon_0, uint64_t timestamp)
|
||||
{
|
||||
initReference(lat_0, lon_0, timestamp);
|
||||
}
|
||||
|
||||
/**
|
||||
* Initializes the map transformation
|
||||
*
|
||||
* Initializes the transformation between the geographic coordinate system and
|
||||
* the azimuthal equidistant plane
|
||||
* @param lat in degrees (47.1234567°, not 471234567°)
|
||||
* @param lon in degrees (8.1234567°, not 81234567°)
|
||||
*/
|
||||
void initReference(double lat_0, double lon_0, uint64_t timestamp);
|
||||
|
||||
/**
|
||||
* Initializes the map transformation
|
||||
*
|
||||
* Initializes the transformation between the geographic coordinate system and
|
||||
* the azimuthal equidistant plane
|
||||
* @param lat in degrees (47.1234567°, not 471234567°)
|
||||
* @param lon in degrees (8.1234567°, not 81234567°)
|
||||
*/
|
||||
inline void initReference(double lat_0, double lon_0)
|
||||
{
|
||||
initReference(lat_0, lon_0, hrt_absolute_time());
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Returns true, if the map reference has been initialized before
|
||||
*/
|
||||
inline bool isInitialized() const
|
||||
{
|
||||
return _ref_init_done;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the timestamp of the map projection
|
||||
* @return the timestamp of the map_projection
|
||||
*/
|
||||
inline uint64_t getProjectionReferenceTimestamp() const
|
||||
{
|
||||
return _ref_timestamp;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Get the Projection Reference latitude in degrees
|
||||
*/
|
||||
inline double getProjectionReferenceLat() const
|
||||
{
|
||||
return math::degrees(_ref_lat);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Get the Projection Reference longitude in degrees
|
||||
*/
|
||||
inline double getProjectionReferenceLon() const
|
||||
{
|
||||
return math::degrees(_ref_lon);
|
||||
}
|
||||
|
||||
/**
|
||||
* Transforms a point in the geographic coordinate system to the local
|
||||
* azimuthal equidistant plane using the projection
|
||||
* @param x north
|
||||
* @param y east
|
||||
* @param lat in degrees (47.1234567°, not 471234567°)
|
||||
* @param lon in degrees (8.1234567°, not 81234567°)
|
||||
*/
|
||||
void project(double lat, double lon, float &x, float &y) const;
|
||||
|
||||
/**
|
||||
* Transforms a point in the geographic coordinate system to the local
|
||||
* azimuthal equidistant plane using the projection
|
||||
* @param lat in degrees (47.1234567°, not 471234567°)
|
||||
* @param lon in degrees (8.1234567°, not 81234567°)
|
||||
* @return the point in local coordinates as north / east
|
||||
*/
|
||||
inline matrix::Vector2f project(double lat, double lon) const
|
||||
{
|
||||
matrix::Vector2f res;
|
||||
project(lat, lon, res(0), res(1));
|
||||
return res;
|
||||
}
|
||||
|
||||
/**
|
||||
* Transforms a point in the local azimuthal equidistant plane to the
|
||||
* geographic coordinate system using the projection
|
||||
*
|
||||
* @param x north
|
||||
* @param y east
|
||||
* @param lat in degrees (47.1234567°, not 471234567°)
|
||||
* @param lon in degrees (8.1234567°, not 81234567°)
|
||||
* @return 0 if map_projection_init was called before, -1 else
|
||||
*/
|
||||
void reproject(float x, float y, double &lat, double &lon) const;
|
||||
};
|
||||
|
|
|
@ -42,16 +42,11 @@ class GeoTest : public ::testing::Test
|
|||
public:
|
||||
void SetUp() override
|
||||
{
|
||||
origin.timestamp = 0;
|
||||
origin.lat_rad = math::radians(473566094 / 1e7);
|
||||
origin.lon_rad = math::radians(85190237 / 1e7);
|
||||
origin.sin_lat = sin(origin.lat_rad);
|
||||
origin.cos_lat = cos(origin.lat_rad);
|
||||
origin.init_done = true;
|
||||
proj.initReference(math::radians(473566094 / 1e7), math::radians(85190237 / 1e7), 0);
|
||||
}
|
||||
|
||||
protected:
|
||||
map_projection_reference_s origin;
|
||||
MapProjection proj;
|
||||
|
||||
};
|
||||
|
||||
|
@ -63,13 +58,13 @@ TEST_F(GeoTest, reprojectProject)
|
|||
float y = 1;
|
||||
double lat;
|
||||
double lon;
|
||||
map_projection_reproject(&origin, x, y, &lat, &lon);
|
||||
proj.reproject(x, y, lat, lon);
|
||||
float x_new;
|
||||
float y_new;
|
||||
map_projection_project(&origin, lat, lon, &x_new, &y_new);
|
||||
proj.project(lat, lon, x_new, y_new);
|
||||
double lat_new;
|
||||
double lon_new;
|
||||
map_projection_reproject(&origin, x_new, y_new, &lat_new, &lon_new);
|
||||
proj.reproject(x_new, y_new, lat_new, lon_new);
|
||||
EXPECT_FLOAT_EQ(x, x_new);
|
||||
EXPECT_FLOAT_EQ(y, y_new);
|
||||
EXPECT_FLOAT_EQ(lat, lat_new);
|
||||
|
@ -83,13 +78,13 @@ TEST_F(GeoTest, projectReproject)
|
|||
double lon = 8.5190505981445313;
|
||||
float x;
|
||||
float y;
|
||||
map_projection_project(&origin, lat, lon, &x, &y);
|
||||
proj.project(lat, lon, x, y);
|
||||
double lat_new;
|
||||
double lon_new;
|
||||
map_projection_reproject(&origin, x, y, &lat_new, &lon_new);
|
||||
proj.reproject(x, y, lat_new, lon_new);
|
||||
float x_new;
|
||||
float y_new;
|
||||
map_projection_project(&origin, lat_new, lon_new, &x_new, &y_new);
|
||||
proj.project(lat_new, lon_new, x_new, y_new);
|
||||
// WHEN: apply the mapping and its inverse the output should stay the same
|
||||
EXPECT_FLOAT_EQ(x, x_new);
|
||||
EXPECT_FLOAT_EQ(y, y_new);
|
||||
|
|
|
@ -979,11 +979,10 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
|||
home.manual_home = true;
|
||||
|
||||
// update local projection reference including altitude
|
||||
struct map_projection_reference_s ref_pos;
|
||||
map_projection_init(&ref_pos, local_pos.ref_lat, local_pos.ref_lon);
|
||||
MapProjection ref_pos{local_pos.ref_lat, local_pos.ref_lon};
|
||||
float home_x;
|
||||
float home_y;
|
||||
map_projection_project(&ref_pos, lat, lon, &home_x, &home_y);
|
||||
ref_pos.project(lat, lon, home_x, home_y);
|
||||
const float home_z = -(alt - local_pos.ref_alt);
|
||||
fillLocalHomePos(home, home_x, home_y, home_z, yaw);
|
||||
|
||||
|
@ -1688,11 +1687,10 @@ Commander::set_in_air_home_position()
|
|||
if (_home_pub.get().valid_lpos) {
|
||||
// Back-compute lon, lat and alt of home position given the home
|
||||
// and current positions in local frame
|
||||
map_projection_reference_s ref_pos;
|
||||
MapProjection ref_pos{gpos.lat, gpos.lon};
|
||||
double home_lat;
|
||||
double home_lon;
|
||||
map_projection_init(&ref_pos, gpos.lat, gpos.lon);
|
||||
map_projection_reproject(&ref_pos, home.x - lpos.x, home.y - lpos.y, &home_lat, &home_lon);
|
||||
ref_pos.reproject(home.x - lpos.x, home.y - lpos.y, home_lat, home_lon);
|
||||
const float home_alt = gpos.alt + home.z;
|
||||
fillGlobalHomePos(home, home_lat, home_lon, home_alt);
|
||||
|
||||
|
|
|
@ -700,8 +700,8 @@ matrix::Vector<float, 24> Ekf::getStateAtFusionHorizonAsVector() const
|
|||
bool Ekf::getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const
|
||||
{
|
||||
origin_time = _last_gps_origin_time_us;
|
||||
latitude = math::degrees(_pos_ref.lat_rad);
|
||||
longitude = math::degrees(_pos_ref.lon_rad);
|
||||
latitude = _pos_ref.getProjectionReferenceLat();
|
||||
longitude = _pos_ref.getProjectionReferenceLon();
|
||||
origin_alt = _gps_alt_ref;
|
||||
return _NED_origin_initialised;
|
||||
}
|
||||
|
@ -714,18 +714,17 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons
|
|||
float current_alt = 0.f;
|
||||
|
||||
// if we are already doing aiding, correct for the change in position since the EKF started navigating
|
||||
if (map_projection_initialized(&_pos_ref) && isHorizontalAidingActive()) {
|
||||
map_projection_reproject(&_pos_ref, _state.pos(0), _state.pos(1), ¤t_lat, ¤t_lon);
|
||||
if (_pos_ref.isInitialized() && isHorizontalAidingActive()) {
|
||||
_pos_ref.reproject(_state.pos(0), _state.pos(1), current_lat, current_lon);
|
||||
current_alt = -_state.pos(2) + _gps_alt_ref;
|
||||
current_pos_available = true;
|
||||
}
|
||||
|
||||
// reinitialize map projection to latitude, longitude, altitude, and reset position
|
||||
if (map_projection_init_timestamped(&_pos_ref, latitude, longitude, _time_last_imu) == 0) {
|
||||
_pos_ref.initReference(latitude, longitude, _time_last_imu);
|
||||
if (current_pos_available) {
|
||||
// reset horizontal position
|
||||
Vector2f position;
|
||||
map_projection_project(&_pos_ref, current_lat, current_lon, &position(0), &position(1));
|
||||
Vector2f position = _pos_ref.project(current_lat, current_lon);
|
||||
resetHorizontalPositionTo(position);
|
||||
|
||||
// reset altitude
|
||||
|
@ -738,9 +737,6 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons
|
|||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
|
@ -197,7 +197,7 @@ void EstimatorInterface::setGpsData(const gps_message &gps)
|
|||
if (collect_gps(gps)) {
|
||||
float lpos_x = 0.0f;
|
||||
float lpos_y = 0.0f;
|
||||
map_projection_project(&_pos_ref, (gps.lat / 1.0e7), (gps.lon / 1.0e7), &lpos_x, &lpos_y);
|
||||
_pos_ref.project((gps.lat / 1.0e7), (gps.lon / 1.0e7), lpos_x, lpos_y);
|
||||
gps_sample_new.pos(0) = lpos_x;
|
||||
gps_sample_new.pos(1) = lpos_y;
|
||||
|
||||
|
|
|
@ -253,7 +253,7 @@ public:
|
|||
const baroSample &get_baro_sample_delayed() const { return _baro_sample_delayed; }
|
||||
|
||||
const bool &global_origin_valid() const { return _NED_origin_initialised; }
|
||||
const map_projection_reference_s &global_origin() const { return _pos_ref; }
|
||||
const MapProjection &global_origin() const { return _pos_ref; }
|
||||
|
||||
void print_status();
|
||||
|
||||
|
@ -324,8 +324,8 @@ protected:
|
|||
bool _gps_speed_valid{false};
|
||||
float _gps_origin_eph{0.0f}; // horizontal position uncertainty of the GPS origin
|
||||
float _gps_origin_epv{0.0f}; // vertical position uncertainty of the GPS origin
|
||||
struct map_projection_reference_s _pos_ref {}; // Contains WGS-84 position latitude and longitude (radians) of the EKF origin
|
||||
struct map_projection_reference_s _gps_pos_prev {}; // Contains WGS-84 position latitude and longitude (radians) of the previous GPS message
|
||||
MapProjection _pos_ref {}; // Contains WGS-84 position latitude and longitude (radians) of the EKF origin
|
||||
MapProjection _gps_pos_prev {}; // Contains WGS-84 position latitude and longitude (radians) of the previous GPS message
|
||||
float _gps_alt_prev{0.0f}; // height from the previous GPS message (m)
|
||||
float _gps_yaw_offset{0.0f}; // Yaw offset angle for dual GPS antennas used for yaw estimation (radians).
|
||||
|
||||
|
|
|
@ -65,22 +65,23 @@ bool Ekf::collect_gps(const gps_message &gps)
|
|||
const double lat = gps.lat * 1.0e-7;
|
||||
const double lon = gps.lon * 1.0e-7;
|
||||
|
||||
if (!map_projection_initialized(&_pos_ref)) {
|
||||
map_projection_init_timestamped(&_pos_ref, lat, lon, _time_last_imu);
|
||||
if (!_pos_ref.isInitialized()) {
|
||||
_pos_ref.initReference(lat, lon, _time_last_imu);
|
||||
|
||||
// if we are already doing aiding, correct for the change in position since the EKF started navigating
|
||||
if (isHorizontalAidingActive()) {
|
||||
double est_lat;
|
||||
double est_lon;
|
||||
map_projection_reproject(&_pos_ref, -_state.pos(0), -_state.pos(1), &est_lat, &est_lon);
|
||||
map_projection_init_timestamped(&_pos_ref, est_lat, est_lon, _time_last_imu);
|
||||
_pos_ref.reproject(-_state.pos(0), -_state.pos(1), est_lat, est_lon);
|
||||
_pos_ref.initReference(est_lat, est_lon, _time_last_imu);
|
||||
}
|
||||
}
|
||||
|
||||
// Take the current GPS height and subtract the filter height above origin to estimate the GPS height of the origin
|
||||
_gps_alt_ref = 1e-3f * (float)gps.alt + _state.pos(2);
|
||||
_NED_origin_initialised = true;
|
||||
_earth_rate_NED = calcEarthRateNED((float)_pos_ref.lat_rad);
|
||||
|
||||
_earth_rate_NED = calcEarthRateNED((float)math::radians(_pos_ref.getProjectionReferenceLat()));
|
||||
_last_gps_origin_time_us = _time_last_imu;
|
||||
|
||||
const bool declination_was_valid = PX4_ISFINITE(_mag_declination_gps);
|
||||
|
@ -169,7 +170,7 @@ bool Ekf::gps_is_good(const gps_message &gps)
|
|||
|
||||
// Calculate time lapsed since last update, limit to prevent numerical errors and calculate a lowpass filter coefficient
|
||||
constexpr float filt_time_const = 10.0f;
|
||||
const float dt = math::constrain(float(int64_t(_time_last_imu) - int64_t(_gps_pos_prev.timestamp)) * 1e-6f, 0.001f, filt_time_const);
|
||||
const float dt = math::constrain(float(int64_t(_time_last_imu) - int64_t(_gps_pos_prev.getProjectionReferenceTimestamp())) * 1e-6f, 0.001f, filt_time_const);
|
||||
const float filter_coef = dt / filt_time_const;
|
||||
|
||||
// The following checks are only valid when the vehicle is at rest
|
||||
|
@ -182,12 +183,12 @@ bool Ekf::gps_is_good(const gps_message &gps)
|
|||
float delta_pos_e = 0.0f;
|
||||
|
||||
// calculate position movement since last GPS fix
|
||||
if (_gps_pos_prev.timestamp > 0) {
|
||||
map_projection_project(&_gps_pos_prev, lat, lon, &delta_pos_n, &delta_pos_e);
|
||||
if (_gps_pos_prev.getProjectionReferenceTimestamp() > 0) {
|
||||
_gps_pos_prev.project(lat, lon, delta_pos_n, delta_pos_e);
|
||||
|
||||
} else {
|
||||
// no previous position has been set
|
||||
map_projection_init_timestamped(&_gps_pos_prev, lat, lon, _time_last_imu);
|
||||
_gps_pos_prev.initReference(lat, lon, _time_last_imu);
|
||||
_gps_alt_prev = 1e-3f * (float)gps.alt;
|
||||
}
|
||||
|
||||
|
@ -235,7 +236,7 @@ bool Ekf::gps_is_good(const gps_message &gps)
|
|||
}
|
||||
|
||||
// save GPS fix for next time
|
||||
map_projection_init_timestamped(&_gps_pos_prev, lat, lon, _time_last_imu);
|
||||
_gps_pos_prev.initReference(lat, lon, _time_last_imu);
|
||||
_gps_alt_prev = 1e-3f * (float)gps.alt;
|
||||
|
||||
// Check the filtered difference between GPS and EKF vertical velocity
|
||||
|
|
|
@ -716,7 +716,7 @@ void EKF2::PublishGlobalPosition(const hrt_abstime ×tamp)
|
|||
global_pos.timestamp_sample = timestamp;
|
||||
|
||||
// Position of local NED origin in GPS / WGS84 frame
|
||||
map_projection_reproject(&_ekf.global_origin(), position(0), position(1), &global_pos.lat, &global_pos.lon);
|
||||
_ekf.global_origin().reproject(position(0), position(1), global_pos.lat, global_pos.lon);
|
||||
|
||||
float delta_xy[2];
|
||||
_ekf.get_posNE_reset(delta_xy, &global_pos.lat_lon_reset_counter);
|
||||
|
@ -867,9 +867,9 @@ void EKF2::PublishLocalPosition(const hrt_abstime ×tamp)
|
|||
|
||||
// Position of local NED origin in GPS / WGS84 frame
|
||||
if (_ekf.global_origin_valid()) {
|
||||
lpos.ref_timestamp = _ekf.global_origin().timestamp;
|
||||
lpos.ref_lat = math::degrees(_ekf.global_origin().lat_rad); // Reference point latitude in degrees
|
||||
lpos.ref_lon = math::degrees(_ekf.global_origin().lon_rad); // Reference point longitude in degrees
|
||||
lpos.ref_timestamp = _ekf.global_origin().getProjectionReferenceTimestamp();
|
||||
lpos.ref_lat = _ekf.global_origin().getProjectionReferenceLat(); // Reference point latitude in degrees
|
||||
lpos.ref_lon = _ekf.global_origin().getProjectionReferenceLon(); // Reference point longitude in degrees
|
||||
lpos.ref_alt = _ekf.getEkfGlobalOriginAltitude(); // Reference point in MSL altitude meters
|
||||
lpos.xy_global = true;
|
||||
lpos.z_global = true;
|
||||
|
|
|
@ -98,11 +98,11 @@ void Gps::stepHorizontalPositionByMeters(const Vector2f hpos_change)
|
|||
double lat_new {0.0};
|
||||
double lon_new {0.0};
|
||||
|
||||
map_projection_project(&_ekf->global_origin(), _gps_data.lat * 1e-7, _gps_data.lon * 1e-7, &hposN_curr, &hposE_curr);
|
||||
_ekf->global_origin().project(_gps_data.lat * 1e-7, _gps_data.lon * 1e-7, hposN_curr, hposE_curr);
|
||||
|
||||
Vector2f hpos_new = Vector2f{hposN_curr, hposE_curr} + hpos_change;
|
||||
|
||||
map_projection_reproject(&_ekf->global_origin(), hpos_new(0), hpos_new(1), &lat_new, &lon_new);
|
||||
_ekf->global_origin().reproject(hpos_new(0), hpos_new(1), lat_new, lon_new);
|
||||
|
||||
_gps_data.lon = static_cast<int32_t>(lon_new * 1e7);
|
||||
_gps_data.lat = static_cast<int32_t>(lat_new * 1e7);
|
||||
|
|
|
@ -365,8 +365,8 @@ bool FlightTaskAuto::_evaluateTriplets()
|
|||
_lock_position_xy.setAll(NAN);
|
||||
|
||||
// Convert from global to local frame.
|
||||
map_projection_project(&_reference_position,
|
||||
_sub_triplet_setpoint.get().current.lat, _sub_triplet_setpoint.get().current.lon, &tmp_target(0), &tmp_target(1));
|
||||
_reference_position.project(_sub_triplet_setpoint.get().current.lat, _sub_triplet_setpoint.get().current.lon,
|
||||
tmp_target(0), tmp_target(1));
|
||||
}
|
||||
|
||||
tmp_target(2) = -(_sub_triplet_setpoint.get().current.alt - _reference_altitude);
|
||||
|
@ -407,8 +407,8 @@ bool FlightTaskAuto::_evaluateTriplets()
|
|||
_prev_prev_wp = _triplet_prev_wp;
|
||||
|
||||
if (_isFinite(_sub_triplet_setpoint.get().previous) && _sub_triplet_setpoint.get().previous.valid) {
|
||||
map_projection_project(&_reference_position, _sub_triplet_setpoint.get().previous.lat,
|
||||
_sub_triplet_setpoint.get().previous.lon, &_triplet_prev_wp(0), &_triplet_prev_wp(1));
|
||||
_reference_position.project(_sub_triplet_setpoint.get().previous.lat,
|
||||
_sub_triplet_setpoint.get().previous.lon, _triplet_prev_wp(0), _triplet_prev_wp(1));
|
||||
_triplet_prev_wp(2) = -(_sub_triplet_setpoint.get().previous.alt - _reference_altitude);
|
||||
|
||||
} else {
|
||||
|
@ -421,8 +421,8 @@ bool FlightTaskAuto::_evaluateTriplets()
|
|||
_triplet_next_wp = _triplet_target;
|
||||
|
||||
} else if (_isFinite(_sub_triplet_setpoint.get().next) && _sub_triplet_setpoint.get().next.valid) {
|
||||
map_projection_project(&_reference_position, _sub_triplet_setpoint.get().next.lat,
|
||||
_sub_triplet_setpoint.get().next.lon, &_triplet_next_wp(0), &_triplet_next_wp(1));
|
||||
_reference_position.project(_sub_triplet_setpoint.get().next.lat,
|
||||
_sub_triplet_setpoint.get().next.lon, _triplet_next_wp(0), _triplet_next_wp(1));
|
||||
_triplet_next_wp(2) = -(_sub_triplet_setpoint.get().next.alt - _reference_altitude);
|
||||
|
||||
} else {
|
||||
|
@ -591,7 +591,7 @@ bool FlightTaskAuto::_evaluateGlobalReference()
|
|||
}
|
||||
|
||||
// init projection
|
||||
map_projection_init(&_reference_position, ref_lat, ref_lon);
|
||||
_reference_position.initReference(ref_lat, ref_lon);
|
||||
|
||||
// check if everything is still finite
|
||||
return PX4_ISFINITE(_reference_altitude) && PX4_ISFINITE(ref_lat) && PX4_ISFINITE(ref_lon);
|
||||
|
|
|
@ -196,7 +196,7 @@ private:
|
|||
_triplet_next_wp; /**< next triplet from navigator which may differ from the intenal one (_next_wp) depending on the vehicle state.*/
|
||||
matrix::Vector2f _closest_pt; /**< closest point to the vehicle position on the line previous - target */
|
||||
|
||||
map_projection_reference_s _reference_position{}; /**< Structure used to project lat/lon setpoint into local frame. */
|
||||
MapProjection _reference_position{}; /**< Structure used to project lat/lon setpoint into local frame. */
|
||||
float _reference_altitude{NAN}; /**< Altitude relative to ground. */
|
||||
hrt_abstime _time_stamp_reference{0}; /**< time stamp when last reference update occured. */
|
||||
|
||||
|
|
|
@ -154,11 +154,10 @@ void FlightTask::_evaluateVehicleLocalPosition()
|
|||
|
||||
// global frame reference coordinates to enable conversions
|
||||
if (_sub_vehicle_local_position.get().xy_global && _sub_vehicle_local_position.get().z_global) {
|
||||
if (!map_projection_initialized(&_global_local_proj_ref)
|
||||
|| (_global_local_proj_ref.timestamp != _sub_vehicle_local_position.get().ref_timestamp)) {
|
||||
if (!_geo_projection.isInitialized()
|
||||
|| (_geo_projection.getProjectionReferenceTimestamp() != _sub_vehicle_local_position.get().ref_timestamp)) {
|
||||
|
||||
map_projection_init_timestamped(&_global_local_proj_ref,
|
||||
_sub_vehicle_local_position.get().ref_lat, _sub_vehicle_local_position.get().ref_lon,
|
||||
_geo_projection.initReference(_sub_vehicle_local_position.get().ref_lat, _sub_vehicle_local_position.get().ref_lon,
|
||||
_sub_vehicle_local_position.get().ref_timestamp);
|
||||
|
||||
_global_local_alt0 = _sub_vehicle_local_position.get().ref_alt;
|
||||
|
|
|
@ -206,7 +206,7 @@ protected:
|
|||
virtual void _ekfResetHandlerVelocityZ(float delta_vz) {};
|
||||
virtual void _ekfResetHandlerHeading(float delta_psi) {};
|
||||
|
||||
map_projection_reference_s _global_local_proj_ref{};
|
||||
MapProjection _geo_projection{};
|
||||
float _global_local_alt0{NAN};
|
||||
|
||||
/* Time abstraction */
|
||||
|
|
|
@ -84,10 +84,8 @@ bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command)
|
|||
|
||||
// commanded center coordinates
|
||||
if (PX4_ISFINITE(command.param5) && PX4_ISFINITE(command.param6)) {
|
||||
if (map_projection_initialized(&_global_local_proj_ref)) {
|
||||
map_projection_project(&_global_local_proj_ref,
|
||||
command.param5, command.param6,
|
||||
&_center(0), &_center(1));
|
||||
if (_geo_projection.isInitialized()) {
|
||||
_center.xy() = _geo_projection.project(command.param5, command.param6);
|
||||
|
||||
} else {
|
||||
ret = false;
|
||||
|
@ -96,7 +94,7 @@ bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command)
|
|||
|
||||
// commanded altitude
|
||||
if (PX4_ISFINITE(command.param7)) {
|
||||
if (map_projection_initialized(&_global_local_proj_ref)) {
|
||||
if (_geo_projection.isInitialized()) {
|
||||
_center(2) = _global_local_alt0 - command.param7;
|
||||
|
||||
} else {
|
||||
|
@ -121,9 +119,9 @@ bool FlightTaskOrbit::sendTelemetry()
|
|||
orbit_status.frame = 0; // MAV_FRAME::MAV_FRAME_GLOBAL
|
||||
orbit_status.yaw_behaviour = _yaw_behaviour;
|
||||
|
||||
if (map_projection_initialized(&_global_local_proj_ref)) {
|
||||
if (_geo_projection.isInitialized()) {
|
||||
// local -> global
|
||||
map_projection_reproject(&_global_local_proj_ref, _center(0), _center(1), &orbit_status.x, &orbit_status.y);
|
||||
_geo_projection.reproject(_center(0), _center(1), orbit_status.x, orbit_status.y);
|
||||
orbit_status.z = _global_local_alt0 - _position_setpoint(2);
|
||||
|
||||
} else {
|
||||
|
|
|
@ -1915,10 +1915,10 @@ FixedwingPositionControl::Run()
|
|||
|
||||
if (_control_mode.flag_control_offboard_enabled) {
|
||||
// Convert Local setpoints to global setpoints
|
||||
if (!map_projection_initialized(&_global_local_proj_ref)
|
||||
|| (_global_local_proj_ref.timestamp != _local_pos.ref_timestamp)) {
|
||||
if (!_global_local_proj_ref.isInitialized()
|
||||
|| (_global_local_proj_ref.getProjectionReferenceTimestamp() != _local_pos.ref_timestamp)) {
|
||||
|
||||
map_projection_init_timestamped(&_global_local_proj_ref, _local_pos.ref_lat, _local_pos.ref_lon,
|
||||
_global_local_proj_ref.initReference(_local_pos.ref_lat, _local_pos.ref_lon,
|
||||
_local_pos.ref_timestamp);
|
||||
_global_local_alt0 = _local_pos.ref_alt;
|
||||
}
|
||||
|
@ -1930,7 +1930,8 @@ FixedwingPositionControl::Run()
|
|||
double lat;
|
||||
double lon;
|
||||
|
||||
if (map_projection_reproject(&_global_local_proj_ref, trajectory_setpoint.x, trajectory_setpoint.y, &lat, &lon) == 0) {
|
||||
if (_global_local_proj_ref.isInitialized()) {
|
||||
_global_local_proj_ref.reproject(trajectory_setpoint.x, trajectory_setpoint.y, lat, lon);
|
||||
_pos_sp_triplet = {}; // clear any existing
|
||||
|
||||
_pos_sp_triplet.timestamp = trajectory_setpoint.timestamp;
|
||||
|
|
|
@ -172,7 +172,7 @@ private:
|
|||
|
||||
perf_counter_t _loop_perf; ///< loop performance counter
|
||||
|
||||
map_projection_reference_s _global_local_proj_ref{};
|
||||
MapProjection _global_local_proj_ref{};
|
||||
float _global_local_alt0{NAN};
|
||||
|
||||
float _takeoff_ground_alt{0.0f}; ///< ground altitude at which plane was launched
|
||||
|
|
|
@ -24,9 +24,6 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
|
|||
// this block has no parent, and has name LPE
|
||||
SuperBlock(nullptr, "LPE"),
|
||||
|
||||
// map projection
|
||||
_map_ref(),
|
||||
|
||||
// flow gyro
|
||||
_flow_gyro_x_high_pass(this, "FGYRO_HP"),
|
||||
_flow_gyro_y_high_pass(this, "FGYRO_HP"),
|
||||
|
@ -123,9 +120,6 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
|
|||
_u.setZero();
|
||||
initSS();
|
||||
|
||||
// map
|
||||
_map_ref.init_done = false;
|
||||
|
||||
// print fusion settings to console
|
||||
PX4_INFO("fuse gps: %d, flow: %d, vis_pos: %d, "
|
||||
"landing_target: %d, land: %d, pub_agl_z: %d, flow_gyro: %d, "
|
||||
|
@ -176,7 +170,7 @@ void BlockLocalPositionEstimator::Run()
|
|||
const double longitude = vehicle_command.param6;
|
||||
const float altitude = vehicle_command.param7;
|
||||
|
||||
map_projection_init_timestamped(&_global_local_proj_ref, latitude, longitude, vehicle_command.timestamp);
|
||||
_global_local_proj_ref.initReference(latitude, longitude, vehicle_command.timestamp);
|
||||
_global_local_alt0 = altitude;
|
||||
|
||||
PX4_INFO("New NED origin (LLA): %3.10f, %3.10f, %4.3f\n", latitude, longitude, static_cast<double>(altitude));
|
||||
|
@ -351,8 +345,8 @@ void BlockLocalPositionEstimator::Run()
|
|||
checkTimeouts();
|
||||
|
||||
// if we have no lat, lon initialize projection to LPE_LAT, LPE_LON parameters
|
||||
if (!_map_ref.init_done && (_estimatorInitialized & EST_XY) && _param_lpe_fake_origin.get()) {
|
||||
map_projection_init(&_map_ref,
|
||||
if (!_map_ref.isInitialized() && (_estimatorInitialized & EST_XY) && _param_lpe_fake_origin.get()) {
|
||||
_map_ref.initReference(
|
||||
(double)_param_lpe_lat.get(),
|
||||
(double)_param_lpe_lon.get());
|
||||
|
||||
|
@ -519,7 +513,7 @@ void BlockLocalPositionEstimator::Run()
|
|||
_pub_innov_var.get().timestamp = hrt_absolute_time();
|
||||
_pub_innov_var.update();
|
||||
|
||||
if ((_estimatorInitialized & EST_XY) && (_map_ref.init_done || _param_lpe_fake_origin.get())) {
|
||||
if ((_estimatorInitialized & EST_XY) && (_map_ref.isInitialized() || _param_lpe_fake_origin.get())) {
|
||||
publishGlobalPos();
|
||||
}
|
||||
}
|
||||
|
@ -623,8 +617,8 @@ void BlockLocalPositionEstimator::publishLocalPos()
|
|||
_pub_lpos.get().xy_global = _estimatorInitialized & EST_XY;
|
||||
_pub_lpos.get().z_global = !(_sensorTimeout & SENSOR_BARO) && _altOriginGlobal;
|
||||
_pub_lpos.get().ref_timestamp = _time_origin;
|
||||
_pub_lpos.get().ref_lat = _map_ref.lat_rad * 180 / M_PI;
|
||||
_pub_lpos.get().ref_lon = _map_ref.lon_rad * 180 / M_PI;
|
||||
_pub_lpos.get().ref_lat = _map_ref.getProjectionReferenceLat();
|
||||
_pub_lpos.get().ref_lon = _map_ref.getProjectionReferenceLon();
|
||||
_pub_lpos.get().ref_alt = _altOrigin;
|
||||
_pub_lpos.get().dist_bottom = _aglLowPass.getState();
|
||||
// we estimate agl even when we don't have terrain info
|
||||
|
@ -785,7 +779,7 @@ void BlockLocalPositionEstimator::publishGlobalPos()
|
|||
double lat = 0;
|
||||
double lon = 0;
|
||||
const Vector<float, n_x> &xLP = _xLowPass.getState();
|
||||
map_projection_reproject(&_map_ref, xLP(X_x), xLP(X_y), &lat, &lon);
|
||||
_map_ref.reproject(xLP(X_x), xLP(X_y), lat, lon);
|
||||
float alt = -xLP(X_z) + _altOrigin;
|
||||
|
||||
// lie about eph/epv to allow visual odometry only navigation when velocity est. good
|
||||
|
|
|
@ -294,9 +294,9 @@ private:
|
|||
uORB::PublicationData<estimator_innovations_s> _pub_innov_var{ORB_ID(estimator_innovation_variances)};
|
||||
|
||||
// map projection
|
||||
struct map_projection_reference_s _map_ref;
|
||||
MapProjection _map_ref;
|
||||
|
||||
map_projection_reference_s _global_local_proj_ref{};
|
||||
MapProjection _global_local_proj_ref{};
|
||||
float _global_local_alt0{NAN};
|
||||
|
||||
// target mode paramters from landing_target_estimator module
|
||||
|
|
|
@ -57,15 +57,15 @@ void BlockLocalPositionEstimator::gpsInit()
|
|||
// find lat, lon of current origin by subtracting x and y
|
||||
// if not using vision position since vision will
|
||||
// have it's own origin, not necessarily where vehicle starts
|
||||
if (!_map_ref.init_done) {
|
||||
if (!_map_ref.isInitialized()) {
|
||||
double gpsLatOrigin = 0;
|
||||
double gpsLonOrigin = 0;
|
||||
// reproject at current coordinates
|
||||
map_projection_init(&_map_ref, gpsLat, gpsLon);
|
||||
_map_ref.initReference(gpsLat, gpsLon);
|
||||
// find origin
|
||||
map_projection_reproject(&_map_ref, -_x(X_x), -_x(X_y), &gpsLatOrigin, &gpsLonOrigin);
|
||||
_map_ref.reproject(-_x(X_x), -_x(X_y), gpsLatOrigin, gpsLonOrigin);
|
||||
// reinit origin
|
||||
map_projection_init(&_map_ref, gpsLatOrigin, gpsLonOrigin);
|
||||
_map_ref.initReference(gpsLatOrigin, gpsLonOrigin);
|
||||
// set timestamp when origin was set to current time
|
||||
_time_origin = _timeStamp;
|
||||
|
||||
|
@ -119,7 +119,7 @@ void BlockLocalPositionEstimator::gpsCorrect()
|
|||
float px = 0;
|
||||
float py = 0;
|
||||
float pz = -(alt - _gpsAltOrigin);
|
||||
map_projection_project(&_map_ref, lat, lon, &px, &py);
|
||||
_map_ref.project(lat, lon, px, py);
|
||||
Vector<float, n_y_gps> y;
|
||||
y.setZero();
|
||||
y(Y_gps_x) = px;
|
||||
|
|
|
@ -37,17 +37,17 @@ void BlockLocalPositionEstimator::mocapInit()
|
|||
_sensorFault &= ~SENSOR_MOCAP;
|
||||
|
||||
// get reference for global position
|
||||
_ref_lat = math::degrees(_global_local_proj_ref.lat_rad);
|
||||
_ref_lon = math::degrees(_global_local_proj_ref.lon_rad);
|
||||
_ref_lat = _global_local_proj_ref.getProjectionReferenceLat();
|
||||
_ref_lon = _global_local_proj_ref.getProjectionReferenceLon();
|
||||
_ref_alt = _global_local_alt0;
|
||||
|
||||
_is_global_cov_init = map_projection_initialized(&_global_local_proj_ref);
|
||||
_is_global_cov_init = _global_local_proj_ref.isInitialized();
|
||||
|
||||
if (!_map_ref.init_done && _is_global_cov_init && !_visionUpdated) {
|
||||
if (!_map_ref.isInitialized() && _is_global_cov_init && !_visionUpdated) {
|
||||
// initialize global origin using the mocap estimator reference (only if the vision estimation is not being fused as well)
|
||||
mavlink_log_info(&mavlink_log_pub, "[lpe] global origin init (mocap) : lat %6.2f lon %6.2f alt %5.1f m",
|
||||
double(_ref_lat), double(_ref_lon), double(_ref_alt));
|
||||
map_projection_init(&_map_ref, _ref_lat, _ref_lon);
|
||||
_map_ref.initReference(_ref_lat, _ref_lon);
|
||||
// set timestamp when origin was set to current time
|
||||
_time_origin = _timeStamp;
|
||||
}
|
||||
|
@ -55,7 +55,7 @@ void BlockLocalPositionEstimator::mocapInit()
|
|||
if (!_altOriginInitialized) {
|
||||
_altOriginInitialized = true;
|
||||
_altOriginGlobal = true;
|
||||
_altOrigin = map_projection_initialized(&_global_local_proj_ref) ? _ref_alt : 0.0f;
|
||||
_altOrigin = _global_local_proj_ref.isInitialized() ? _ref_alt : 0.0f;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
@ -42,17 +42,17 @@ void BlockLocalPositionEstimator::visionInit()
|
|||
_sensorFault &= ~SENSOR_VISION;
|
||||
|
||||
// get reference for global position
|
||||
_ref_lat = math::degrees(_global_local_proj_ref.lat_rad);
|
||||
_ref_lon = math::degrees(_global_local_proj_ref.lon_rad);
|
||||
_ref_lat = _global_local_proj_ref.getProjectionReferenceLat();
|
||||
_ref_lon = _global_local_proj_ref.getProjectionReferenceLon();
|
||||
_ref_alt = _global_local_alt0;
|
||||
|
||||
_is_global_cov_init = map_projection_initialized(&_global_local_proj_ref);
|
||||
_is_global_cov_init = _global_local_proj_ref.isInitialized();
|
||||
|
||||
if (!_map_ref.init_done && _is_global_cov_init) {
|
||||
if (!_map_ref.isInitialized() && _is_global_cov_init) {
|
||||
// initialize global origin using the visual estimator reference
|
||||
mavlink_log_info(&mavlink_log_pub, "[lpe] global origin init (vision) : lat %6.2f lon %6.2f alt %5.1f m",
|
||||
double(_ref_lat), double(_ref_lon), double(_ref_alt));
|
||||
map_projection_init(&_map_ref, _ref_lat, _ref_lon);
|
||||
_map_ref.initReference(_ref_lat, _ref_lon);
|
||||
// set timestamp when origin was set to current time
|
||||
_time_origin = _timeStamp;
|
||||
}
|
||||
|
@ -60,7 +60,7 @@ void BlockLocalPositionEstimator::visionInit()
|
|||
if (!_altOriginInitialized) {
|
||||
_altOriginInitialized = true;
|
||||
_altOriginGlobal = true;
|
||||
_altOrigin = map_projection_initialized(&_global_local_proj_ref) ? _ref_alt : 0.0f;
|
||||
_altOrigin = _global_local_proj_ref.isInitialized() ? _ref_alt : 0.0f;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
@ -1150,13 +1150,12 @@ MavlinkReceiver::handle_message_set_position_target_global_int(mavlink_message_t
|
|||
return;
|
||||
}
|
||||
|
||||
map_projection_reference_s global_local_proj_ref{};
|
||||
map_projection_init_timestamped(&global_local_proj_ref, local_pos.ref_lat, local_pos.ref_lon, local_pos.ref_timestamp);
|
||||
MapProjection global_local_proj_ref{local_pos.ref_lat, local_pos.ref_lon, local_pos.ref_timestamp};
|
||||
|
||||
// global -> local
|
||||
const double lat = target_global_int.lat_int / 1e7;
|
||||
const double lon = target_global_int.lon_int / 1e7;
|
||||
map_projection_project(&global_local_proj_ref, lat, lon, &setpoint.x, &setpoint.y);
|
||||
global_local_proj_ref.project(lat, lon, setpoint.x, setpoint.y);
|
||||
|
||||
if (target_global_int.coordinate_frame == MAV_FRAME_GLOBAL_INT) {
|
||||
setpoint.z = local_pos.ref_alt - target_global_int.alt;
|
||||
|
@ -2705,20 +2704,20 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
|
|||
const double lat = hil_state.lat * 1e-7;
|
||||
const double lon = hil_state.lon * 1e-7;
|
||||
|
||||
if (!map_projection_initialized(&_global_local_proj_ref) || !PX4_ISFINITE(_global_local_alt0)) {
|
||||
map_projection_init(&_global_local_proj_ref, lat, lon);
|
||||
if (!_global_local_proj_ref.isInitialized() || !PX4_ISFINITE(_global_local_alt0)) {
|
||||
_global_local_proj_ref.initReference(lat, lon);
|
||||
_global_local_alt0 = hil_state.alt / 1000.f;
|
||||
}
|
||||
|
||||
float x = 0.f;
|
||||
float y = 0.f;
|
||||
map_projection_project(&_global_local_proj_ref, lat, lon, &x, &y);
|
||||
_global_local_proj_ref.project(lat, lon, x, y);
|
||||
|
||||
vehicle_local_position_s hil_local_pos{};
|
||||
hil_local_pos.timestamp_sample = timestamp_sample;
|
||||
hil_local_pos.ref_timestamp = _global_local_proj_ref.timestamp;
|
||||
hil_local_pos.ref_lat = math::degrees(_global_local_proj_ref.lat_rad);
|
||||
hil_local_pos.ref_lon = math::degrees(_global_local_proj_ref.lon_rad);
|
||||
hil_local_pos.ref_timestamp = _global_local_proj_ref.getProjectionReferenceTimestamp();
|
||||
hil_local_pos.ref_lat = _global_local_proj_ref.getProjectionReferenceLat();
|
||||
hil_local_pos.ref_lon = _global_local_proj_ref.getProjectionReferenceLon();
|
||||
hil_local_pos.ref_alt = _global_local_alt0;
|
||||
hil_local_pos.xy_valid = true;
|
||||
hil_local_pos.z_valid = true;
|
||||
|
|
|
@ -362,7 +362,7 @@ private:
|
|||
PX4Magnetometer *_px4_mag{nullptr};
|
||||
|
||||
float _global_local_alt0{NAN};
|
||||
map_projection_reference_s _global_local_proj_ref{};
|
||||
MapProjection _global_local_proj_ref{};
|
||||
|
||||
hrt_abstime _last_utm_global_pos_com{0};
|
||||
|
||||
|
|
|
@ -52,17 +52,15 @@ TEST_F(GeofenceBreachAvoidanceTest, waypointFromBearingAndDistance)
|
|||
{
|
||||
|
||||
GeofenceBreachAvoidance gf_avoidance(nullptr);
|
||||
struct map_projection_reference_s ref = {};
|
||||
Vector2d home_global(42.1, 8.2);
|
||||
map_projection_init(&ref, home_global(0), home_global(1));
|
||||
|
||||
MapProjection ref{home_global(0), home_global(1)};
|
||||
|
||||
Vector2f waypoint_north_east_local(1.0, 1.0);
|
||||
waypoint_north_east_local = waypoint_north_east_local.normalized() * 10.5;
|
||||
Vector2d waypoint_north_east_global = gf_avoidance.waypointFromBearingAndDistance(home_global, M_PI_F * 0.25f, 10.5);
|
||||
|
||||
float x, y;
|
||||
map_projection_project(&ref, waypoint_north_east_global(0), waypoint_north_east_global(1), &x, &y);
|
||||
Vector2f waypoint_north_east_reprojected(x, y);
|
||||
Vector2f waypoint_north_east_reprojected = ref.project(waypoint_north_east_global(0), waypoint_north_east_global(1));
|
||||
|
||||
EXPECT_FLOAT_EQ(waypoint_north_east_local(0), waypoint_north_east_reprojected(0));
|
||||
EXPECT_FLOAT_EQ(waypoint_north_east_local(1), waypoint_north_east_reprojected(1));
|
||||
|
@ -72,8 +70,7 @@ TEST_F(GeofenceBreachAvoidanceTest, waypointFromBearingAndDistance)
|
|||
|
||||
Vector2d waypoint_southwest_global = gf_avoidance.waypointFromBearingAndDistance(home_global, M_PI_F * 0.25f, -10.5);
|
||||
|
||||
map_projection_project(&ref, waypoint_southwest_global(0), waypoint_southwest_global(1), &x, &y);
|
||||
Vector2f waypoint_south_west_reprojected(x, y);
|
||||
Vector2f waypoint_south_west_reprojected = ref.project(waypoint_southwest_global(0), waypoint_southwest_global(1));
|
||||
|
||||
EXPECT_FLOAT_EQ(waypoint_south_west_local(0), waypoint_south_west_reprojected(0));
|
||||
EXPECT_FLOAT_EQ(waypoint_south_west_local(1), waypoint_south_west_reprojected(1));
|
||||
|
@ -88,9 +85,8 @@ TEST_F(GeofenceBreachAvoidanceTest, generateLoiterPointForFixedWing)
|
|||
{
|
||||
GeofenceBreachAvoidance gf_avoidance(nullptr);
|
||||
FakeGeofence geo;
|
||||
struct map_projection_reference_s ref = {};
|
||||
Vector2d home_global(42.1, 8.2);
|
||||
map_projection_init(&ref, home_global(0), home_global(1));
|
||||
MapProjection ref{home_global(0), home_global(1)};
|
||||
|
||||
geofence_violation_type_u gf_violation;
|
||||
gf_violation.flags.fence_violation = true;
|
||||
|
@ -136,9 +132,8 @@ TEST_F(GeofenceBreachAvoidanceTest, generateLoiterPointForMultirotor)
|
|||
{
|
||||
GeofenceBreachAvoidance gf_avoidance(nullptr);
|
||||
FakeGeofence geo;
|
||||
struct map_projection_reference_s ref = {};
|
||||
Vector2d home_global(42.1, 8.2);
|
||||
map_projection_init(&ref, home_global(0), home_global(1));
|
||||
MapProjection ref{42.1, 8.2};
|
||||
|
||||
param_t param = param_handle(px4::params::MPC_ACC_HOR);
|
||||
|
||||
|
@ -245,9 +240,8 @@ TEST_F(GeofenceBreachAvoidanceTest, maxDistToHomeViolationMulticopter)
|
|||
{
|
||||
GeofenceBreachAvoidance gf_avoidance(nullptr);
|
||||
FakeGeofence geo;
|
||||
struct map_projection_reference_s ref = {};
|
||||
Vector2d home_global(42.1, 8.2);
|
||||
map_projection_init(&ref, home_global(0), home_global(1));
|
||||
MapProjection ref{home_global(0), home_global(1)};
|
||||
geofence_violation_type_u gf_violation;
|
||||
gf_violation.flags.dist_to_home_exceeded = true;
|
||||
|
||||
|
@ -278,9 +272,8 @@ TEST_F(GeofenceBreachAvoidanceTest, maxDistToHomeViolationFixedWing)
|
|||
{
|
||||
GeofenceBreachAvoidance gf_avoidance(nullptr);
|
||||
FakeGeofence geo;
|
||||
struct map_projection_reference_s ref = {};
|
||||
Vector2d home_global(42.1, 8.2);
|
||||
map_projection_init(&ref, home_global(0), home_global(1));
|
||||
MapProjection ref{home_global(0), home_global(1)};
|
||||
geofence_violation_type_u gf_violation;
|
||||
gf_violation.flags.dist_to_home_exceeded = true;
|
||||
|
||||
|
|
|
@ -122,13 +122,10 @@ private:
|
|||
|
||||
bool _gf_boundary_is_20m_north(double lat, double lon, float alt)
|
||||
{
|
||||
struct map_projection_reference_s ref = {};
|
||||
matrix::Vector2<double> home_global(42.1, 8.2);
|
||||
map_projection_init(&ref, home_global(0), home_global(1));
|
||||
|
||||
float x, y;
|
||||
map_projection_project(&ref, lat, lon, &x, &y);
|
||||
matrix::Vector2f waypoint_local(x, y);
|
||||
MapProjection projection{home_global(0), home_global(1)};
|
||||
matrix::Vector2f waypoint_local = projection.project(lat, lon);
|
||||
|
||||
if (waypoint_local(0) >= 20.0f) {
|
||||
return false;
|
||||
|
|
|
@ -94,7 +94,7 @@ void FollowTarget::on_activation()
|
|||
|
||||
void FollowTarget::on_active()
|
||||
{
|
||||
struct map_projection_reference_s target_ref;
|
||||
MapProjection target_ref;
|
||||
follow_target_s target_motion_with_offset = {};
|
||||
uint64_t current_time = hrt_absolute_time();
|
||||
bool radius_entered = false;
|
||||
|
@ -134,9 +134,9 @@ void FollowTarget::on_active()
|
|||
|
||||
// get distance to target
|
||||
|
||||
map_projection_init(&target_ref, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
|
||||
map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon, &_target_distance(0),
|
||||
&_target_distance(1));
|
||||
target_ref.initReference(_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
|
||||
target_ref.project(_current_target_motion.lat, _current_target_motion.lon, _target_distance(0),
|
||||
_target_distance(1));
|
||||
|
||||
}
|
||||
|
||||
|
@ -149,11 +149,11 @@ void FollowTarget::on_active()
|
|||
// ignore a small dt
|
||||
if (dt_ms > 10.0F) {
|
||||
// get last gps known reference for target
|
||||
map_projection_init(&target_ref, _previous_target_motion.lat, _previous_target_motion.lon);
|
||||
target_ref.initReference(_previous_target_motion.lat, _previous_target_motion.lon);
|
||||
|
||||
// calculate distance the target has moved
|
||||
map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon,
|
||||
&(_target_position_delta(0)), &(_target_position_delta(1)));
|
||||
target_ref.project(_current_target_motion.lat, _current_target_motion.lon,
|
||||
(_target_position_delta(0)), (_target_position_delta(1)));
|
||||
|
||||
// update the average velocity of the target based on the position
|
||||
if (PX4_ISFINITE(_current_target_motion.vx) && PX4_ISFINITE(_current_target_motion.vy)) {
|
||||
|
@ -229,9 +229,9 @@ void FollowTarget::on_active()
|
|||
|
||||
// get the target position using the calculated offset
|
||||
|
||||
map_projection_init(&target_ref, _current_target_motion.lat, _current_target_motion.lon);
|
||||
map_projection_reproject(&target_ref, _target_position_offset(0), _target_position_offset(1),
|
||||
&target_motion_with_offset.lat, &target_motion_with_offset.lon);
|
||||
target_ref.initReference(_current_target_motion.lat, _current_target_motion.lon);
|
||||
target_ref.reproject(_target_position_offset(0), _target_position_offset(1),
|
||||
target_motion_with_offset.lat, target_motion_with_offset.lon);
|
||||
}
|
||||
|
||||
// clamp yaw rate smoothing if we are with in
|
||||
|
|
|
@ -447,13 +447,13 @@ bool Geofence::insideCircle(const PolygonInfo &polygon, double lat, double lon,
|
|||
return false;
|
||||
}
|
||||
|
||||
if (!map_projection_initialized(&_projection_reference)) {
|
||||
map_projection_init(&_projection_reference, lat, lon);
|
||||
if (!_projection_reference.isInitialized()) {
|
||||
_projection_reference.initReference(lat, lon);
|
||||
}
|
||||
|
||||
float x1, y1, x2, y2;
|
||||
map_projection_project(&_projection_reference, lat, lon, &x1, &y1);
|
||||
map_projection_project(&_projection_reference, circle_point.lat, circle_point.lon, &x2, &y2);
|
||||
_projection_reference.project(lat, lon, x1, y1);
|
||||
_projection_reference.project(circle_point.lat, circle_point.lon, x2, y2);
|
||||
float dx = x1 - x2, dy = y1 - y2;
|
||||
return dx * dx + dy * dy < circle_point.circle_radius * circle_point.circle_radius;
|
||||
}
|
||||
|
|
|
@ -173,7 +173,7 @@ private:
|
|||
PolygonInfo *_polygons{nullptr};
|
||||
int _num_polygons{0};
|
||||
|
||||
map_projection_reference_s _projection_reference = {}; ///< reference to convert (lon, lat) to local [m]
|
||||
MapProjection _projection_reference = {}; ///< reference to convert (lon, lat) to local [m]
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamInt<px4::params::GF_ACTION>) _param_gf_action,
|
||||
|
|
|
@ -248,17 +248,14 @@ MissionBlock::is_mission_item_reached()
|
|||
if (curr_sp->valid) {
|
||||
|
||||
// location of gate (mission item)
|
||||
struct map_projection_reference_s ref_pos;
|
||||
map_projection_init(&ref_pos, _mission_item.lat, _mission_item.lon);
|
||||
MapProjection ref_pos{_mission_item.lat, _mission_item.lon};
|
||||
|
||||
// current setpoint
|
||||
matrix::Vector2f gate_to_curr_sp;
|
||||
map_projection_project(&ref_pos, curr_sp->lat, curr_sp->lon, &gate_to_curr_sp(0), &gate_to_curr_sp(1));
|
||||
matrix::Vector2f gate_to_curr_sp = ref_pos.project(curr_sp->lat, curr_sp->lon);
|
||||
|
||||
// system position
|
||||
matrix::Vector2f vehicle_pos;
|
||||
map_projection_project(&ref_pos, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
|
||||
&vehicle_pos(0), &vehicle_pos(1));
|
||||
matrix::Vector2f vehicle_pos = ref_pos.project(_navigator->get_global_position()->lat,
|
||||
_navigator->get_global_position()->lon);
|
||||
const float dot_product = vehicle_pos.dot(gate_to_curr_sp.normalized());
|
||||
|
||||
// if the dot product (projected vector) is positive, then
|
||||
|
|
|
@ -78,8 +78,8 @@ PrecLand::on_activation()
|
|||
|
||||
vehicle_local_position_s *vehicle_local_position = _navigator->get_local_position();
|
||||
|
||||
if (!map_projection_initialized(&_map_ref)) {
|
||||
map_projection_init(&_map_ref, vehicle_local_position->ref_lat, vehicle_local_position->ref_lon);
|
||||
if (!_map_ref.isInitialized()) {
|
||||
_map_ref.initReference(vehicle_local_position->ref_lat, vehicle_local_position->ref_lon);
|
||||
}
|
||||
|
||||
position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
|
||||
|
@ -279,7 +279,7 @@ PrecLand::run_state_horizontal_approach()
|
|||
|
||||
// XXX need to transform to GPS coords because mc_pos_control only looks at that
|
||||
double lat, lon;
|
||||
map_projection_reproject(&_map_ref, x, y, &lat, &lon);
|
||||
_map_ref.reproject(x, y, lat, lon);
|
||||
|
||||
pos_sp_triplet->current.lat = lat;
|
||||
pos_sp_triplet->current.lon = lon;
|
||||
|
@ -316,7 +316,7 @@ PrecLand::run_state_descend_above_target()
|
|||
|
||||
// XXX need to transform to GPS coords because mc_pos_control only looks at that
|
||||
double lat, lon;
|
||||
map_projection_reproject(&_map_ref, _target_pose.x_abs, _target_pose.y_abs, &lat, &lon);
|
||||
_map_ref.reproject(_target_pose.x_abs, _target_pose.y_abs, lat, lon);
|
||||
|
||||
pos_sp_triplet->current.lat = lat;
|
||||
pos_sp_triplet->current.lon = lon;
|
||||
|
@ -555,8 +555,8 @@ void PrecLand::slewrate(float &sp_x, float &sp_y)
|
|||
dt = 50000 / SEC2USEC;
|
||||
|
||||
// set a best guess for previous setpoints for smooth transition
|
||||
map_projection_project(&_map_ref, _navigator->get_position_setpoint_triplet()->current.lat,
|
||||
_navigator->get_position_setpoint_triplet()->current.lon, &_sp_pev(0), &_sp_pev(1));
|
||||
_map_ref.project(_navigator->get_position_setpoint_triplet()->current.lat,
|
||||
_navigator->get_position_setpoint_triplet()->current.lon, _sp_pev(0), _sp_pev(1));
|
||||
_sp_pev_prev(0) = _sp_pev(0) - _navigator->get_local_position()->vx * dt;
|
||||
_sp_pev_prev(1) = _sp_pev(1) - _navigator->get_local_position()->vy * dt;
|
||||
}
|
||||
|
|
|
@ -111,7 +111,7 @@ private:
|
|||
bool _target_pose_valid{false}; /**< whether we have received a landing target position message */
|
||||
bool _target_pose_updated{false}; /**< wether the landing target position message is updated */
|
||||
|
||||
struct map_projection_reference_s _map_ref {}; /**< reference for local/global projections */
|
||||
MapProjection _map_ref {}; /**< reference for local/global projections */
|
||||
|
||||
uint64_t _state_start_time{0}; /**< time when we entered current state */
|
||||
uint64_t _last_slewrate_time{0}; /**< time when we last limited setpoint changes */
|
||||
|
|
|
@ -419,10 +419,10 @@ RoverPositionControl::Run()
|
|||
|
||||
// Convert Local setpoints to global setpoints
|
||||
if (_control_mode.flag_control_offboard_enabled) {
|
||||
if (!map_projection_initialized(&_global_local_proj_ref)
|
||||
|| (_global_local_proj_ref.timestamp != _local_pos.ref_timestamp)) {
|
||||
if (!_global_local_proj_ref.isInitialized()
|
||||
|| (_global_local_proj_ref.getProjectionReferenceTimestamp() != _local_pos.ref_timestamp)) {
|
||||
|
||||
map_projection_init_timestamped(&_global_local_proj_ref, _local_pos.ref_lat, _local_pos.ref_lon,
|
||||
_global_local_proj_ref.initReference(_local_pos.ref_lat, _local_pos.ref_lon,
|
||||
_local_pos.ref_timestamp);
|
||||
|
||||
_global_local_alt0 = _local_pos.ref_alt;
|
||||
|
@ -431,9 +431,9 @@ RoverPositionControl::Run()
|
|||
_trajectory_setpoint_sub.update(&_trajectory_setpoint);
|
||||
|
||||
// local -> global
|
||||
map_projection_reproject(&_global_local_proj_ref,
|
||||
_global_local_proj_ref.reproject(
|
||||
_trajectory_setpoint.x, _trajectory_setpoint.y,
|
||||
&_pos_sp_triplet.current.lat, &_pos_sp_triplet.current.lon);
|
||||
_pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon);
|
||||
|
||||
_pos_sp_triplet.current.alt = _global_local_alt0 - _trajectory_setpoint.z;
|
||||
_pos_sp_triplet.current.valid = true;
|
||||
|
|
|
@ -134,7 +134,7 @@ private:
|
|||
hrt_abstime _control_position_last_called{0}; /**<last call of control_position */
|
||||
hrt_abstime _manual_setpoint_last_called{0};
|
||||
|
||||
map_projection_reference_s _global_local_proj_ref{};
|
||||
MapProjection _global_local_proj_ref{};
|
||||
float _global_local_alt0{NAN};
|
||||
|
||||
/* Pid controller for the speed. Here we assume we can control airspeed but the control variable is actually on
|
||||
|
|
|
@ -264,7 +264,7 @@ private:
|
|||
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
|
||||
|
||||
// hil map_ref data
|
||||
map_projection_reference_s _global_local_proj_ref{};
|
||||
MapProjection _global_local_proj_ref{};
|
||||
float _global_local_alt0{NAN};
|
||||
|
||||
vehicle_status_s _vehicle_status{};
|
||||
|
|
|
@ -542,14 +542,14 @@ void Simulator::handle_message_hil_state_quaternion(const mavlink_message_t *msg
|
|||
double lat = hil_state.lat * 1e-7;
|
||||
double lon = hil_state.lon * 1e-7;
|
||||
|
||||
if (!map_projection_initialized(&_global_local_proj_ref)) {
|
||||
map_projection_init(&_global_local_proj_ref, lat, lon);
|
||||
if (!_global_local_proj_ref.isInitialized()) {
|
||||
_global_local_proj_ref.initReference(lat, lon);
|
||||
_global_local_alt0 = hil_state.alt / 1000.f;
|
||||
}
|
||||
|
||||
float x;
|
||||
float y;
|
||||
map_projection_project(&_global_local_proj_ref, lat, lon, &x, &y);
|
||||
_global_local_proj_ref.project(lat, lon, x, y);
|
||||
hil_lpos.timestamp = timestamp;
|
||||
hil_lpos.xy_valid = true;
|
||||
hil_lpos.z_valid = true;
|
||||
|
@ -569,9 +569,9 @@ void Simulator::handle_message_hil_state_quaternion(const mavlink_message_t *msg
|
|||
hil_lpos.heading = euler.psi();
|
||||
hil_lpos.xy_global = true;
|
||||
hil_lpos.z_global = true;
|
||||
hil_lpos.ref_timestamp = _global_local_proj_ref.timestamp;
|
||||
hil_lpos.ref_lat = math::degrees(_global_local_proj_ref.lat_rad);
|
||||
hil_lpos.ref_lon = math::degrees(_global_local_proj_ref.lon_rad);
|
||||
hil_lpos.ref_timestamp = _global_local_proj_ref.getProjectionReferenceTimestamp();
|
||||
hil_lpos.ref_lat = _global_local_proj_ref.getProjectionReferenceLat();
|
||||
hil_lpos.ref_lon = _global_local_proj_ref.getProjectionReferenceLon();
|
||||
hil_lpos.ref_alt = _global_local_alt0;
|
||||
hil_lpos.vxy_max = std::numeric_limits<float>::infinity();
|
||||
hil_lpos.vz_max = std::numeric_limits<float>::infinity();
|
||||
|
|
|
@ -75,13 +75,13 @@ void OutputBase::publish()
|
|||
float OutputBase::_calculate_pitch(double lon, double lat, float altitude,
|
||||
const vehicle_global_position_s &global_position)
|
||||
{
|
||||
if (!map_projection_initialized(&_projection_reference)) {
|
||||
map_projection_init(&_projection_reference, global_position.lat, global_position.lon);
|
||||
if (!_projection_reference.isInitialized()) {
|
||||
_projection_reference.initReference(global_position.lat, global_position.lon);
|
||||
}
|
||||
|
||||
float x1, y1, x2, y2;
|
||||
map_projection_project(&_projection_reference, lat, lon, &x1, &y1);
|
||||
map_projection_project(&_projection_reference, global_position.lat, global_position.lon, &x2, &y2);
|
||||
_projection_reference.project(lat, lon, x1, y1);
|
||||
_projection_reference.project(global_position.lat, global_position.lon, x2, y2);
|
||||
float dx = x1 - x2, dy = y1 - y2;
|
||||
float target_distance = sqrtf(dx * dx + dy * dy);
|
||||
float z = altitude - global_position.alt;
|
||||
|
|
|
@ -101,7 +101,7 @@ protected:
|
|||
float _calculate_pitch(double lon, double lat, float altitude,
|
||||
const vehicle_global_position_s &global_position);
|
||||
|
||||
map_projection_reference_s _projection_reference = {}; ///< reference to convert (lon, lat) to local [m]
|
||||
MapProjection _projection_reference = {}; ///< reference to convert (lon, lat) to local [m]
|
||||
|
||||
const OutputConfig &_config;
|
||||
|
||||
|
|
Loading…
Reference in New Issue