文章目錄

• • PX4磁偏補償
  • 磁偏數據庫
  • 磁偏計算

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.

總結

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

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。