PX4磁偏补偿
PX4 姿态解算中可以通过参数直接设置磁偏角,如果有GPS也可以通过GPS坐标查询。
// px4 attitude_estimator_q.cpp - update_parameters
float mag_decl_deg = 0.0f;
param_get(_params_handles.mag_decl, &mag_decl_deg);
update_mag_declination(math::radians(mag_decl_deg));
if (_mag_decl_auto && gpos.eph < 20.0f && hrt_elapsed_time(&gpos.timestamp) < 1000000)
{
/* set magnetic declination automatically */
update_mag_declination(math::radians(get_mag_declination(gpos.lat, gpos.lon)));
}
磁偏数据库
通过GPS坐标查询磁偏角需要首先建立磁偏数据库,磁偏角查询
根据px4中设置的坐标范围和划分精度,建立数据库,设置如下图。
/** set this always to the sampling in degrees for the table below */
static constexpr float SAMPLING_RES = 10.0f;
static constexpr float SAMPLING_MIN_LAT = -60.0f;
static constexpr float SAMPLING_MAX_LAT = 60.0f;
static constexpr float SAMPLING_MIN_LON = -180.0f;
static constexpr float SAMPLING_MAX_LON = 180.0f;
导出的数据库如下图,PX4中存储的是取整后的值,会发现两个表中有的数会有1度的差别,因为这个值是缓慢变化的。
磁偏计算
磁偏数据库建立完成之后就可以根据GPS坐标查询到上图中对应的网格,PX4中采用双线性插值,即根据网格四个顶点的磁偏角计算坐标所在点的磁偏角。
static float
get_table_data(float lat, float lon, const int8_t table[13][37])
{
/*
* If the values exceed valid ranges, return zero as default
* as we have no way of knowing what the closest real value
* would be.
*/
if (lat < -90.0f || lat > 90.0f ||
lon < -180.0f || lon > 180.0f) {
return 0.0f;
}
/* round down to nearest sampling resolution */
float min_lat = floorf(lat / SAMPLING_RES) * SAMPLING_RES;
float min_lon = floorf(lon / SAMPLING_RES) * SAMPLING_RES;
/* find index of nearest low sampling point */
unsigned min_lat_index = get_lookup_table_index(&min_lat, SAMPLING_MIN_LAT, SAMPLING_MAX_LAT);
unsigned min_lon_index = get_lookup_table_index(&min_lon, SAMPLING_MIN_LON, SAMPLING_MAX_LON);
const float data_sw = table[min_lat_index][min_lon_index];
const float data_se = table[min_lat_index][min_lon_index + 1];
const float data_ne = table[min_lat_index + 1][min_lon_index + 1];
const float data_nw = table[min_lat_index + 1][min_lon_index];
/* perform bilinear interpolation on the four grid corners */
const float lat_scale = constrain((lat - min_lat) / SAMPLING_RES, 0.0f, 1.0f);
const float lon_scale = constrain((lon - min_lon) / SAMPLING_RES, 0.0f, 1.0f);
const float data_min = lon_scale * (data_se - data_sw) + data_sw;
const float data_max = lon_scale * (data_ne - data_nw) + data_nw;
return lat_scale * (data_max - data_min) + data_min;
}
对应算法如下图:
双线性插值,可理解为两次插值,分别插值出R1,R2, 再通过R1 R2插值出P.