Line data    Source code

       1             : #pragma once
       2             : 
       3             : #include <AP_Math/AP_Math.h>
       4             : 
       5             : #define LOCATION_ALT_MAX_M  83000   // maximum altitude (in meters) that can be fit into Location structure's alt field
       6             : 
       7             : class Location
       8             : {
       9             : public:
      10             : 
      11             :     uint8_t relative_alt : 1;           // 1 if altitude is relative to home
      12             :     uint8_t loiter_ccw   : 1;           // 0 if clockwise, 1 if counter clockwise
      13             :     uint8_t terrain_alt  : 1;           // this altitude is above terrain
      14             :     uint8_t origin_alt   : 1;           // this altitude is above ekf origin
      15             :     uint8_t loiter_xtrack : 1;          // 0 to crosstrack from center of waypoint, 1 to crosstrack from tangent exit location
      16             : 
      17             :     // note that mission storage only stores 24 bits of altitude (~ +/- 83km)
      18             :     int32_t alt; // in cm
      19             :     int32_t lat; // in 1E7 degrees
      20             :     int32_t lng; // in 1E7 degrees
      21             : 
      22             :     /// enumeration of possible altitude types
      23             :     enum class AltFrame {
      24             :         ABSOLUTE = 0,
      25             :         ABOVE_HOME = 1,
      26             :         ABOVE_ORIGIN = 2,
      27             :         ABOVE_TERRAIN = 3
      28             :     };
      29             : 
      30             :     /// constructors
      31  2113597493 :     Location() { zero(); }
      32             :     Location(int32_t latitude, int32_t longitude, int32_t alt_in_cm, AltFrame frame);
      33             :     Location(const Vector3f &ekf_offset_neu_cm, AltFrame frame);
      34             :     Location(const Vector3d &ekf_offset_neu_cm, AltFrame frame);
      35             : 
      36             :     // named constructors
      37             :     static Location from_ekf_offset_NED_m(const Vector3f& ekf_offset_ned_m, AltFrame frame);
      38             :     static Location from_ekf_offset_NED_m(const Vector3d& ekf_offset_ned_m, AltFrame frame);
      39             : 
      40             :     // set altitude
      41             :     void set_alt_cm(int32_t alt_cm, AltFrame frame);
      42             :     // set_alt_m - set altitude in metres
      43    31422645 :     void set_alt_m(float alt_m, AltFrame frame) {
      44    31422645 :         set_alt_cm(alt_m*100, frame);
      45    31422645 :     }
      46             : 
      47             :     // get altitude (in cm) in the desired frame
      48             :     // does not modify ret_alt_cm unless true is returned
      49             :     // returns false on failure to get altitude in the desired frame which can only happen if the original frame or desired frame is:
      50             :     // - above-terrain and the terrain database can't supply terrain height amsl
      51             :     // - above-home and home is not set
      52             :     // - above-origin and origin is not set
      53             :     bool get_alt_cm(AltFrame desired_frame, int32_t &ret_alt_cm) const WARN_IF_UNUSED;
      54             :     // same as get_alt_cm but in metres:
      55             :     bool get_alt_m(AltFrame desired_frame, float &ret_alt) const WARN_IF_UNUSED;
      56             : 
      57             :     // get altitude frame
      58             :     AltFrame get_alt_frame() const;
      59             : 
      60             :     // converts altitude to new frame
      61             :     // returns false on failure to convert which can only happen if the original frame or desired frame is:
      62             :     // - above-terrain and the terrain database can't supply terrain height amsl
      63             :     // - above-home and home is not set
      64             :     // - above-origin and origin is not set
      65             :     bool change_alt_frame(AltFrame desired_frame);
      66             : 
      67             :     // copy altitude and its frame of other Location object:
      68             :     void copy_alt_from(const Location &other);
      69             : 
      70             :     // get position as a vector (in cm) from origin (x,y only or
      71             :     // x,y,z) return false on failure to get the vector which can only
      72             :     // happen if the EKF origin has not been set yet x, y and z are in
      73             :     // centimetres.  If this method returns false then vec_ne is
      74             :     // unmodified.
      75             :     template<typename T>
      76             :     bool get_vector_xy_from_origin_NE_cm(T &vec_ne) const WARN_IF_UNUSED;
      77             :     // converts location to a vector from origin; if this method returns
      78             :     // false then vec_neu is unmodified
      79             :     template<typename T>
      80             :     bool get_vector_from_origin_NEU_cm(T &vec_neu) const WARN_IF_UNUSED;
      81             :     // same as get_vector_from_origin_NEU_cm, but only here so we can
      82             :     // continue to use it in LUA scripts:
      83             :     template<typename T>
      84             :     bool get_vector_from_origin_NEU(T &vec_neu) const WARN_IF_UNUSED;
      85             : 
      86             :     // get position as a vector (in metres) from origin (x,y only or
      87             :     // x,y,z) return false on failure to get the vector which can only
      88             :     // happen if the EKF origin has not been set yet x, y and z are in
      89             :     // metres.  If this method returns false then vec_ne is
      90             :     // unmodified.
      91             :     template<typename T>
      92             :     bool get_vector_xy_from_origin_NE_m(T &vec_ne) const;
      93             :     template<typename T>
      94             :     bool get_vector_from_origin_NEU_m(T &vec_neu) const;
      95             :     template<typename T>
      96             :     bool get_vector_from_origin_NED_m(T &vec_ned) const;
      97             : 
      98             :     // return horizontal distance in meters between two locations
      99             :     ftype get_distance(const Location &loc2) const;
     100             : 
     101             :     // return the altitude difference in meters taking into account
     102             :     // alt frame.  if loc2 is below this location then "distance" will
     103             :     // be positive.  ie. this method returns how far above loc2 this
     104             :     // location is.
     105             :     bool get_height_above(const Location &loc2, ftype &distance) const WARN_IF_UNUSED;
     106             : 
     107             :     // return the distance in meters in North/East/Down plane as a N/E/D vector to loc2
     108             :     // NOT CONSIDERING ALT FRAME!
     109             :     Vector3f get_distance_NED(const Location &loc2) const;
     110             :     Vector3p get_distance_NED_postype(const Location &loc2) const;
     111             :     Vector3d get_distance_NED_double(const Location &loc2) const;
     112             : 
     113             :     // return the distance in meters in North/East/Down plane as a N/E/D vector to loc2 considering alt frame, if altitude cannot be resolved down distance is 0
     114             :     Vector3f get_distance_NED_alt_frame(const Location &loc2) const;
     115             : 
     116             :     // return the distance in meters in North/East plane as a N/E vector to loc2
     117             :     Vector2f get_distance_NE(const Location &loc2) const;
     118             :     Vector2p get_distance_NE_postype(const Location &loc2) const;
     119             :     Vector2d get_distance_NE_double(const Location &loc2) const;
     120             :     Vector2F get_distance_NE_ftype(const Location &loc2) const;
     121             : 
     122             :     // extrapolate latitude/longitude given distances (in meters) north and east
     123             :     static void offset_latlng(int32_t &lat, int32_t &lng, ftype ofs_north, ftype ofs_east);
     124             :     void offset(ftype ofs_north, ftype ofs_east);
     125             :     // extrapolate latitude/longitude given distances (in meters) north
     126             :     // and east. Note that this is metres, *even for the altitude*.
     127             :     void offset(const Vector3p &ofs_ned);
     128       16024 :     void offset_up_cm(int32_t alt_offset_cm) {
     129       16024 :         alt += alt_offset_cm;
     130       16024 :     }
     131      190055 :     void offset_up_m(float alt_offset_m) {
     132      190055 :         alt += alt_offset_m * 100;
     133      190055 :     }
     134             : 
     135             :     // extrapolate latitude/longitude given bearing and distance
     136             :     void offset_bearing(ftype bearing_deg, ftype distance);
     137             :     
     138             :     // extrapolate latitude/longitude given bearing, pitch and distance
     139             :     void offset_bearing_and_pitch(ftype bearing_deg, ftype pitch_deg, ftype distance);
     140             : 
     141             :     // longitude_scale - returns the scaler to compensate for
     142             :     // shrinking longitude as you move north or south from the equator
     143             :     // Note: this does not include the scaling to convert
     144             :     // longitude/latitude points to meters or centimeters
     145             :     static ftype longitude_scale(int32_t lat);
     146             : 
     147             :     bool is_zero(void) const WARN_IF_UNUSED;
     148             : 
     149             :     void zero(void);
     150             : 
     151             :     // return the bearing in radians, from 0 to 2*Pi
     152             :     ftype get_bearing(const Location &loc2) const;
     153             : 
     154             :     // return bearing in centi-degrees from location to loc2, return is 0 to 35999
     155     1506264 :     int32_t get_bearing_to(const Location &loc2) const {
     156     1506264 :         return int32_t(rad_to_cd(get_bearing(loc2)) + 0.5);
     157             :     }
     158             : 
     159             :     // check if lat and lng match. Ignore altitude and options
     160             :     bool same_latlon_as(const Location &loc2) const;
     161             : 
     162             :     // check if altitude matches.
     163             :     bool same_alt_as(const Location &loc2) const;
     164             : 
     165             :     // check if lat, lng, and alt match.
     166       33471 :     bool same_loc_as(const Location &loc2) const {
     167       33471 :         return same_latlon_as(loc2) && same_alt_as(loc2);
     168             :     }
     169             : 
     170             :     /*
     171             :      * convert invalid waypoint with useful data. return true if location changed
     172             :      */
     173             :     bool sanitize(const Location &defaultLoc);
     174             : 
     175             :     // return true when lat and lng are within range
     176             :     bool check_latlng() const;
     177             : 
     178             :     // see if location is past a line perpendicular to
     179             :     // the line between point1 and point2 and passing through point2.
     180             :     // If point1 is our previous waypoint and point2 is our target waypoint
     181             :     // then this function returns true if we have flown past
     182             :     // the target waypoint
     183             :     bool past_interval_finish_line(const Location &point1, const Location &point2) const;
     184             : 
     185             :     /*
     186             :       return the proportion we are along the path from point1 to
     187             :       point2, along a line parallel to point1<->point2.
     188             :       This will be more than 1 if we have passed point2
     189             :      */
     190             :     float line_path_proportion(const Location &point1, const Location &point2) const;
     191             : 
     192             :     // update altitude and alt-frame base on this location's horizontal position between point1 and point2
     193             :     // this location's lat,lon is used to calculate the alt of the closest point on the line between point1 and point2
     194             :     // origin and destination's altitude frames must be the same
     195             :     // this alt-frame will be updated to match the destination alt frame
     196             :     void linearly_interpolate_alt(const Location &point1, const Location &point2);
     197             : 
     198   127425144 :     bool initialised() const { return (lat !=0 || lng != 0 || alt != 0); }
     199             :     // return true if alt is *exactly* zero.  This is not intended to
     200             :     // be used for anything except magically changing a 0-altitude
     201             :     // passed via mavlink into the current vehicle's altitude
     202        2644 :     bool alt_is_zero() const { return alt == 0; }
     203             : 
     204             :     // wrap longitude at -180e7 to 180e7
     205             :     static int32_t wrap_longitude(int64_t lon);
     206             : 
     207             :     // limit latitude to -90e7 to 90e7
     208             :     static int32_t limit_lattitude(int32_t lat);
     209             :     
     210             :     // get lon1-lon2, wrapping at -180e7 to 180e7
     211             :     static int32_t diff_longitude(int32_t lon1, int32_t lon2);
     212             : 
     213             : private:
     214             : 
     215             :     // scaling factor from 1e-7 degrees to meters at equator
     216             :     // == 1.0e-7 * DEG_TO_RAD * RADIUS_OF_EARTH
     217             :     static constexpr float LOCATION_SCALING_FACTOR = LATLON_TO_M;
     218             :     // inverse of LOCATION_SCALING_FACTOR
     219             :     static constexpr float LOCATION_SCALING_FACTOR_INV = LATLON_TO_M_INV;
     220             : };