ArduPilot源码阅读——Location篇

location class类中有很多关于经纬度计算

小距离内近似获取方向角

获取当前this对象相对于loc2对象的方向角

这里采用了一种近似算法，把经纬度球面坐标系在小范围内（10KM）近似为平面直角坐标系，

采用NED形式，x轴指东表示经度，y轴指北表示纬度

// return bearing in radians from location to loc2, return is 0 to 2*Pi
ftype Location::get_bearing(const Location &loc2) const
{
    const int32_t off_x = diff_longitude(loc2.lng,lng);
    const int32_t off_y = \		//取两点中点作为经度缩放系数
		(loc2.lat - lat) / loc2.longitude_scale((lat+loc2.lat)/2);	

    ftype bearing = (M_PI*0.5) + atan2F(-off_y, off_x);	//转化角度范围0-360
    if (bearing < 0) {
        bearing += 2*M_PI;
    }
    return bearing;
}

其中diff_longitude 计算经度之差，考虑到经度符号变化

/*
  get lon1-lon2, wrapping at -180e7 to 180e7
 */
int32_t Location::diff_longitude(int32_t lon1, int32_t lon2)
{
    if ((lon1 & 0x80000000) == (lon2 & 0x80000000)) {
        // common case of same sign
        return lon1 - lon2;
    }
    int64_t dlon = int64_t(lon1)-int64_t(lon2);
    if (dlon > 1800000000LL) {
        dlon -= 3600000000LL;
    } else if (dlon < -1800000000LL) {
        dlon += 3600000000LL;
    }
    return int32_t(dlon);
}

其中 longitude_scale返回经度缩放系数，考虑在不同纬度下，每1度经度表示的距离不同

ftype Location::longitude_scale(int32_t lat)
{
    ftype scale = cosF(lat * (1.0e-7 * DEG_TO_RAD));
    return MAX(scale, 0.01);
}

小距离内近似获取平面距离

也是同理，把球体近似到二维坐标系

其中LOCATION_SCALING_FACTOR = DEG2RAD * RADIUM_EARTH

可以改写成 DEG2RAD(norm(dlat, dlng)) * RADIUM_EARTH;

// return horizontal distance in meters between two locations
ftype Location::get_distance(const Location &loc2) const
{
    ftype dlat = (ftype)(loc2.lat - lat);
    ftype dlng = \
	((ftype)diff_longitude(loc2.lng,lng)) * longitude_scale((lat+loc2.lat)/2);
    return norm(dlat, dlng) * LOCATION_SCALING_FACTOR;
}