forked from Archive/PX4-Autopilot
geo: refactoring on comments and usage
This commit is contained in:
parent
8db7a6225b
commit
10ceea2fe6
|
@ -44,8 +44,6 @@
|
|||
|
||||
#include "geo.h"
|
||||
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <matrix/math.hpp>
|
||||
#include <float.h>
|
||||
|
||||
using matrix::wrap_pi;
|
||||
|
@ -60,19 +58,16 @@ using matrix::wrap_2pi;
|
|||
* formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html
|
||||
*/
|
||||
|
||||
|
||||
void MapProjection::initReference(double lat_0, double lon_0, uint64_t timestamp)
|
||||
{
|
||||
_ref_timestamp = timestamp;
|
||||
_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;
|
||||
}
|
||||
|
||||
|
||||
void MapProjection::project(double lat, double lon, float &x, float &y) const
|
||||
{
|
||||
const double lat_rad = math::radians(lat);
|
||||
|
@ -116,7 +111,6 @@ void MapProjection::reproject(float x, float y, double &lat, double &lon) const
|
|||
lat = math::degrees(_ref_lat);
|
||||
lon = math::degrees(_ref_lon);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
|
||||
|
|
|
@ -48,9 +48,8 @@
|
|||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include <matrix/Matrix.hpp>
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <lib/mathlib/mathlib.h>
|
||||
#include <lib/matrix/matrix/math.hpp>
|
||||
|
||||
static constexpr float CONSTANTS_ONE_G = 9.80665f; // m/s^2
|
||||
|
||||
|
@ -164,11 +163,11 @@ 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};
|
||||
double _ref_lat{0.0};
|
||||
double _ref_lon{0.0};
|
||||
double _ref_sin_lat{0.0};
|
||||
double _ref_cos_lat{0.0};
|
||||
bool _ref_init_done{false};
|
||||
|
||||
public:
|
||||
/**
|
||||
|
@ -179,7 +178,7 @@ public:
|
|||
MapProjection() = default;
|
||||
|
||||
/**
|
||||
* @brief Construct and initializes a new Map Projection object
|
||||
* @brief Construct and initialize a new Map Projection object
|
||||
*/
|
||||
MapProjection(double lat_0, double lon_0)
|
||||
{
|
||||
|
@ -187,7 +186,7 @@ public:
|
|||
}
|
||||
|
||||
/**
|
||||
* @brief Construct and initializes a new Map Projection object
|
||||
* @brief Construct and initialize a new Map Projection object
|
||||
*/
|
||||
MapProjection(double lat_0, double lon_0, uint64_t timestamp)
|
||||
{
|
||||
|
@ -195,7 +194,7 @@ public:
|
|||
}
|
||||
|
||||
/**
|
||||
* Initializes the map transformation
|
||||
* Initialize the map transformation
|
||||
*
|
||||
* Initializes the transformation between the geographic coordinate system and
|
||||
* the azimuthal equidistant plane
|
||||
|
@ -205,10 +204,10 @@ public:
|
|||
void initReference(double lat_0, double lon_0, uint64_t timestamp);
|
||||
|
||||
/**
|
||||
* Initializes the map transformation
|
||||
* Initialize the map transformation
|
||||
*
|
||||
* Initializes the transformation between the geographic coordinate system and
|
||||
* the azimuthal equidistant plane
|
||||
* with reference coordinates on the geographic coordinate system
|
||||
* where the azimuthal equidistant plane's origin is located
|
||||
* @param lat in degrees (47.1234567°, not 471234567°)
|
||||
* @param lon in degrees (8.1234567°, not 81234567°)
|
||||
*/
|
||||
|
@ -218,50 +217,37 @@ public:
|
|||
}
|
||||
|
||||
/**
|
||||
* @brief Returns true, if the map reference has been initialized before
|
||||
* @return true, if the map reference has been initialized before
|
||||
*/
|
||||
inline bool isInitialized() const
|
||||
{
|
||||
return _ref_init_done;
|
||||
}
|
||||
bool isInitialized() const { return _ref_init_done; };
|
||||
|
||||
/**
|
||||
* Get the timestamp of the map projection
|
||||
* @return the timestamp of the map_projection
|
||||
* @return the timestamp of the reference which the map projection was initialized with
|
||||
*/
|
||||
inline uint64_t getProjectionReferenceTimestamp() const
|
||||
{
|
||||
return _ref_timestamp;
|
||||
}
|
||||
uint64_t getProjectionReferenceTimestamp() const { return _ref_timestamp; };
|
||||
|
||||
/**
|
||||
* @brief Get the Projection Reference latitude in degrees
|
||||
* @return the projection reference latitude in degrees
|
||||
*/
|
||||
inline double getProjectionReferenceLat() const
|
||||
{
|
||||
return math::degrees(_ref_lat);
|
||||
}
|
||||
double getProjectionReferenceLat() const { return math::degrees(_ref_lat); };
|
||||
|
||||
/**
|
||||
* @brief Get the Projection Reference longitude in degrees
|
||||
* @return the projection reference longitude in degrees
|
||||
*/
|
||||
inline double getProjectionReferenceLon() const
|
||||
{
|
||||
return math::degrees(_ref_lon);
|
||||
}
|
||||
double getProjectionReferenceLon() const { return math::degrees(_ref_lon); };
|
||||
|
||||
/**
|
||||
* Transforms a point in the geographic coordinate system to the local
|
||||
* Transform 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°)
|
||||
* @param x north
|
||||
* @param y east
|
||||
*/
|
||||
void project(double lat, double lon, float &x, float &y) const;
|
||||
|
||||
/**
|
||||
* Transforms a point in the geographic coordinate system to the local
|
||||
* Transform 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°)
|
||||
|
@ -275,14 +261,13 @@ public:
|
|||
}
|
||||
|
||||
/**
|
||||
* Transforms a point in the local azimuthal equidistant plane to the
|
||||
* Transform 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;
|
||||
};
|
||||
|
|
|
@ -157,7 +157,7 @@ public:
|
|||
// get the ekf WGS-84 origin position and height and the system time it was last set
|
||||
// return true if the origin is valid
|
||||
bool getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const;
|
||||
bool setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude);
|
||||
void setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude);
|
||||
|
||||
float getEkfGlobalOriginAltitude() const { return _gps_alt_ref; }
|
||||
bool setEkfGlobalOriginAltitude(const float altitude);
|
||||
|
|
|
@ -706,7 +706,7 @@ bool Ekf::getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &lo
|
|||
return _NED_origin_initialised;
|
||||
}
|
||||
|
||||
bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude)
|
||||
void Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude)
|
||||
{
|
||||
bool current_pos_available = false;
|
||||
double current_lat = static_cast<double>(NAN);
|
||||
|
@ -735,8 +735,6 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons
|
|||
// reset altitude
|
||||
_gps_alt_ref = altitude;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
|
@ -195,11 +195,7 @@ void EstimatorInterface::setGpsData(const gps_message &gps)
|
|||
|
||||
// Only calculate the relative position if the WGS-84 location of the origin is set
|
||||
if (collect_gps(gps)) {
|
||||
float lpos_x = 0.0f;
|
||||
float lpos_y = 0.0f;
|
||||
_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;
|
||||
gps_sample_new.pos = _pos_ref.project((gps.lat / 1.0e7), (gps.lon / 1.0e7));
|
||||
|
||||
} else {
|
||||
gps_sample_new.pos(0) = 0.0f;
|
||||
|
|
|
@ -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
|
||||
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
|
||||
MapProjection _pos_ref{}; // Contains WGS-84 position latitude and longitude of the EKF origin
|
||||
MapProjection _gps_pos_prev{}; // Contains WGS-84 position latitude and longitude 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).
|
||||
|
||||
|
|
|
@ -196,8 +196,8 @@ 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 */
|
||||
|
||||
MapProjection _reference_position{}; /**< Structure used to project lat/lon setpoint into local frame. */
|
||||
float _reference_altitude{NAN}; /**< Altitude relative to ground. */
|
||||
MapProjection _reference_position{}; /**< Class 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. */
|
||||
|
||||
WeatherVane *_ext_yaw_handler{nullptr}; /**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */
|
||||
|
|
|
@ -206,8 +206,8 @@ protected:
|
|||
virtual void _ekfResetHandlerVelocityZ(float delta_vz) {};
|
||||
virtual void _ekfResetHandlerHeading(float delta_psi) {};
|
||||
|
||||
MapProjection _geo_projection{};
|
||||
float _global_local_alt0{NAN};
|
||||
MapProjection _geo_projection{};
|
||||
float _global_local_alt0{NAN};
|
||||
|
||||
/* Time abstraction */
|
||||
static constexpr uint64_t _timeout = 500000; /**< maximal time in us before a loop or data times out */
|
||||
|
|
|
@ -297,7 +297,7 @@ private:
|
|||
MapProjection _map_ref;
|
||||
|
||||
MapProjection _global_local_proj_ref{};
|
||||
float _global_local_alt0{NAN};
|
||||
float _global_local_alt0{NAN};
|
||||
|
||||
// target mode paramters from landing_target_estimator module
|
||||
enum TargetMode {
|
||||
|
|
|
@ -135,8 +135,8 @@ void FollowTarget::on_active()
|
|||
// get distance to target
|
||||
|
||||
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));
|
||||
target_ref.project(_current_target_motion.lat, _current_target_motion.lon,
|
||||
_target_distance(0), _target_distance(1));
|
||||
|
||||
}
|
||||
|
||||
|
|
|
@ -173,7 +173,7 @@ private:
|
|||
PolygonInfo *_polygons{nullptr};
|
||||
int _num_polygons{0};
|
||||
|
||||
MapProjection _projection_reference = {}; ///< reference to convert (lon, lat) to local [m]
|
||||
MapProjection _projection_reference{}; ///< class to convert (lon, lat) to local [m]
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamInt<px4::params::GF_ACTION>) _param_gf_action,
|
||||
|
|
|
@ -278,11 +278,8 @@ PrecLand::run_state_horizontal_approach()
|
|||
slewrate(x, y);
|
||||
|
||||
// XXX need to transform to GPS coords because mc_pos_control only looks at that
|
||||
double lat, lon;
|
||||
_map_ref.reproject(x, y, lat, lon);
|
||||
_map_ref.reproject(x, y, pos_sp_triplet->current.lat, pos_sp_triplet->current.lon);
|
||||
|
||||
pos_sp_triplet->current.lat = lat;
|
||||
pos_sp_triplet->current.lon = lon;
|
||||
pos_sp_triplet->current.alt = _approach_alt;
|
||||
pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
|
||||
|
||||
|
@ -315,11 +312,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_ref.reproject(_target_pose.x_abs, _target_pose.y_abs, lat, lon);
|
||||
|
||||
pos_sp_triplet->current.lat = lat;
|
||||
pos_sp_triplet->current.lon = lon;
|
||||
_map_ref.reproject(_target_pose.x_abs, _target_pose.y_abs, pos_sp_triplet->current.lat, pos_sp_triplet->current.lon);
|
||||
|
||||
pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_LAND;
|
||||
|
||||
|
@ -555,8 +548,8 @@ void PrecLand::slewrate(float &sp_x, float &sp_y)
|
|||
dt = 50000 / SEC2USEC;
|
||||
|
||||
// set a best guess for previous setpoints for smooth transition
|
||||
_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 = _map_ref.project(_navigator->get_position_setpoint_triplet()->current.lat,
|
||||
_navigator->get_position_setpoint_triplet()->current.lon);
|
||||
_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 */
|
||||
|
||||
MapProjection _map_ref {}; /**< reference for local/global projections */
|
||||
MapProjection _map_ref{}; /**< class 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 */
|
||||
|
|
|
@ -264,8 +264,8 @@ private:
|
|||
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
|
||||
|
||||
// hil map_ref data
|
||||
MapProjection _global_local_proj_ref{};
|
||||
float _global_local_alt0{NAN};
|
||||
MapProjection _global_local_proj_ref{};
|
||||
float _global_local_alt0{NAN};
|
||||
|
||||
vehicle_status_s _vehicle_status{};
|
||||
|
||||
|
|
|
@ -547,16 +547,12 @@ void Simulator::handle_message_hil_state_quaternion(const mavlink_message_t *msg
|
|||
_global_local_alt0 = hil_state.alt / 1000.f;
|
||||
}
|
||||
|
||||
float x;
|
||||
float y;
|
||||
_global_local_proj_ref.project(lat, lon, x, y);
|
||||
hil_lpos.timestamp = timestamp;
|
||||
hil_lpos.xy_valid = true;
|
||||
hil_lpos.z_valid = true;
|
||||
hil_lpos.v_xy_valid = true;
|
||||
hil_lpos.v_z_valid = true;
|
||||
hil_lpos.x = x;
|
||||
hil_lpos.y = y;
|
||||
_global_local_proj_ref.project(lat, lon, hil_lpos.x, hil_lpos.y);
|
||||
hil_lpos.z = _global_local_alt0 - hil_state.alt / 1000.0f;
|
||||
hil_lpos.vx = hil_state.vx / 100.0f;
|
||||
hil_lpos.vy = hil_state.vy / 100.0f;
|
||||
|
|
|
@ -101,7 +101,7 @@ protected:
|
|||
float _calculate_pitch(double lon, double lat, float altitude,
|
||||
const vehicle_global_position_s &global_position);
|
||||
|
||||
MapProjection _projection_reference = {}; ///< reference to convert (lon, lat) to local [m]
|
||||
MapProjection _projection_reference{}; ///< class to convert (lon, lat) to local [m]
|
||||
|
||||
const OutputConfig &_config;
|
||||
|
||||
|
|
Loading…
Reference in New Issue