PX4 Firmware
PX4 Autopilot Software http://px4.io
|
#include "geo.h"
#include <ecl.h>
#include <mathlib/mathlib.h>
#include <matrix/math.hpp>
#include <float.h>
Go to the source code of this file.
Functions | |
bool | map_projection_global_initialized () |
Checks if global projection was initialized. More... | |
bool | map_projection_initialized (const struct map_projection_reference_s *ref) |
Checks if projection given as argument was initialized. More... | |
uint64_t | map_projection_global_timestamp () |
Get the timestamp of the global map projection. More... | |
uint64_t | map_projection_timestamp (const struct map_projection_reference_s *ref) |
Get the timestamp of the map projection given by the argument. More... | |
int | map_projection_global_init (double lat_0, double lon_0, uint64_t timestamp) |
Initializes the global map transformation. More... | |
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. More... | |
int | map_projection_init (struct map_projection_reference_s *ref, double lat_0, double lon_0) |
Initializes the map transformation given by the argument and sets the timestamp to now. More... | |
int | map_projection_global_reference (double *ref_lat_rad, double *ref_lon_rad) |
Writes the reference values of the global projection to ref_lat and ref_lon. More... | |
int | map_projection_reference (const struct map_projection_reference_s *ref, double *ref_lat_rad, double *ref_lon_rad) |
Writes the reference values of the projection given by the argument to ref_lat and ref_lon. More... | |
int | map_projection_global_project (double lat, double lon, float *x, float *y) |
Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane using the global projection. More... | |
int | map_projection_project (const struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y) |
int | map_projection_global_reproject (float x, float y, double *lat, double *lon) |
Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system using the global projection. More... | |
int | map_projection_reproject (const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon) |
Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system using the projection given by the argument. More... | |
int | map_projection_global_getref (double *lat_0, double *lon_0) |
Get reference position of the global map projection. More... | |
int | globallocalconverter_init (double lat_0, double lon_0, float alt_0, uint64_t timestamp) |
Initialize the global mapping between global position (spherical) and local position (NED). More... | |
bool | globallocalconverter_initialized () |
Checks if globallocalconverter was initialized. More... | |
int | globallocalconverter_tolocal (double lat, double lon, float alt, float *x, float *y, float *z) |
Convert from global position coordinates to local position coordinates using the global reference. More... | |
int | globallocalconverter_toglobal (float x, float y, float z, double *lat, double *lon, float *alt) |
Convert from local position coordinates to global position coordinates using the global reference. More... | |
int | globallocalconverter_getref (double *lat_0, double *lon_0, float *alt_0) |
Get reference position of the global to local converter. More... | |
float | get_distance_to_next_waypoint (double lat_now, double lon_now, double lat_next, double lon_next) |
Returns the distance to the next waypoint in meters. More... | |
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) |
Creates a new waypoint C on the line of two given waypoints (A, B) at certain distance from waypoint A. More... | |
void | waypoint_from_heading_and_distance (double lat_start, double lon_start, float bearing, float dist, double *lat_target, double *lon_target) |
Creates a waypoint from given waypoint, distance and bearing see http://www.movable-type.co.uk/scripts/latlong.html. More... | |
float | get_bearing_to_next_waypoint (double lat_now, double lon_now, double lat_next, double lon_next) |
Returns the bearing to the next waypoint in radians. More... | |
void | get_vector_to_next_waypoint (double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, float *v_e) |
void | get_vector_to_next_waypoint_fast (double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, float *v_e) |
void | add_vector_to_global_position (double lat_now, double lon_now, float v_n, float v_e, double *lat_res, double *lon_res) |
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) |
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) |
float | get_distance_to_point_global_wgs84 (double lat_now, double lon_now, float alt_now, double lat_next, double lon_next, float alt_next, float *dist_xy, float *dist_z) |
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) |
Variables | |
static struct map_projection_reference_s | mp_ref |
static struct globallocal_converter_reference_s | gl_ref = {0.0f, false} |
void add_vector_to_global_position | ( | double | lat_now, |
double | lon_now, | ||
float | v_n, | ||
float | v_e, | ||
double * | lat_res, | ||
double * | lon_res | ||
) |
Definition at line 364 of file geo.cpp.
References CONSTANTS_RADIUS_OF_EARTH, matrix::cos(), math::degrees(), and math::radians().
Referenced by Ekf2::apply_gps_offsets(), Ekf2::calc_gps_blend_output(), and Ekf2::update_gps_blend_states().
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 | ||
) |
Creates a new waypoint C on the line of two given waypoints (A, B) at certain distance from waypoint A.
lat_A | waypoint A latitude in degrees (47.1234567°, not 471234567°) |
lon_A | waypoint A longitude in degrees (8.1234567°, not 81234567°) |
lat_B | waypoint B latitude in degrees (47.1234567°, not 471234567°) |
lon_B | waypoint B longitude in degrees (8.1234567°, not 81234567°) |
dist | distance of target waypoint from waypoint A in meters (can be negative) |
lat_target | latitude of target waypoint C in degrees (47.1234567°, not 471234567°) |
lon_target | longitude of target waypoint C in degrees (47.1234567°, not 471234567°) |
Definition at line 285 of file geo.cpp.
References FLT_EPSILON, get_bearing_to_next_waypoint(), M_PI_F, waypoint_from_heading_and_distance(), and matrix::wrap_2pi().
Referenced by FixedwingPositionControl::control_landing(), and FixedwingPositionControl::get_waypoint_heading_distance().
float get_bearing_to_next_waypoint | ( | double | lat_now, |
double | lon_now, | ||
double | lat_next, | ||
double | lon_next | ||
) |
Returns the bearing to the next waypoint in radians.
lat_now | current position in degrees (47.1234567°, not 471234567°) |
lon_now | current position in degrees (8.1234567°, not 81234567°) |
lat_next | next waypoint position in degrees (47.1234567°, not 471234567°) |
lon_next | next waypoint position in degrees (8.1234567°, not 81234567°) |
Definition at line 320 of file geo.cpp.
References matrix::cos(), math::radians(), matrix::sin(), and matrix::wrap_pi().
Referenced by vmount::OutputBase::_handle_position_update(), build_gps_response(), FixedwingPositionControl::control_landing(), create_waypoint_from_line_and_dist(), get_distance_to_arc(), get_distance_to_line(), Mission::heading_sp_update(), MissionBlock::is_mission_item_reached(), ECL_L1_Pos_Controller::navigate_loiter(), ECL_L1_Pos_Controller::navigate_waypoints(), FollowTarget::on_active(), Mission::set_align_mission_item(), Mission::set_mission_items(), and RTL::set_rtl_item().
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 | ||
) |
Definition at line 419 of file geo.cpp.
References crosstrack_error_s::bearing, matrix::cos(), crosstrack_error_s::distance, f(), get_bearing_to_next_waypoint(), get_distance_to_next_waypoint(), M_PI_F, crosstrack_error_s::past_end, matrix::sin(), and matrix::wrap_pi().
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 | ||
) |
Definition at line 375 of file geo.cpp.
References crosstrack_error_s::bearing, crosstrack_error_s::distance, f(), get_bearing_to_next_waypoint(), get_distance_to_next_waypoint(), crosstrack_error_s::past_end, and matrix::wrap_pi().
Referenced by Navigator::check_traffic().
float get_distance_to_next_waypoint | ( | double | lat_now, |
double | lon_now, | ||
double | lat_next, | ||
double | lon_next | ||
) |
Returns the distance to the next waypoint in meters.
lat_now | current position in degrees (47.1234567°, not 471234567°) |
lon_now | current position in degrees (8.1234567°, not 81234567°) |
lat_next | next waypoint position in degrees (47.1234567°, not 471234567°) |
lon_next | next waypoint position in degrees (8.1234567°, not 81234567°) |
Definition at line 270 of file geo.cpp.
References matrix::atan2(), CONSTANTS_RADIUS_OF_EARTH, matrix::cos(), math::radians(), matrix::sin(), and matrix::sqrt().
Referenced by Mission::altitude_sp_foh_update(), build_gps_response(), RTL::calculate_return_alt_from_cone_half_angle(), MissionFeasibilityChecker::checkDistancesBetweenWaypoints(), MissionFeasibilityChecker::checkDistanceToFirstWaypoint(), MissionFeasibilityChecker::checkFixedWingLanding(), FixedwingPositionControl::control_landing(), RoverPositionControl::control_position(), FixedwingPositionControl::control_position(), Mission::do_need_move_to_land(), Mission::do_need_move_to_takeoff(), get_distance_to_arc(), get_distance_to_line(), Mission::heading_sp_update(), MissionBlock::is_mission_item_reached(), RoverPositionControl::run(), PrecLand::run_state_start(), Mission::set_mission_items(), RTL::set_rtl_item(), and FixedwingPositionControl::status_publish().
float get_distance_to_point_global_wgs84 | ( | double | lat_now, |
double | lon_now, | ||
float | alt_now, | ||
double | lat_next, | ||
double | lon_next, | ||
float | alt_next, | ||
float * | dist_xy, | ||
float * | dist_z | ||
) |
Definition at line 523 of file geo.cpp.
References matrix::atan2(), CONSTANTS_RADIUS_OF_EARTH, matrix::cos(), M_PI, matrix::sin(), and matrix::sqrt().
Referenced by Navigator::check_traffic(), Geofence::checkAll(), Mission::index_closest_mission_item(), and MissionBlock::is_mission_item_reached().
void get_vector_to_next_waypoint | ( | double | lat_now, |
double | lon_now, | ||
double | lat_next, | ||
double | lon_next, | ||
float * | v_n, | ||
float * | v_e | ||
) |
Definition at line 337 of file geo.cpp.
References CONSTANTS_RADIUS_OF_EARTH, matrix::cos(), math::radians(), and matrix::sin().
Referenced by Ekf2::calc_gps_blend_output(), Ekf2::update_gps_blend_states(), and Ekf2::update_gps_offsets().
void get_vector_to_next_waypoint_fast | ( | double | lat_now, |
double | lon_now, | ||
double | lat_next, | ||
double | lon_next, | ||
float * | v_n, | ||
float * | v_e | ||
) |
Definition at line 349 of file geo.cpp.
References CONSTANTS_RADIUS_OF_EARTH, matrix::cos(), and math::radians().
int globallocalconverter_getref | ( | double * | lat_0, |
double * | lon_0, | ||
float * | alt_0 | ||
) |
Get reference position of the global to local converter.
Definition at line 253 of file geo.cpp.
References globallocal_converter_reference_s::alt, map_projection_global_getref(), and map_projection_global_initialized().
Referenced by BlockLocalPositionEstimator::mocapInit(), and BlockLocalPositionEstimator::visionInit().
int globallocalconverter_init | ( | double | lat_0, |
double | lon_0, | ||
float | alt_0, | ||
uint64_t | timestamp | ||
) |
Initialize the global mapping between global position (spherical) and local position (NED).
Definition at line 211 of file geo.cpp.
References globallocal_converter_reference_s::alt, globallocal_converter_reference_s::init_done, and map_projection_global_init().
Referenced by FlightTask::_evaluateVehicleLocalPosition(), MavlinkReceiver::handle_message_gps_global_origin(), MavlinkReceiver::handle_message_set_position_target_global_int(), RoverPositionControl::run(), and FixedwingPositionControl::Run().
bool globallocalconverter_initialized | ( | void | ) |
Checks if globallocalconverter was initialized.
Definition at line 224 of file geo.cpp.
References globallocal_converter_reference_s::init_done, and map_projection_global_initialized().
Referenced by MavlinkReceiver::handle_message_gps_global_origin(), MavlinkReceiver::handle_message_set_position_target_global_int(), BlockLocalPositionEstimator::mocapInit(), RoverPositionControl::run(), FixedwingPositionControl::Run(), and BlockLocalPositionEstimator::visionInit().
int globallocalconverter_toglobal | ( | float | x, |
float | y, | ||
float | z, | ||
double * | lat, | ||
double * | lon, | ||
float * | alt | ||
) |
Convert from local position coordinates to global position coordinates using the global reference.
Definition at line 241 of file geo.cpp.
References globallocal_converter_reference_s::alt, map_projection_global_initialized(), and map_projection_global_reproject().
Referenced by RoverPositionControl::run(), FixedwingPositionControl::Run(), and FlightTaskOrbit::sendTelemetry().
int globallocalconverter_tolocal | ( | double | lat, |
double | lon, | ||
float | alt, | ||
float * | x, | ||
float * | y, | ||
float * | z | ||
) |
Convert from global position coordinates to local position coordinates using the global reference.
Definition at line 229 of file geo.cpp.
References globallocal_converter_reference_s::alt, map_projection_global_initialized(), and map_projection_global_project().
Referenced by FlightTaskOrbit::applyCommandParameters(), and MavlinkReceiver::handle_message_set_position_target_global_int().
int map_projection_global_getref | ( | double * | lat_0, |
double * | lon_0 | ||
) |
Get reference position of the global map projection.
Definition at line 194 of file geo.cpp.
References math::degrees(), map_projection_reference_s::lat_rad, map_projection_reference_s::lon_rad, map_projection_global_initialized(), and mp_ref.
Referenced by globallocalconverter_getref().
int map_projection_global_init | ( | double | lat_0, |
double | lon_0, | ||
uint64_t | timestamp | ||
) |
Initializes the global map transformation.
Initializes the transformation between the geographic coordinate system and the azimuthal equidistant plane
lat | in degrees (47.1234567°, not 471234567°) |
lon | in degrees (8.1234567°, not 81234567°) |
Definition at line 84 of file geo.cpp.
References map_projection_init_timestamped(), and mp_ref.
Referenced by globallocalconverter_init().
bool map_projection_global_initialized | ( | ) |
Checks if global projection was initialized.
Definition at line 63 of file geo.cpp.
References map_projection_initialized(), and mp_ref.
Referenced by globallocalconverter_getref(), globallocalconverter_initialized(), globallocalconverter_toglobal(), globallocalconverter_tolocal(), and map_projection_global_getref().
int map_projection_global_project | ( | double | lat, |
double | lon, | ||
float * | x, | ||
float * | y | ||
) |
Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane using the global projection.
x | north |
y | east |
lat | in degrees (47.1234567°, not 471234567°) |
lon | in degrees (8.1234567°, not 81234567°) |
Definition at line 127 of file geo.cpp.
References map_projection_project(), and mp_ref.
Referenced by globallocalconverter_tolocal().
int map_projection_global_reference | ( | double * | ref_lat_rad, |
double * | ref_lon_rad | ||
) |
Writes the reference values of the global projection to ref_lat and ref_lon.
Definition at line 110 of file geo.cpp.
References map_projection_reference(), and mp_ref.
int map_projection_global_reproject | ( | float | x, |
float | y, | ||
double * | lat, | ||
double * | lon | ||
) |
Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system using the global projection.
x | north |
y | east |
lat | in degrees (47.1234567°, not 471234567°) |
lon | in degrees (8.1234567°, not 81234567°) |
Definition at line 161 of file geo.cpp.
References map_projection_reproject(), and mp_ref.
Referenced by globallocalconverter_toglobal().
uint64_t map_projection_global_timestamp | ( | void | ) |
Get the timestamp of the global map projection.
Definition at line 73 of file geo.cpp.
References map_projection_timestamp(), and mp_ref.
int map_projection_init | ( | struct map_projection_reference_s * | ref, |
double | lat_0, | ||
double | lon_0 | ||
) |
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
lat | in degrees (47.1234567°, not 471234567°) |
lon | in degrees (8.1234567°, not 81234567°) |
Definition at line 105 of file geo.cpp.
References ecl_absolute_time, and map_projection_init_timestamped().
Referenced by vmount::OutputBase::_calculate_pitch(), FlightTaskAuto::_evaluateGlobalReference(), BlockLocalPositionEstimator::gpsInit(), Commander::handle_command(), MavlinkReceiver::handle_message_hil_state_quaternion(), Simulator::handle_message_hil_state_quaternion(), Geofence::insideCircle(), BlockLocalPositionEstimator::mocapInit(), PrecLand::on_activation(), FollowTarget::on_active(), BlockLocalPositionEstimator::Run(), and BlockLocalPositionEstimator::visionInit().
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.
Initializes the transformation between the geographic coordinate system and the azimuthal equidistant plane
lat | in degrees (47.1234567°, not 471234567°) |
lon | in degrees (8.1234567°, not 81234567°) |
Definition at line 90 of file geo.cpp.
References matrix::cos(), map_projection_reference_s::cos_lat, map_projection_reference_s::init_done, map_projection_reference_s::lat_rad, map_projection_reference_s::lon_rad, math::radians(), matrix::sin(), map_projection_reference_s::sin_lat, and map_projection_reference_s::timestamp.
Referenced by Ekf::collect_gps(), Ekf::gps_is_good(), map_projection_global_init(), and map_projection_init().
bool map_projection_initialized | ( | const struct map_projection_reference_s * | ref | ) |
Checks if projection given as argument was initialized.
Definition at line 68 of file geo.cpp.
References map_projection_reference_s::init_done.
Referenced by vmount::OutputBase::_calculate_pitch(), Geofence::insideCircle(), map_projection_global_initialized(), map_projection_project(), map_projection_reference(), map_projection_reproject(), and PrecLand::on_activation().
int map_projection_project | ( | const struct map_projection_reference_s * | ref, |
double | lat, | ||
double | lon, | ||
float * | x, | ||
float * | y | ||
) |
Definition at line 132 of file geo.cpp.
References matrix::acos(), CONSTANTS_RADIUS_OF_EARTH, math::constrain(), matrix::cos(), map_projection_reference_s::cos_lat, map_projection_reference_s::lon_rad, map_projection_initialized(), math::radians(), matrix::sin(), and map_projection_reference_s::sin_lat.
Referenced by vmount::OutputBase::_calculate_pitch(), FlightTaskAuto::_evaluateTriplets(), Ekf::gps_is_good(), BlockLocalPositionEstimator::gpsCorrect(), Commander::handle_command(), MavlinkReceiver::handle_message_hil_state_quaternion(), Simulator::handle_message_hil_state_quaternion(), Geofence::insideCircle(), map_projection_global_project(), FollowTarget::on_active(), EstimatorInterface::setGpsData(), and PrecLand::slewrate().
int map_projection_reference | ( | const struct map_projection_reference_s * | ref, |
double * | ref_lat_rad, | ||
double * | ref_lon_rad | ||
) |
Writes the reference values of the projection given by the argument to ref_lat and ref_lon.
Definition at line 115 of file geo.cpp.
References map_projection_reference_s::lat_rad, map_projection_reference_s::lon_rad, and map_projection_initialized().
Referenced by map_projection_global_reference().
int map_projection_reproject | ( | const struct map_projection_reference_s * | ref, |
float | x, | ||
float | y, | ||
double * | lat, | ||
double * | lon | ||
) |
Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system using the projection given by the argument.
x | north |
y | east |
lat | in degrees (47.1234567°, not 471234567°) |
lon | in degrees (8.1234567°, not 81234567°) |
Definition at line 166 of file geo.cpp.
References matrix::asin(), matrix::atan2(), CONSTANTS_RADIUS_OF_EARTH, matrix::cos(), map_projection_reference_s::cos_lat, math::degrees(), map_projection_reference_s::lat_rad, map_projection_reference_s::lon_rad, map_projection_initialized(), matrix::sin(), map_projection_reference_s::sin_lat, and matrix::sqrt().
Referenced by Ekf::collect_gps(), BlockLocalPositionEstimator::gpsInit(), map_projection_global_reproject(), FollowTarget::on_active(), BlockLocalPositionEstimator::publishGlobalPos(), Ekf2::Run(), PrecLand::run_state_descend_above_target(), and PrecLand::run_state_horizontal_approach().
uint64_t map_projection_timestamp | ( | const struct map_projection_reference_s * | ref | ) |
Get the timestamp of the map projection given by the argument.
Definition at line 78 of file geo.cpp.
References map_projection_reference_s::timestamp.
Referenced by map_projection_global_timestamp().
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 | ||
) |
Definition at line 547 of file geo.cpp.
Referenced by Commander::run().
void waypoint_from_heading_and_distance | ( | double | lat_start, |
double | lon_start, | ||
float | bearing, | ||
float | dist, | ||
double * | lat_target, | ||
double * | lon_target | ||
) |
Creates a waypoint from given waypoint, distance and bearing see http://www.movable-type.co.uk/scripts/latlong.html.
lat_start | latitude of starting waypoint in degrees (47.1234567°, not 471234567°) |
lon_start | longitude of starting waypoint in degrees (8.1234567°, not 81234567°) |
bearing | in rad |
distance | in meters |
lat_target | latitude of target waypoint in degrees (47.1234567°, not 471234567°) |
lon_target | longitude of target waypoint in degrees (47.1234567°, not 471234567°) |
Definition at line 303 of file geo.cpp.
References matrix::asin(), matrix::atan2(), CONSTANTS_RADIUS_OF_EARTH, matrix::cos(), math::degrees(), math::radians(), matrix::sin(), and matrix::wrap_2pi().
Referenced by Navigator::check_traffic(), create_waypoint_from_line_and_dist(), Navigator::fake_traffic(), Mission::generate_waypoint_from_heading(), FixedwingPositionControl::get_waypoint_heading_distance(), and MissionBlock::is_mission_item_reached().
|
static |
|
static |
Definition at line 60 of file geo.cpp.
Referenced by map_projection_global_getref(), map_projection_global_init(), map_projection_global_initialized(), map_projection_global_project(), map_projection_global_reference(), map_projection_global_reproject(), and map_projection_global_timestamp().