原文鏈接：https://blog.csdn.net/zouxu634866/article/details/79806948
#include <drivers/drv_hrt.h> //高精度的定時器。 在控制過程中多數環節都是使用經典的PID控制算法爲
//了獲取較爲實時的響應最重要的時間變量，這就涉及如何獲取高精度的時間問題。pixhawk中就有高精度的RTC（實時時鐘），這個
//頭文件就做了介紹
#include <lib/geo/geo.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp> /**濾波器**/
#include <mathlib/mathlib.h>
#include <px4_config.h>
#include <px4_posix.h>
#include <px4_tasks.h>
#include <systemlib/err.h> /**包含一些錯誤警告的功能**/
#include <systemlib/param/param.h>
#include <systemlib/perf_counter.h> /**性能評估**/
#include <uORB/topics/att_pos_mocap.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>/**我們在這個程序中解算的姿態就要發佈進這個**/
#include <uORB/topics/vehicle_global_position.h> /**去看看有什麼，後面講位置估計時要用到**/
extern "C" __EXPORT int attitude_estimator_q_main(int argc, char *argv[]);
using math::Vector; /*****使用向量****/
using math::Matrix; /*****使用矩陣****/
using math::Quaternion; /*****使用四元數****/
class AttitudeEstimatorQ;
namespace attitude_estimator_q
{
AttitudeEstimatorQ *instance;
} // namespace attitude_estimator_q attitude_estimator_q
class AttitudeEstimatorQ
{
public:
/**
* Constructor
*/
AttitudeEstimatorQ();
/**
* Destructor, also kills task.
*/
~AttitudeEstimatorQ();
/**
* Start task.
*
* @return OK on success.
*/
int start(); //創建一個新的任務
static void task_main_trampoline(int argc, char *argv[]);
void task_main();
private:
const float _dt_min = 0.00001f; /***dt是指更新數據的時間間隔，也就是離散pid公式中的T***/
const float _dt_max = 0.02f;
bool _task_should_exit = false; /**< if true, task should exit */
int _control_task = -1; /**< task handle for task */
int _params_sub = -1; /****與parameter_update有關****/
int _sensors_sub = -1; /****這個變量是訂閱傳感器數據時用的句柄****/
int _global_pos_sub = -1; /****這個變量是訂閱地理位置數據時用的句柄****/
int _vision_sub = -1; /****這個變量是視覺信息時用的句柄****/
int _mocap_sub = -1; /****mocap=motion capture，動作捕捉****/
orb_advert_t _att_pub = nullptr; /***nullptr是c++11標準引入的更嚴謹的空指針，這裏的att_pub是用於發佈姿態信息的handle***/
struct {
/**前面帶w都是權重的意思**/
param_t w_acc; /**這裏的acc爲加速度計的權重，用於後面互補濾波**/
param_t w_mag; /**mag爲磁力計**/
param_t w_ext_hdg;
param_t w_gyro_bias; /***陀螺儀偏差***/
param_t mag_decl; /**磁方位角**/
param_t mag_decl_auto; /**利用gps自動獲得磁方位角**/
param_t acc_comp; /**加速度的補償**/
param_t bias_max; /**最大偏差**/
param_t ext_hdg_mode; /***外部的航向使用模式，=1表示來自於視覺，=2表示來自mocap***/
} _params_handles{}; /**< handles for interesting parameters 這個結構體裏面全是一些系統參數**/
/**在px4的程序中獲取系統參數的方法是用param_get和param_find去獲取，而不是通過uorb**/
/**下面這些初始化的數據是用來存放從系統參數get過來的數據***/
float _w_accel = 0.0f;
float _w_mag = 0.0f;
float _w_ext_hdg = 0.0f;
float _w_gyro_bias = 0.0f;
float _mag_decl = 0.0f;
bool _mag_decl_auto = false;
bool _acc_comp = false;
float _bias_max = 0.0f;
int32_t _ext_hdg_mode = 0;
Vector<3> _gyro; /**通過傳感器獲取的三軸的角速度**/
Vector<3> _accel; /***通過加速度計獲取的三軸的加速度***/
Vector<3> _mag; /***通過磁力計獲取的磁航向***/
Vector<3> _vision_hdg; /**通過視覺獲取的handing**/
Vector<3> _mocap_hdg; /**通過mocap獲取的handing**/
Quaternion _q;
Vector<3> _rates; /**與上面_gyro不同的是這個用於存放修正後的繞三軸角速度**/
Vector<3> _gyro_bias;
Vector<3> _vel_prev; /**前一刻的速度**/
hrt_abstime _vel_prev_t = 0; /**uint64_t的typedef，abstime意思爲絕對時間，這裏爲記錄前一時刻速度時的絕對時間**/
/**用於後面根據速度差除以時間差求加速度**/
Vector<3> _pos_acc; //運動加速度，注意：這個加速度不包括垂直方向的
bool _inited = false; //初始化標識
bool _data_good = false;//數據可用
bool _ext_hdg_good = false;//外部航向可用
void update_parameters(bool force);
int update_subscriptions(); /**只有聲明，無定義？？？字面的意思是更新訂閱信息？？**/
bool init();//初始化函數，初始化旋轉矩陣和四元數
bool update(float dt);//dt是更新週期，這個函數是整個程序的核心
//Update magnetic declination (in rads) immediately changing yaw rotation
//偏航角改變立刻更新磁方位角
void update_mag_declination(float new_declination);
};
AttitudeEstimatorQ::AttitudeEstimatorQ()
{
//先用find匹配系統參數，然後用get拷貝系統的參數，之所以這樣不會直接影響到系統參數，但是又可以正常使用系統參數。
_params_handles.w_acc = param_find("ATT_W_ACC");
_params_handles.w_mag = param_find("ATT_W_MAG");
_params_handles.w_ext_hdg = param_find("ATT_W_EXT_HDG");
_params_handles.w_gyro_bias = param_find("ATT_W_GYRO_BIAS");
_params_handles.mag_decl = param_find("ATT_MAG_DECL");
_params_handles.mag_decl_auto = param_find("ATT_MAG_DECL_A");
_params_handles.acc_comp = param_find("ATT_ACC_COMP");
_params_handles.bias_max = param_find("ATT_BIAS_MAX");
_params_handles.ext_hdg_mode = param_find("ATT_EXT_HDG_M");
_vel_prev.zero();/**清零**/
_pos_acc.zero();
_gyro.zero();
_accel.zero();
_mag.zero();
_vision_hdg.zero();/**通過視覺得到的航向清零**/
_mocap_hdg.zero();/**通過mocap得到的航向清零**/
_q.zero();
_rates.zero();
_gyro_bias.zero();
_vel_prev.zero();
_pos_acc.zero();
}
/**
* Destructor, also kills task.
* 析構函數，也用於結束任務
*/
AttitudeEstimatorQ::~AttitudeEstimatorQ()
{
if (_control_task != -1) {
/* task wakes up every 100ms or so at the longest */
_task_should_exit = true;
/* wait for a second for the task to quit at our request */
unsigned i = 0;
do {
/* wait 20ms */
usleep(20000);
/* if we have given up, kill it */
if (++i > 50) {
px4_task_delete(_control_task);
break;
}
} while (_control_task != -1);
}
attitude_estimator_q::instance = nullptr;
}
int AttitudeEstimatorQ::start()
{
ASSERT(_control_task == -1); //AEESRT是assert函數的define了，作用是參數值爲0時終止程序
/* start the task */
/*這裏的px4_task_spawn_cmd函數的作用是創建一個新的任務，屬於nuttx系統中的**/
_control_task = px4_task_spawn_cmd("attitude_estimator_q",
SCHED_DEFAULT,
SCHED_PRIORITY_ESTIMATOR,
2000,
(px4_main_t)&AttitudeEstimatorQ::task_main_trampoline,
nullptr);
if (_control_task < 0)
{
warn("task start failed");
return -errno;
}
return OK;
}
void AttitudeEstimatorQ::task_main_trampoline(int argc, char *argv[]) /**運行task_main**/
{
attitude_estimator_q::instance->task_main();
}
void AttitudeEstimatorQ::task_main()
{
#ifdef __PX4_POSIX
/**加速度計的表現性能。。第一個參數是counter的類型，第二個是counter的名字**/
perf_counter_t _perf_accel(perf_alloc_once(PC_ELAPSED, "sim_accel_delay"));
/**mpu的表現性能**/
perf_counter_t _perf_mpu(perf_alloc_once(PC_ELAPSED, "sim_mpu_delay"));
/**mag的表現性能**/
perf_counter_t _perf_mag(perf_alloc_once(PC_ELAPSED, "sim_mag_delay"));
#endif
_sensors_sub = orb_subscribe(ORB_ID(sensor_combined));/**訂閱傳感器信息**/
_vision_sub = orb_subscribe(ORB_ID(vehicle_vision_attitude));/**訂閱視覺信息**/
_mocap_sub = orb_supdbscribe(ORB_ID(att_pos_mocap));/**訂閱mocap信息**/
_params_sub = orb_subscribe(ORB_ID(parameter_update));/**訂閱parameter信息，用於之後的parameter_update的copy**/
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));/**訂閱位置信息**/
/**上面是訂閱，copy部分在下面這個函數中**/
update_parameters(true);
hrt_abstime last_time = 0;
/************poll**************/
/******nuttx任務使能********/
px4_pollfd_struct_t fds[1] = {};
fds[0].fd = _sensors_sub;
fds[0].events = POLLIN;
while (!_task_should_exit)
{
int ret = px4_poll(fds, 1, 1000);
if (ret < 0) {
// Poll error, sleep and try again
usleep(10000);
PX4_WARN("POLL ERROR");
continue;
} else if (ret == 0) {
// Poll timeout, do nothing
PX4_WARN("POLL TIMEOUT");
continue;
}
update_parameters(false); //498行
// Update sensors
sensor_combined_s sensors; /**新建的sensor結構體用於裝複製的傳感器信息**/
if (orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors) == PX4_OK) {
// Feed validator with recent sensor data
if (sensors.timestamp > 0) {
_gyro(0) = sensors.gyro_rad[0];
_gyro(1) = sensors.gyro_rad[1];
_gyro(2) = sensors.gyro_rad[2];
}
if (sensors.accelerometer_timestamp_relative != sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) {
_accel(0) = sensors.accelerometer_m_s2[0];
_accel(1) = sensors.accelerometer_m_s2[1];
_accel(2) = sensors.accelerometer_m_s2[2];
if (_accel.length() < 0.01f) {
PX4_ERR("WARNING: degenerate accel!");
continue;
}
}
if (sensors.magnetometer_timestamp_relative != sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) {
_mag(0) = sensors.magnetometer_ga[0];
_mag(1) = sensors.magnetometer_ga[1];
_mag(2) = sensors.magnetometer_ga[2];
if (_mag.length() < 0.01f) {
PX4_ERR("WARNING: degenerate mag!");
continue;
}
}
_data_good = true;
}
// Update vision and motion capture heading
// 通過uORB模型獲取vision和mocap的數據（視覺和mocap）
bool vision_updated = false;
orb_check(_vision_sub, &vision_updated);
if (vision_updated) {
vehicle_attitude_s vision; /**新建vision結構體**/
if (orb_copy(ORB_ID(vehicle_vision_attitude), _vision_sub, &vision) == PX4_OK) {
math::Quaternion q(vision.q);//將基於視覺獲得的四元數賦給新建的math：：q
math::Matrix<3, 3> Rvis = q.to_dcm(); /**基於視覺得到的轉換矩陣**/
math::Vector<3> v(1.0f, 0.0f, 0.4f); /**從他給定的v可知這裏的v是一個指北的向量**/
/**至於爲什麼初始給的是0.4,還有疑惑**/
// Rvis is Rwr (robot respect to world) while v is respect to world.
// Hence Rvis must be transposed having (Rwr)' * Vw v是在地系下的，而dcm是b—>e,所以要轉置
// Rrw * Vw = vn. This way we have consistency
_vision_hdg = Rvis.transposed() * v;/**轉置後與v乘，就反映了視覺上的指北的向量**/
// vision external heading usage (ATT_EXT_HDG_M 1)
if (_ext_hdg_mode == 1) {
// Check for timeouts on data 檢查數據超時
_ext_hdg_good = vision.timestamp > 0 && (hrt_elapsed_time(&vision.timestamp) < 500000);
}
}
}
/**基於mocap獲取handing同理**/
bool mocap_updated = false;
orb_check(_mocap_sub, &mocap_updated);
if (mocap_updated) {
att_pos_mocap_s mocap;
if (orb_copy(ORB_ID(att_pos_mocap), _mocap_sub, &mocap) == PX4_OK) {
math::Quaternion q(mocap.q);
math::Matrix<3, 3> Rmoc = q.to_dcm();
math::Vector<3> v(1.0f, 0.0f, 0.4f);
// Rmoc is Rwr (robot respect to world) while v is respect to world.
// Hence Rmoc must be transposed having (Rwr)' * Vw
// Rrw * Vw = vn. This way we have consistency
_mocap_hdg = Rmoc.transposed() * v;
// Motion Capture external heading usage (ATT_EXT_HDG_M 2)
if (_ext_hdg_mode == 2) {
// Check for timeouts on data
_ext_hdg_good = mocap.timestamp > 0 && (hrt_elapsed_time(&mocap.timestamp) < 500000);
}
}
}
//下面的是自動獲取磁偏角
bool gpos_updated = false;
orb_check(_global_pos_sub, &gpos_updated);
if (gpos_updated) {//如果位置已經更新 就取出位置數據
vehicle_global_position_s gpos;
if (orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &gpos) == PX4_OK)
{
//首先檢測是否配置了自動磁方位角獲取，即用下面的if
if (_mag_decl_auto && gpos.eph < 20.0f && hrt_elapsed_time(&gpos.timestamp) < 1000000) {
/* set magnetic declination automatically */
//自動獲取磁方位角，如果配置了則用當前的經緯度（longitude and latitude）通過get_mag_declination(_gpos.lat,_gpos.lon)函數獲取當前位置的磁偏角
update_mag_declination(math::radians(get_mag_declination(gpos.lat, gpos.lon)));
}
//獲取機體的速度，通過速度計算機體的加速度 。
//先判斷位置信息是否有效
if (_acc_comp && gpos.timestamp != 0 && hrt_absolute_time() < gpos.timestamp + 20000 && gpos.eph < 5.0f && _inited)
{
/* position data is actual */
Vector<3> vel(gpos.vel_n, gpos.vel_e, gpos.vel_d); //北東地系
/* velocity updated */
if (_vel_prev_t != 0 && gpos.timestamp != _vel_prev_t) {
float vel_dt = (gpos.timestamp - _vel_prev_t) / 1e6f; //us
/* calculate acceleration in body frame */
//計算e系下的運動加速度，這裏的pos_acc很重要，要用於後面加速度計的校正
_pos_acc = _q.conjugate_inversed((vel - _vel_prev) / vel_dt);
}
_vel_prev_t = gpos.timestamp; //爲迭代做準備
_vel_prev = vel;
}
else {/**否則位置信息過時，將加速度信息清零**/
/* position data is outdated, reset acceleration */
_pos_acc.zero();
_vel_prev.zero();
_vel_prev_t = 0;
}
}
}
/* time from previous iteration */
hrt_abstime now = hrt_absolute_time();
//下面的constrain函數用於限定作用，限定規則：return (val < min_val) ? min_val : ((val > max_val) ? max_val : val);
const float dt = math::constrain((now - last_time) / 1e6f, _dt_min, _dt_max);
last_time = now;
if (update(dt)) {
vehicle_attitude_s att = {
.timestamp = sensors.timestamp,
.rollspeed = _rates(0),
.pitchspeed = _rates(1),
.yawspeed = _rates(2),
.q = {_q(0), _q(1), _q(2), _q(3)},
.delta_q_reset = {},
.quat_reset_counter = 0,
};
/* the instance count is not used here */
//最後發佈給了vehicle_attitude,這也對應了最開始我們說的我們不能直接把陀螺儀的數據當成姿態信息，而應該
//經過我們處理後才能使用。即vehicle_attitude的信息流爲：訂閱的是sensor combined，發佈的是vehicle attitude
int att_inst;
orb_publish_auto(ORB_ID(vehicle_attitude), &_att_pub, &att, &att_inst, ORB_PRIO_HIGH);
}
}
#ifdef __PX4_POSIX
perf_end(_perf_accel);
perf_end(_perf_mpu);
perf_end(_perf_mag);
#endif
orb_unsubscribe(_params_sub);
orb_unsubscribe(_sensors_sub);
orb_unsubscribe(_global_pos_sub);
orb_unsubscribe(_vision_sub);
orb_unsubscribe(_mocap_sub);
}
void AttitudeEstimatorQ::update_parameters(bool force)
{
bool updated = force;
if (!updated) //如果更新了
{
orb_check(_params_sub, &updated); /***檢查一個主題在發佈者最後更新後，有沒parameter_update有人調用過orb_copy來接收如果主題在被公告前就有人訂閱，那麼這個API將返回“not-updated”直到主題被公告。***/
}
if (updated) //如果沒更新
{
parameter_update_s param_update; /**建立建構體param_update，裏面有更新的時間、是否更新、填充信息**/
orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_update);/**根據頭文件可知parameter_update是自帶的主題*/
/***疑問：上面的copy之前爲什麼沒有訂閱，是不是後面參數更新後還要發佈出去？其實不是，訂閱發生在前面的task_main函數中***/
param_get(_params_handles.w_acc, &_w_accel);/**把獲得的參數值賦值給後面那個參數，前面那些參數的值都在構造函數中定義了**/
param_get(_params_handles.w_mag, &_w_mag);
param_get(_params_handles.w_ext_hdg, &_w_ext_hdg);
param_get(_params_handles.w_gyro_bias, &_w_gyro_bias);
float mag_decl_deg = 0.0f;
param_get(_params_handles.mag_decl, &mag_decl_deg);
update_mag_declination(math::radians(mag_decl_deg));/**radians函數爲取弧度，外面的函數爲消除偏差的積累***/
int32_t mag_decl_auto_int;
param_get(_params_handles.mag_decl_auto, &mag_decl_auto_int);
_mag_decl_auto = (mag_decl_auto_int != 0);
int32_t acc_comp_int;
param_get(_params_handles.acc_comp, &acc_comp_int);
_acc_comp = (acc_comp_int != 0);
param_get(_params_handles.bias_max, &_bias_max);
param_get(_params_handles.ext_hdg_mode, &_ext_hdg_mode);
}
}
/***********init（）函數作用：由加速度計和磁力計初始化旋轉矩陣和四元數**************/
bool AttitudeEstimatorQ::init()
{
// Rotation matrix can be easily constructed from acceleration and mag field vectors
// 'k' is Earth Z axis (Down) unit vector in body frame
//，所以取反向
Vector<3> k = -_accel; //accel已經在task_main中訂閱
//k爲加速度傳感器測量到加速度方向向量，由於第一次測量數據時無人機一般爲平穩狀態無運動狀態，
//所以可以直接將測到的加速度視爲重力加速度g，以此作爲dcm旋轉矩陣的第三行k
k.normalize(); //向量歸一化，也就是向量除以他的長度
// 'i' is Earth X axis (North) unit vector in body frame, orthogonal with 'k'
// i是體座標系下指向正北的單位向量，與k正交
Vector<3> i = (_mag - k * (_mag * k));//施密特正交化,強制使i與k垂直;因向量k已歸一化，故分母等於1;
//這裏的_mag是指北的，所以下面求得的R是默認飛機機頭也是指向北
i.normalize(); //向量歸一化，也就是向量除以他的長度
// 'j' is Earth Y axis (East) unit vector in body frame, orthogonal with 'k' and 'i'
//j是e座標系下指向正東的單位向量在b系下的向量，與k、i正交
Vector<3> j = k % i;
// 填充旋轉矩陣
Matrix<3, 3> R; //新建一個3*3的矩陣R
R.set_row(0, i); //set_row函數的第一個參數爲第幾行，將i向量填入矩陣第一行
R.set_row(1, j); //將j向量填入矩陣第二行
R.set_row(2, k); //將k向量填入矩陣第三行，注意：從這裏可知該旋轉矩陣爲b繫到n系
// 將R矩陣轉換爲四元數
_q.from_dcm(R);
// Compensate for magnetic declination 補償飛機的磁方位角，因爲前面求得q是默認飛機指向北，但起飛時飛機不一定指向北
Quaternion decl_rotation;
decl_rotation.from_yaw(_mag_decl);/**將旋轉矩陣僅僅通過yaw偏航的四元數表示**/
_q = decl_rotation * _q;
_q.normalize(); /**歸一化**/ /**此時的q纔是真正的初始的q**/
if (PX4_ISFINITE(_q(0)) && PX4_ISFINITE(_q(1)) && /**判斷是否發散**/
PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3)) &&
_q.length() > 0.95f && _q.length() < 1.05f)
{
_inited = true; /**初始化完成**/
}
else
{
_inited = false; /**初始化失敗**/
}
return _inited;
}
bool AttitudeEstimatorQ::update(float dt)
{
if (!_inited)
{
if (!_data_good) /**在前面task_main函數中如果傳感器數據訂閱正常，data_good就爲true**/
{
return false;
}
return init(); /**沒有初始化（前提：傳感器數據訂閱正常）就初始化**/
//並且注意：init在一次飛行trampoline中只執行一次，因爲以後的四元數和轉換矩陣由輸出的角速度經過龍格庫塔法求，而飛機在初始飛行時就需要通過init求轉換矩陣
}
/**前面的init等都完成就繼續往下執行**/
Quaternion q_last = _q; /**注意：此時四元數_q已經有內容了，此處的q_last的作用爲給q弄一個備份，可從最後一個if看出**/
// Angular rate of correction 修正角速率,下面的是重點
Vector<3> corr;
float spinRate = _gyro.length(); /**定義一個變量：旋轉速率，length函數爲平方和開方**/
//首先判斷是使用什麼mode做修正的，比如vision、mocap、acc和mag，這裏我們着重分析用acc和mag做修正，另外兩個同理
if (_ext_hdg_mode > 0 && _ext_hdg_good)
{
if (_ext_hdg_mode == 1)
{
// Vision heading correction 對基於視覺的航向模式進行修正
// Project heading to global frame and extract XY component
Vector<3> vision_hdg_earth = _q.conjugate(_vision_hdg);/**將b系轉爲e系**/
float vision_hdg_err = _wrap_pi(atan2f(vision_hdg_earth(1), vision_hdg_earth(0)));
// Project correction to body frame
corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -vision_hdg_err)) * _w_ext_hdg;
}
if (_ext_hdg_mode == 2) {
// Mocap heading correction
// Project heading to global frame and extract XY component
Vector<3> mocap_hdg_earth = _q.conjugate(_mocap_hdg);
float mocap_hdg_err = _wrap_pi(atan2f(mocap_hdg_earth(1), mocap_hdg_earth(0)));
// Project correction to body frame
corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mocap_hdg_err)) * _w_ext_hdg;
}
}
if (_ext_hdg_mode == 0 || !_ext_hdg_good) {
// Magnetometer correction
// Project mag field vector to global frame and extract XY component
Vector<3> mag_earth = _q.conjugate(_mag); /**將b系轉爲e系**/
//只考慮Vector<3> mag_earth中的前兩維的數據mag_earth(1)和mag_earth(0)（即x、y，忽略z軸上的偏移），通
//過arctan（mag_earth(1),mag_earth(0)）得到的角度和前面根據經緯度獲取的磁方位角做差值得到誤差角度mag_err
//_wrap_pi函數是用於限定結果-pi到pi的函數，大於pi則與2pi做差取補，小於-pi則與2pi做和取補
//注意1：這裏在修正磁力計時用到的是有gps校準時的修正辦法，即下面的減去自動獲取的磁偏角。沒有gps的校準方法見ppt
//注意2：這裏校正的方法是用磁力計計算的磁偏角與gps得出來的做差，然後轉換到b系。
float mag_err = _wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl);//與自動獲取的磁方位角做差
float gainMult = 1.0f;
const float fifty_dps = 0.873f;
if (spinRate > fifty_dps) {
gainMult = math::min(spinRate / fifty_dps, 10.0f);
}
// Project magnetometer correction to body frame
//下面*Vector<3>(0.0f, 0.0f, -mag_err))是因爲raw本質上應該由磁力計產生的水平磁向量與gps產生的水平磁向量叉乘得到，而叉乘得到的正好向下
corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mag_err)) * _w_mag * gainMult;
}
_q.normalize();
// Accelerometer correction
// Project 'k' unit vector of earth frame to body frame
// Vector<3> k = _q.conjuq.conjugate_inversed(Vector<3>(0.0f, 0.0f, 1.0f));
// Optimized version with dropped zeros
//下面的k是n系中重力的單位向量轉換到b系中，即左乘旋轉矩陣得到的
Vector<3> k(
2.0f * (_q(1) * _q(3) - _q(0) * _q(2)),
2.0f * (_q(2) * _q(3) + _q(0) * _q(1)),
(_q(0) * _q(0) - _q(1) * _q(1) - _q(2) * _q(2) + _q(3) * _q(3))
);
//總的受到合力的方向（_accel）減去機體加速度方向（_pos_acc）得到g的方向，即總加速度（加速度獲取）減去機體運動加速度獲取重力加速度
//重力加速度的方向是導航座標系下的z軸，加上運動加速度之後，總加速度的方向就不是與導航座標系的天或地平行了，所以要消除這個誤差，即“_accel-_pos_acc”
//這裏與k差乘得到的是與重力方向的夾角sinx，約等於x，即roll和pitch
corr += (k % (_accel - _pos_acc).normalized()) * _w_accel;
// Gyro bias estimation
if (spinRate < 0.175f) {
_gyro_bias += corr * (_w_gyro_bias * dt);//對應積分控制
//對_gyro_bias做約束處理。
for (int i = 0; i < 3; i++) {
_gyro_bias(i) = math::constrain(_gyro_bias(i), -_bias_max, _bias_max);
}
}
_rates = _gyro + _gyro_bias;//角速度 = 陀螺儀的測量值 + 誤差校準
// pi控制器的體現，對應比例部分
corr += _rates;//corr爲update函數中定義的變量，所以每次進入update函數中時會刷新corr變量的數據
// Apply correction to state
//最後就是使用修正的數據更新四元數，並把_rates和_gyro_bias置零便於下次調用時使用。
_q += _q.derivative(corr) * dt;//用龍格庫塔法更新四元數
// Normalize quaternion
_q.normalize();
if (!(PX4_ISFINITE(_q(0)) && PX4_ISFINITE(_q(1)) &&
PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3)))) {
// 如果數據不合適就恢復備份的q，即q_last
_q = q_last;
_rates.zero();
_gyro_bias.zero();
return false;
}
return true;
} //更新完四元數後，我們跳到update函數被調用的地方，即444行，發現後面將更新後的姿態信息發佈出去了，到此整個過程結束
/**偏航角改變立刻更新磁方位角**/
void AttitudeEstimatorQ::update_mag_declination(float new_declination)
{
// Apply initial declination or trivial rotations without changing estimation
// 忽略微小旋轉（比如機體的微小震動），繼續沿用之前的磁方位角
if (!_inited || fabsf(new_declination - _mag_decl) < 0.0001f)
{
_mag_decl = new_declination;
}
else //轉動較大時，修正姿態（q）和更新磁方位角
{
// Immediately rotate current estimation to avoid gyro bias growth
Quaternion decl_rotation;
decl_rotation.from_yaw(new_declination - _mag_decl);//偏航角生成的四元數，生成方法是令另外2個角度爲0,使用歐拉角轉四元數公式
_q = decl_rotation * _q;//此處兩個四元數相乘表示角度相加，因爲在數學上q1*q2表示q1q2這個合成動作，所以此處修正了四元數
_mag_decl = new_declination;//使用新的磁偏角
}
}
//下面的總結起來：attitude_estimator_q { start }：實例化instance，運行instance->start()；
//attitude_estimator_q { stop }：delete instance，指針置空；
//attitude_estimator_q { status}：instance->print()，打印是否在running
int attitude_estimator_q_main(int argc, char *argv[])
{
if (argc < 2) /**命令行總的參數個數<2,由後面可知有三個參數**/
{
warnx("usage: attitude_estimator_q {start|stop|status}");
return 1;
}
if (!strcmp(argv[1], "start")) /**用戶輸入的第一個參數是否爲start，是的話即繼續執行**/
{
if (attitude_estimator_q::instance != nullptr) /**不爲空即代表已生成了內容，所以已經啓動**/
{
warnx("already running");
return 1;
}
attitude_estimator_q::instance = new AttitudeEstimatorQ;/**instanc爲空就申請空間，申請完成後不再爲空指針**/
if (attitude_estimator_q::instance == nullptr) /**再一次判斷是否爲空**/
{
warnx("alloc failed"); /**是的話即分配空間失敗**/
return 1;
}
if (OK != attitude_estimator_q::instance->start()) /**到這一步說明已經申請成功，start函數返回ok代表系統已經新建了一個進程任務，這裏代表沒有啓動**/
{
delete attitude_estimator_q::instance; /**沒有啓動就釋放空間，重新變爲空指針**/
attitude_estimator_q::instance = nullptr;
warnx("start failed");
return 1;
}
return 0;
}
if (!strcmp(argv[1], "stop")) {
if (attitude_estimator_q::instance == nullptr)
{
warnx("not running");
return 1;
}
delete attitude_estimator_q::instance; /**能執行這一步說明instance不爲空指針，任務已經執行，然後進行刪除**/
attitude_estimator_q::instance = nullptr; /**恢復原樣**/
return 0;
}
if (!strcmp(argv[1], "status"))
{
if (attitude_estimator_q::instance) /**這裏代碼補全應爲if (attitude_estimator_q::instance != nullptr)**/
{
warnx("running");
return 0;
}
else
{
warnx("not running");
return 1;
}
}
warnx("unrecognized command"); /**出去start、stop、status外其他的爲unrecognized command**/
return 1;
}
————————————————
版權聲明：本文爲CSDN博主「這是小旭哦」的原創文章，遵循 CC 4.0 BY-SA 版權協議，轉載請附上原文出處鏈接及本聲明。
原文鏈接：https://blog.csdn.net/zouxu634866/article/details/79806948