文章目錄

• • PX4磁偏補(bǔ)償
  • 磁偏數(shù)據(jù)庫
  • 磁偏計(jì)算

PX4磁偏補(bǔ)償

PX4 姿態(tài)解算中可以通過參數(shù)直接設(shè)置磁偏角，如果有GPS也可以通過GPS坐標(biāo)查詢。

// 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))); }

磁偏數(shù)據(jù)庫

通過GPS坐標(biāo)查詢磁偏角需要首先建立磁偏數(shù)據(jù)庫，磁偏角查詢
根據(jù)px4中設(shè)置的坐標(biāo)范圍和劃分精度，建立數(shù)據(jù)庫，設(shè)置如下圖。

/** 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;

導(dǎo)出的數(shù)據(jù)庫如下圖，PX4中存儲(chǔ)的是取整后的值，會(huì)發(fā)現(xiàn)兩個(gè)表中有的數(shù)會(huì)有1度的差別，因?yàn)檫@個(gè)值是緩慢變化的。

磁偏計(jì)算

磁偏數(shù)據(jù)庫建立完成之后就可以根據(jù)GPS坐標(biāo)查詢到上圖中對應(yīng)的網(wǎng)格，PX4中采用雙線性插值，即根據(jù)網(wǎng)格四個(gè)頂點(diǎn)的磁偏角計(jì)算坐標(biāo)所在點(diǎn)的磁偏角。

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; }

對應(yīng)算法如下圖：

雙線性插值，可理解為兩次插值，分別插值出R1,R2, 再通過R1 R2插值出P.

總結(jié)

以上是生活随笔為你收集整理的PX4姿态解算磁偏补偿的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。