pixhawk 姿态与控制部分的记录
此篇是把之前看到的资料总结整理一遍，大部分是搬砖，也加入了自己的一点思考，写的过程中晕了好多次，先大体记录下来，肯定有错误，日后再改正吧。
关于pixhawk程序执行流程，依然还没有实际运行调试过程序，只是逻辑上理清先做什么再做什么，以防实际调试程序时脑袋晕掉，所以还只是纸上谈兵。把整个程序流程脑袋中跑一遍、来龙去脉搞清楚点后再实际调试的。
经过前面这些博客分析，基本把程序分析的障碍扫除了，但是理论基础十分薄弱（还没来得及细细看来）。
姿态解算
这部分需要的理论知识较多，现在只浅浅的理解了一部分，先记下来，以免以后搞混了乱了。主要感谢牛叔的讲解！
（1）Mahony算法解析：
传感器自身属性，陀螺仪短时期动态响应可以利用、长时期会温漂，加速度机长时间可以利用短时期振动太厉害不能利用。因此就形成了互补融合滤波。网上一个很好的比喻，让飞行器保持一个姿态相当于是开一艘船到某个目的地，陀螺仪相当于是掌舵手，自己可以短时期推断船的航向，但是没有指向，一段时间后就会走偏了，所以需要一个观望者，指明方向，告诉他开的方向对不对，进行校正，而这个角色在飞行器中就是由加速度计来担任。所以mahony的算法核心就是：掌舵者一直问观望者我开的对不对，然后校正。
以上是感性认识，以下是理论算法上解释：
首先需要明确几个概念：飞行器姿态有几种表达方式，比如欧拉角（pitch、roll、yaw）、四元数矩阵（q1q2q3q4）。而飞行器自身传感器测量的数据都是相对于飞行器坐标而言的，也就是body坐标系；实际上我们需要飞行器保持什么姿态、去哪个地方都是相对于地理坐标系而言的，也就是r坐标系，为了达到控制目的，所以需要一个从b坐标系到r坐标系的旋转矩阵，为了校正磁偏也需要从r坐标系到b坐标系的旋转矩阵。不同的旋转顺序，旋转矩阵是不同的，因为相同的两矩阵相乘哪个在前哪个在后乘出来结果是不同的，对应于坐标系就是对齐x轴、y轴、z轴的先后顺序了，比如b坐标系旋转到r坐标系，旋转顺序是x-y-z，那么旋转回来到b坐标系就是z-y-x，所以不同的旋转顺序，对应的旋转矩阵是不同的！
PI校正的解释（这个也是mahony的核心部分）：
这个就是总体的流程
说到底这个姿态解算的过程就是，先在b坐标系下用传感器进行测量，并且用四元数表达出来，其中用向量叉积求得2个向量之间的误差，加速度计的向量矩阵与陀螺仪的向量矩阵做叉积以按时间来分配权重，重新得到一个新的姿态也就是新的四元数矩阵，转换成欧拉角输出给后面的控制。哎，理论知识太欠缺了，只能理解到这一步。
这部分的代码就是
void MahonyIMUupdate(float gx, float gy, float gz, float ax, float ay, float az)
{
float recipNorm;
float halfvx, halfvy, halfvz;
float halfex, halfey, halfez;
float qa, qb, qc;
gx*=0.0174;
gy*=0.0174;
gz*=0.0174;
// Normalise accelerometer measurement
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// Estimated direction of gravity and vector perpendicular to magnetic flux
halfvx = q1 * q3 - q0 * q2;
halfvy = q0 * q1 + q2 * q3;
halfvz = q0 * q0 - 0.5f + q3 * q3;
// Error is sum of cross product between estimated and measured direction of gravity
halfex = (ay * halfvz - az * halfvy);
halfey = (az * halfvx - ax * halfvz);
halfez = (ax * halfvy - ay * halfvx);
// Compute and apply integral feedback if enabled
integralFBx += twoKi * halfex * dt; // integral error scaled by Ki
integralFBy += twoKi * halfey * dt;
integralFBz += twoKi * halfez * dt;
gx += integralFBx; // apply integral feedback
gy += integralFBy;
gz += integralFBz;
// Apply proportional feedback
gx += twoKp * halfex;
gy += twoKp * halfey;
gz += twoKp * halfez;
// Integrate rate of change of quaternion
gx *= 0.0125f;//(0.5f * dt); // pre-multiply common factors
gy *= 0.0125f;//(0.5f * dt);
gz *= 0.0125f;//(0.5f * dt); //0.00125f
qa = q0;
qb = q1;
qc = q2;
q0 += (-qb * gx - qc * gy - q3 * gz);
q1 += (qa * gx + qc * gz - q3 * gy);
q2 += (qa * gy - qb * gz + q3 * gx);
q3 += (qa * gz + qb * gy - qc * gx);
// Normalise quaternion
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
Mahony最后的那个公式解释：
这张图的意思是：假如左边[x y z]’是地理坐标下的姿态，右边[x0 y0 z0]’是机体坐标下的姿态，中间通过旋转矩阵得以转换，而欧拉角和四元数是旋转矩阵不同的表达方式，相同的姿态，两个坐标系对应都是相同的，所以四元数和欧拉角之间有个对应关系(当然也和旋转顺序有关)，这个关系就是mahony算法最后的那个公式
Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
Yaw = atan2(2*q1*q2 - 2*q0*q3, 2*q0*q0 + 2*q1*q1 - 1) * 57.3;//-360;
更加详细的解释可以查看“四元数解算姿态完全解析及资料汇总”
截下关键步骤
之前的都没有加入磁力计，因为磁力计和加速度计不一样，它在三维空间中并不和某个轴重合，而受到的影响也是不一样的，所以需要不同的处理方法，也就是需要从机体坐标系旋转到地理坐标系干个什么事，在旋转回机体坐标系进行误差补偿。先这么理解吧，笔者也不大懂。
（3）pixhawk姿态解算流程
以下规定黑色为最外层（控制主程序），红色为第二层（主程序的注释），蓝色为第三层（主程序注释程序的注释）
程序入口Ardupilot/modules/PX4Firmware/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
void AttitudeEstimatorQ::task_main()
{
#ifdef __PX4_POSIX
perf_counter_t_perf_accel(perf_alloc_once(PC_ELAPSED, "sim_accel_delay"));
perf_counter_t_perf_mpu(perf_alloc_once(PC_ELAPSED, "sim_mpu_delay"));
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(vision_position_estimate));
_mocap_sub= orb_subscribe(ORB_ID(att_pos_mocap));
_airspeed_sub= orb_subscribe(ORB_ID(airspeed));
_params_sub= orb_subscribe(ORB_ID(parameter_update));
_global_pos_sub= orb_subscribe(ORB_ID(vehicle_global_position));
订阅各种数据
update_parameters(true);
初始化各种参数
hrt_abstimelast_time = 0;
px4_pollfd_struct_tfds[1] = {};
fds[0].fd= _sensors_sub;
fds[0].events= POLLIN;
while(!_task_should_exit) {
intret = px4_poll(fds, 1, 1000);
if(ret < 0) {
//Poll error, sleep and try again
usleep(10000);
PX4_WARN("QPOLL ERROR");
continue;
}else if (ret == 0) {
//Poll timeout, do nothing
PX4_WARN("QPOLL TIMEOUT");
continue;
}
update_parameters(false);
//Update sensors
sensor_combined_ssensors;
intbest_gyro = 0;
intbest_accel = 0;
intbest_mag = 0;
if(!orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors)) {
//Feed validator with recent sensor data获取传感器数据
Ardupilot/modules/PX4Firmware/src/modules/uORB/topics/sensor_combined.h
#ifdef __cplusplus
struct __EXPORTsensor_combined_s {
#else
struct sensor_combined_s{
#endif
uint64_t timestamp;
uint64_t gyro_timestamp[3];
int16_t gyro_raw[9];
float gyro_rad_s[9];
uint32_t gyro_priority[3];
float gyro_integral_rad[9];
uint64_t gyro_integral_dt[3];
uint32_t gyro_errcount[3];
float gyro_temp[3];
int16_t accelerometer_raw[9];
float accelerometer_m_s2[9];
float accelerometer_integral_m_s[9];
uint64_t accelerometer_integral_dt[3];
int16_t accelerometer_mode[3];
float accelerometer_range_m_s2[3];
uint64_t accelerometer_timestamp[3];
uint32_t accelerometer_priority[3];
uint32_t accelerometer_errcount[3];
float accelerometer_temp[3];
int16_t magnetometer_raw[9];
float magnetometer_ga[9];
int16_t magnetometer_mode[3];
float magnetometer_range_ga[3];
float magnetometer_cuttoff_freq_hz[3];
uint64_t magnetometer_timestamp[3];
uint32_t magnetometer_priority[3];
uint32_t magnetometer_errcount[3];
float magnetometer_temp[3];
float baro_pres_mbar[3];
float baro_alt_meter[3];
float baro_temp_celcius[3];
uint64_t baro_timestamp[3];
uint32_t baro_priority[3];
uint32_t baro_errcount[3];
float adc_voltage_v[10];
uint16_t adc_mapping[10];
float mcu_temp_celcius;
float differential_pressure_pa[3];
uint64_t differential_pressure_timestamp[3];
float differential_pressure_filtered_pa[3];
uint32_t differential_pressure_priority[3];
uint32_t differential_pressure_errcount[3];
#ifdef __cplusplus
static const int32_t MAGNETOMETER_MODE_NORMAL = 0;
static const int32_t MAGNETOMETER_MODE_POSITIVE_BIAS = 1;
static const int32_t MAGNETOMETER_MODE_NEGATIVE_BIAS = 2;
static const uint32_t SENSOR_PRIO_MIN = 0;
static const uint32_t SENSOR_PRIO_VERY_LOW = 25;
static const uint32_t SENSOR_PRIO_LOW = 50;
static const uint32_t SENSOR_PRIO_DEFAULT = 75;
static const uint32_t SENSOR_PRIO_HIGH = 100;
static const uint32_t SENSOR_PRIO_VERY_HIGH = 125;
static const uint32_t SENSOR_PRIO_MAX = 255;
#endif
}; for(unsigned i = 0; i < (sizeof(sensors.gyro_timestamp) /sizeof(sensors.gyro_timestamp[0])); i++) {
/*ignore empty fields */
if(sensors.gyro_timestamp[i] > 0) {
floatgyro[3];
for(unsigned j = 0; j < 3; j++) {
if(sensors.gyro_integral_dt[i] > 0) {
gyro[j]= (double)sensors.gyro_integral_rad[i * 3 + j] / (sensors.gyro_integral_dt[i] /1e6);
}else {
/*fall back to angular rate */
gyro[j]= sensors.gyro_rad_s[i * 3 + j];
}
}
_voter_gyro.put(i,sensors.gyro_timestamp[i], &gyro[0], sensors.gyro_errcount[i],sensors.gyro_priority[i]);
}gyro、accel、mag每次读取数据都是三组三组的读取
Ardupilot/modules/PX4Firmware/src/lib/ecl/validation/data_validator_group.cpp
void
DataValidatorGroup::put(unsignedindex, uint64_t timestamp, float val[3], uint64_t error_count, int priority)
{
DataValidator *next = _first;
unsigned i = 0;
while (next != nullptr) {
if (i == index) {
next->put(timestamp, val, error_count,priority);
break;
}
next = next->sibling();
i++;
}
}
Ardupilot/modules/PX4Firmware/src/lib/ecl/validation/data_validator.cpp
void
DataValidator::put(uint64_ttimestamp, float val, uint64_t error_count_in, int priority_in)
{
float data[3];
data[0] = val;
data[1] = 0.0f;
data[2] = 0.0f;
put(timestamp, data, error_count_in, priority_in);
}
void
DataValidator::put(uint64_ttimestamp, float val[3], uint64_t error_count_in, int priority_in)
{
_event_count++;
if (error_count_in > _error_count) {
_error_density += (error_count_in - _error_count);
} else if (_error_density > 0) {
_error_density--;
}
_error_count = error_count_in;
_priority = priority_in;
for (unsigned i = 0; i < _dimensions; i++) {
if (_time_last == 0) {
_mean[i] = 0;
_lp[i] = val[i];
_M2[i] = 0;
} else {
float lp_val = val[i] - _lp[i];
float delta_val = lp_val - _mean[i];
_mean[i] += delta_val / _event_count;
_M2[i] += delta_val * (lp_val -_mean[i]);
_rms[i] = sqrtf(_M2[i] / (_event_count -1));
if (fabsf(_value[i] - val[i]) <0.000001f) {
_value_equal_count++;
} else {
_value_equal_count = 0;
}
}
_vibe[i] = _vibe[i] * 0.99f + 0.01f * fabsf(val[i]- _lp[i]);
// XXX replace with better filter, make itauto-tune to update rate
_lp[i] = _lp[i] * 0.99f + 0.01f * val[i];
_value[i] = val[i];
}
_time_last = timestamp;
}先将每组的数据（例如gyro）将三个维度的的传感器数据put入（如_voter_gyro.put(...))）DataValidatorGroup中，并goto到DataValidator::put函数
在DataValidator函数中计算数据的误差、平均值、并进行滤波。
滤波入口的put函数：
val=传感器读取的数据
_lp=滤波器的系数（lowpass value）
初始化：由上图可知当第一次读到传感器数据时_mean和_M2置0，_lp=val；
lp_val= val - _lp
delta_val= lp_val - _mean
_mean= (平均值）每次数据读取时，每次val和_lp的差值之和的平均值 _mean[i] += delta_val / _event_count
_M2= （均方根值）delta_val * (lp_val - _mean)的和
_rms= 均方根值sqrtf(_M2[i] / (_event_count - 1))
优化滤波器系数：_lp[i]= _lp[i] * 0.5f + val[i] * 0.5f
/*ignore empty fields */
if(sensors.accelerometer_timestamp[i] > 0) {
_voter_accel.put(i,sensors.accelerometer_timestamp[i], &sensors.accelerometer_m_s2[i * 3],
sensors.accelerometer_errcount[i],sensors.accelerometer_priority[i]);
}
/*ignore empty fields */
if(sensors.magnetometer_timestamp[i] > 0) {
_voter_mag.put(i,sensors.magnetometer_timestamp[i], &sensors.magnetometer_ga[i * 3],
sensors.magnetometer_errcount[i],sensors.magnetometer_priority[i]);
}
}
//Get best measurement values
hrt_abstimecurr_time = hrt_absolute_time();
_gyro.set(_voter_gyro.get_best(curr_time,&best_gyro));
_accel.set(_voter_accel.get_best(curr_time,&best_accel));
_mag.set(_voter_mag.get_best(curr_time,&best_mag));Ardupilot/modules/PX4Firmware/src/lib/ecl/validation/data_validator_group.cpp
float*
DataValidatorGroup::get_best(uint64_ttimestamp, int *index)
{
DataValidator *next = _first;
// XXX This should eventually also include voting
int pre_check_best = _curr_best;
float pre_check_confidence = 1.0f;
int pre_check_prio = -1;
float max_confidence = -1.0f;
int max_priority = -1000;
int max_index = -1;
DataValidator *best = nullptr;
unsigned i = 0;
while (next != nullptr) {
float confidence = next->confidence(timestamp);
if (static_cast(i) == pre_check_best) {
pre_check_prio = next->priority();
pre_check_confidence = confidence;
} Ardupilot/modules/PX4Firmware/src/lib/ecl/validation/data_validator.cpp
float
DataValidator::confidence(uint64_ttimestamp)
{
float ret = 1.0f;
/* check if we have any data */
if (_time_last == 0) {
_error_mask |=ERROR_FLAG_NO_DATA;
ret = 0.0f;
/* timed out - that's it */
} else if (timestamp - _time_last >_timeout_interval) {
_error_mask |=ERROR_FLAG_TIMEOUT;
ret = 0.0f;
/* we got the exact same sensor value Ntimes in a row */
} else if (_value_equal_count >VALUE_EQUAL_COUNT_MAX) {
_error_mask |=ERROR_FLAG_STALE_DATA;
ret = 0.0f;
/* check error count limit */
} else if (_error_count >NORETURN_ERRCOUNT) {
_error_mask |=ERROR_FLAG_HIGH_ERRCOUNT;
ret = 0.0f;
/* cap error density counter at windowsize */
} else if (_error_density >ERROR_DENSITY_WINDOW) {
_error_mask |=ERROR_FLAG_HIGH_ERRDENSITY;
_error_density =ERROR_DENSITY_WINDOW;
/* no error */
} else {
_error_mask = ERROR_FLAG_NO_ERROR;
}
/* no critical errors */
if (ret > 0.0f) {
/* return local error densityfor last N measurements */
ret = 1.0f - (_error_density/ ERROR_DENSITY_WINDOW);
}
return ret;
}滤波器的confidence函数（信任度函数，貌似模糊控制理论有个隶属函数，应该类似的功能）：返回值是对上N次测量的验证的信任程度，其值在0到1之间，越大越好。返回值是返回上N次测量的误差诊断，用于get_best函数选择最优值
/*
* Switchif:
* 1) theconfidence is higher and priority is equal or higher
* 2) theconfidence is no less than 1% different and the priority is higher
*/
if (((max_confidence < MIN_REGULAR_CONFIDENCE)&& (confidence >= MIN_REGULAR_CONFIDENCE)) ||
(confidence > max_confidence&& (next->priority() >= max_priority)) ||
(fabsf(confidence - max_confidence) <0.01f && (next->priority() > max_priority))
) {
max_index = i;
max_confidence = confidence;
max_priority = next->priority();
best = next;
}
next = next->sibling();
i++;
}
/* the current best sensor is not matching the previous bestsensor */
if (max_index != _curr_best) {
bool true_failsafe = true;
/* check whether the switch was a failsafe orpreferring a higher priority sensor */
if (pre_check_prio != -1 && pre_check_prio< max_priority &&
fabsf(pre_check_confidence -max_confidence) < 0.1f) {
/* this is not a failover */
true_failsafe = false;
/* reset error flags, this is likely ahotplug sensor coming online late */
best->reset_state();
}
/* if we're no initialized, initialize thebookkeeping but do not count a failsafe */
if (_curr_best < 0) {
_prev_best = max_index;
} else {
/* we were initialized before, this is areal failsafe */
_prev_best = pre_check_best;
if (true_failsafe) {
_toggle_count++;
/* if this is the first time,log when we failed */
if (_first_failover_time == 0) {
_first_failover_time= timestamp;
}
}
}
/* for all cases we want to keep a record of thebest index */
_curr_best = max_index;
}
*index = max_index;
return (best) ? best->value() : nullptr;
}
if(_accel.length() < 0.01f) {
warnx("WARNING:degenerate accel!");
continue;
}
if(_mag.length() < 0.01f) {
warnx("WARNING:degenerate mag!");
continue;
}
_data_good= true;
if(!_failsafe) {
uint32_tflags = DataValidator::ERROR_FLAG_NO_ERROR;
#ifdef __PX4_POSIX
perf_end(_perf_accel);
perf_end(_perf_mpu);
perf_end(_perf_mag);
#endif
if(_voter_gyro.failover_count() > 0) {
_failsafe= true;
flags= _voter_gyro.failover_state();
mavlink_and_console_log_emergency(&_mavlink_log_pub,"Gyro #%i failure :%s%s%s%s%s!",
_voter_gyro.failover_index(),
((flags &DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""),
((flags &DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" :""),
((flags &DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""),
((flags &DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" :""),
((flags &DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" :""));
}
if(_voter_accel.failover_count() > 0) {
_failsafe= true;
flags= _voter_accel.failover_state();
mavlink_and_console_log_emergency(&_mavlink_log_pub,"Accel #%i failure :%s%s%s%s%s!",
_voter_accel.failover_index(),
((flags &DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""),
((flags &DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" :""),
((flags &DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""),
((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT)? " High error count" : ""),
((flags &DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" :""));
}
if(_voter_mag.failover_count() > 0) {
_failsafe= true;
flags= _voter_mag.failover_state();
mavlink_and_console_log_emergency(&_mavlink_log_pub,"Mag #%i failure :%s%s%s%s%s!",
_voter_mag.failover_index(),
((flags &DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""),
((flags & DataValidator::ERROR_FLAG_STALE_DATA)? " Stale data" : ""),
((flags &DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""),
((flags &DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" :""),
((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY)? " High error density" : ""));_voter_gyro、_voter_accel、_voter_mag三个参数的failover_count函数判断是否存在数据获取失误问题，并通过mavlink协议显示错误原因
}
if(_failsafe) {
mavlink_and_console_log_emergency(&_mavlink_log_pub,"SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY");
}
}
if(!_vibration_warning && (_voter_gyro.get_vibration_factor(curr_time)> _vibration_warning_threshold ||
_voter_accel.get_vibration_factor(curr_time) >_vibration_warning_threshold ||
_voter_mag.get_vibration_factor(curr_time) >_vibration_warning_threshold)) {Ardupilot/modules/PX4Firmware/src/lib/ecl/validation/data_validator_group.cpp
float
DataValidatorGroup::get_vibration_factor(uint64_ttimestamp)
{
DataValidator *next = _first;
float vibe = 0.0f;
/* find the best RMS value of a non-timed out sensor */
while (next != nullptr) {
if (next->confidence(timestamp) > 0.5f) {
float* rms = next->rms();
for (unsigned j = 0; j < 3; j++) {
if (rms[j] > vibe) {
vibe= rms[j];
}
}
}
next = next->sibling();
}
return vibe;//将rms变量（原来put函数中的_rms）传回主函数中和_vibration_warning_threshold进行判断。
}根据_voter_gyro、_voter_accel、_voter_mag三个参数的get_vibration_factor函数判断是否有震动现象，返回值是float型的RSM值，其代表振动的幅度大小。
if(_vibration_warning_timestamp == 0) {
_vibration_warning_timestamp= curr_time;
}else if (hrt_elapsed_time(&_vibration_warning_timestamp) > 10000000) {
_vibration_warning= true;
mavlink_and_console_log_critical(&_mavlink_log_pub,"HIGH VIBRATION! g: %d a: %d m: %d",
(int)(100 *_voter_gyro.get_vibration_factor(curr_time)),
(int)(100 *_voter_accel.get_vibration_factor(curr_time)),
(int)(100 *_voter_mag.get_vibration_factor(curr_time)));
}
}else {
_vibration_warning_timestamp= 0;
}
}
//Update vision and motion capture heading
boolvision_updated = false;
orb_check(_vision_sub,&vision_updated);
boolmocap_updated = false;
orb_check(_mocap_sub,&mocap_updated);
if(vision_updated) {
orb_copy(ORB_ID(vision_position_estimate),_vision_sub, &_vision);
math::Quaternionq(_vision.q);
math::Matrix<3,3> Rvis = q.to_dcm();
math::Vector<3>v(1.0f, 0.0f, 0.4f);
//Rvis is Rwr (robot respect to world) while v is respect to world.
//Hence Rvis must be transposed having (Rwr)' * Vw
//Rrw * Vw = vn. This way we have consistency
_vision_hdg= Rvis.transposed() * v;
}
if(mocap_updated) {
orb_copy(ORB_ID(att_pos_mocap),_mocap_sub, &_mocap);
math::Quaternionq(_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;
}通过uORB模型获取vision和mocap的数据（视觉和mocap）
//Update airspeed
boolairspeed_updated = false;
orb_check(_airspeed_sub,&airspeed_updated);
if(airspeed_updated) {
orb_copy(ORB_ID(airspeed),_airspeed_sub, &_airspeed);
}
//Check for timeouts on data
if(_ext_hdg_mode == 1) {
_ext_hdg_good= _vision.timestamp_boot > 0 &&(hrt_elapsed_time(&_vision.timestamp_boot) < 500000);
}else if (_ext_hdg_mode == 2) {
_ext_hdg_good= _mocap.timestamp_boot > 0 &&(hrt_elapsed_time(&_mocap.timestamp_boot) < 500000);
}
boolgpos_updated;
orb_check(_global_pos_sub,&gpos_updated);
if(gpos_updated) {
orb_copy(ORB_ID(vehicle_global_position),_global_pos_sub, &_gpos);
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)));
}
}
if(_acc_comp && _gpos.timestamp != 0 && hrt_absolute_time() <_gpos.timestamp + 20000 && _gpos.eph < 5.0f && _inited) {
/*position data is actual */
if(gpos_updated) {
Vector<3>vel(_gpos.vel_n, _gpos.vel_e, _gpos.vel_d);
/*velocity updated */
if(_vel_prev_t != 0 && _gpos.timestamp != _vel_prev_t) {
floatvel_dt = (_gpos.timestamp - _vel_prev_t) / 1000000.0f;
/*calculate acceleration in body frame */
_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;
}位置加速度获取（position，注意最后在修正时会用到该处的_pos_acc）
/*time from previous iteration */
hrt_abstimenow = hrt_absolute_time();
floatdt = (last_time > 0) ? ((now -last_time) / 1000000.0f) : 0.00001f;
last_time= now;
if(dt > _dt_max) {
dt= _dt_max;
}
if(!update(dt)) {
continue;
}Ardupilot/modules/PX4Firmware/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
boolAttitudeEstimatorQ::update(float dt)
{
if (!_inited) {
if (!_data_good) {
return false;
}
return init();
}首先判断是否是第一次进入该函数，第一次进入该函数先进入init函数初始化，源码如下
bool AttitudeEstimatorQ::init()
{
// Rotation matrix can be easilyconstructed from acceleration and mag field vectors
// 'k' is Earth Z axis (Down) unitvector in body frame
Vector<3> k = -_accel;
k.normalize();初始化方法：k为加速度传感器测量到加速度方向向量，由于第一次测量数据时无人机一般为平稳状态无运动状态，所以可以直接将测到的加速度视为重力加速度g，以此作为dcm旋转矩阵的第三行k
// 'i' is Earth X axis (North) unitvector in body frame, orthogonal with 'k'
Vector<3> i = (_mag - k * (_mag *k));
i.normalize();向量指向正北方，k*(_mag*k)正交correction值，对于最终的四元数归一化以后的范数可以在正负5%以内；感觉不如《DCM IMU:Theory》中提出的理论“强制正交化”修正的好，Renormalization算法在ardupilot的上层应用AP_AHRS_DCM中使用到了
// 'j' is Earth Y axis (East) unitvector in body frame, orthogonal with 'k' and 'i'
Vector<3> j = k % i;外积、叉乘。关于上面的Vector<3>k =-_accel，Vector<3>相当于一个类型（int）定义一个变量k，然后把-_accel负值给k，在定义_accel时也是使用Vector<3>，属于同一种类型的，主要就是为了考虑其实例化过程（类似函数重载）。
// Fill rotation matrix
Matrix<3, 3> R;
R.set_row(0, i);
R.set_row(1, j);
R.set_row(2, k);然后以模板的形式使用“Matrix<3, 3> R”建立3x3矩阵R，通过set_row()函数赋值
ardupilot/modules/PX4Firmware/lib/mathlib/math/Matrix.hpp
/**
* set row from vector
*/
void set_row(unsigned int row, constVector v) {
for (unsigned i = 0; i 第一行赋值i向量指向北，第二行赋值j向量指向东，第三行赋值k向量指向DOWN
// Convert to quaternion
_q.from_dcm(R);ardupilot/modules/PX4Firmware/lib/mathlib/math/Quaternion.hpp
/**
* set quaternion to rotation by DCM
* Reference: Shoemake, Quaternions,http://www.cs.ucr.edu/~vbz/resources/quatut.pdf
*/
void from_dcm(const Matrix<3, 3>&dcm) {
float tr = dcm.data[0][0] +dcm.data[1][1] + dcm.data[2][2];
if (tr > 0.0f) {
float s = sqrtf(tr +1.0f);
data[0] = s * 0.5f;
s = 0.5f / s;
data[1] =(dcm.data[2][1] - dcm.data[1][2]) * s;
data[2] =(dcm.data[0][2] - dcm.data[2][0]) * s;
data[3] = (dcm.data[1][0]- dcm.data[0][1]) * s;
}其他情况去看代码吧，不好解释。然后_q单位化结束初始化。PS：另外根据DCM求取四元数的算法是参考MartinJohnBaker的，就是上述的原始版，该算法在AP_Math/quaternion.cpp中，链接：
http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.h
源码如下
Ardupilot/libraries/AP_Math/quaternion.cpp PS:求四元数参考的代码DCM求取四元数的算法
// return therotation matrix equivalent for this quaternion
// Thanks to MartinJohn Baker
//http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm
voidQuaternion::from_rotation_matrix(const Matrix3f &m)
{
const float &m00 = m.a.x;
const float &m11 = m.b.y;
const float &m22 = m.c.z;
const float &m10 = m.b.x;
const float &m01 = m.a.y;
const float &m20 = m.c.x;
const float &m02 = m.a.z;
const float &m21 = m.c.y;
const float &m12 = m.b.z;
float &qw = q1;
float &qx = q2;
float &qy = q3;
float &qz = q4;
float tr = m00 + m11 + m22;
if (tr > 0) {
float S = sqrtf(tr+1) * 2;
qw = 0.25f * S;
qx = (m21 - m12) / S;
qy = (m02 - m20) / S;
qz = (m10 - m01) / S;
} else if ((m00 > m11) && (m00> m22)) {
float S = sqrtf(1.0f + m00 - m11 - m22)* 2;
qw = (m21 - m12) / S;
qx = 0.25f * S;
qy = (m01 + m10) / S;
qz = (m02 + m20) / S;
} else if (m11 > m22) {
float S = sqrtf(1.0f + m11 - m00 - m22)* 2;
qw = (m02 - m20) / S;
qx = (m01 + m10) / S;
qy = 0.25f * S;
qz = (m12 + m21) / S;
} else {
float S = sqrtf(1.0f + m22 - m00 - m11)* 2;
qw = (m10 - m01) / S;
qx = (m02 + m20) / S;
qy = (m12 + m21) / S;
qz = 0.25f * S;
}
}
else {
/* Find maximumdiagonal element in dcm
* store index indcm_i */
int dcm_i = 0;
for (int i = 1; i< 3; i++) {
if(dcm.data[i][i] > dcm.data[dcm_i][dcm_i]) {
dcm_i= i;
}
}
int dcm_j = (dcm_i +1) % 3;
int dcm_k = (dcm_i +2) % 3;
float s =sqrtf((dcm.data[dcm_i][dcm_i] - dcm.data[dcm_j][dcm_j] -
dcm.data[dcm_k][dcm_k])+ 1.0f);
data[dcm_i + 1] = s* 0.5f;
s= 0.5f / s;
data[dcm_j + 1] =(dcm.data[dcm_i][dcm_j] + dcm.data[dcm_j][dcm_i]) * s;
data[dcm_k + 1] =(dcm.data[dcm_k][dcm_i] + dcm.data[dcm_i][dcm_k]) * s;
data[0] =(dcm.data[dcm_k][dcm_j] - dcm.data[dcm_j][dcm_k]) * s;
}
} // Compensate for magnetic declination
Quaternion decl_rotation;
decl_rotation.from_yaw(_mag_decl);
_q = decl_rotation * _q;
_q.normalize();DCM转换为四元数_q（使用from_dcm），并归一化四元数，一定要保持归一化的思想
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;
} Quaternion q_last = _q;
// Angular rate of correction
Vector<3> corr;
if (_ext_hdg_mode > 0 && _ext_hdg_good) {
if (_ext_hdg_mode == 1) {
// Vision heading correction
// Project heading to global frame andextract XY component
Vector<3> vision_hdg_earth =_q.conjugate(_vision_hdg);
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 andextract 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;
}
}
如果不是第一次进入该函数，则判断是使用什么mode做修正的，比如vision、mocap、acc和mag（DJI精灵4用的视觉壁障应该就是这个vision），Hdg就是heading。
_ext_hdg_mode==1、2时都是利用vision数据和mocap数据对gyro数据进行修正
if (_ext_hdg_mode == 0 || !_ext_hdg_good) {
// Magnetometer correction
// Project mag field vector to global frame andextract XY component
Vector<3> mag_earth = _q.conjugate(_mag);
float mag_err = _wrap_pi(atan2f(mag_earth(1),mag_earth(0)) - _mag_decl);
// Project magnetometer correction to body frame
corr +=_q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mag_err)) * _w_mag;
}利用磁力计修正, _w_mag为mag的权重
mag_earth=_q.conjugate(_mag)，这行代码的含义为先将归一化的_q的旋转矩阵R（b系转n系）乘以_mag向量（以自身机体坐标系为视角看向北方的向量表示），得到n系（NED坐标系）下的磁力计向量
如下就是conjugate函数，其过程就是实现从b系到n系的转换，使用左乘
ardupilot/modules/PX4Firmware/src/lib/mathlib/math/Quaternion.hpp
/**
* conjugation //b系到n系
*/
Vector<3> conjugate(constVector<3> &v) const {
float q0q0 = data[0] *data[0];
float q1q1 = data[1] *data[1];
float q2q2 = data[2] *data[2];
float q3q3 = data[3] *data[3];
return Vector<3>(
v.data[0] *(q0q0 + q1q1 - q2q2 - q3q3) +
v.data[1] *2.0f * (data[1] * data[2] - data[0] * data[3]) +
v.data[2] *2.0f * (data[0] * data[2] + data[1] * data[3]),
v.data[0] *2.0f * (data[1] * data[2] + data[0] * data[3]) +
v.data[1] *(q0q0 - q1q1 + q2q2 - q3q3) +
v.data[2] *2.0f * (data[2] * data[3] - data[0] * data[1]),
v.data[0] *2.0f * (data[1] * data[3] - data[0] * data[2]) +
v.data[1] *2.0f * (data[0] * data[1] + data[2] * data[3]) +
v.data[2] *(q0q0 - q1q1 - q2q2 + q3q3)
);
} _q.normalize();
// Accelerometer correction
// Project 'k' unit vector of earth frame to body frame
// Vector<3> k =_q.conjugate_inversed(Vector<3>(0.0f, 0.0f, 1.0f));
// Optimized version with dropped zeros
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))
);
corr += (k % (_accel - _pos_acc).normalized()) * _w_accel;
// Gyro bias estimation
_gyro_bias += corr * (_w_gyro_bias * dt);
for (int i = 0; i < 3; i++) {
_gyro_bias(i) = math::constrain(_gyro_bias(i),-_bias_max, _bias_max);
}
_rates = _gyro + _gyro_bias;
// Feed forward gyro
corr += _rates;
// Apply correction to state
_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)))) {
// Reset quaternion to last good state
_q = q_last;
_rates.zero();
_gyro_bias.zero();
return false;
}
return true;
} Vector<3>euler = _q.to_euler();
structvehicle_attitude_s att = {};
att.timestamp= sensors.timestamp;
att.roll= euler(0);
att.pitch= euler(1);
att.yaw= euler(2);
att.rollspeed= _rates(0);
att.pitchspeed= _rates(1);
att.yawspeed= _rates(2);
for(int i = 0; i < 3; i++) {
att.g_comp[i]= _accel(i) - _pos_acc(i);
}
/*copy offsets */
memcpy(&att.rate_offsets,_gyro_bias.data, sizeof(att.rate_offsets));
Matrix<3,3> R = _q.to_dcm();
/*copy rotation matrix */
memcpy(&att.R[0],R.data, sizeof(att.R));
att.R_valid= true;
memcpy(&att.q[0],_q.data, sizeof(att.q));
att.q_valid= true;
att.rate_vibration= _voter_gyro.get_vibration_factor(hrt_absolute_time());
att.accel_vibration= _voter_accel.get_vibration_factor(hrt_absolute_time());
att.mag_vibration= _voter_mag.get_vibration_factor(hrt_absolute_time());
/*the instance count is not used here */
intatt_inst;
orb_publish_auto(ORB_ID(vehicle_attitude),&_att_pub, &att, &att_inst, ORB_PRIO_HIGH);
{
structcontrol_state_s ctrl_state = {};
ctrl_state.timestamp= sensors.timestamp;
/*attitude quaternions for control state */
ctrl_state.q[0]= _q(0);
ctrl_state.q[1]= _q(1);
ctrl_state.q[2]= _q(2);
ctrl_state.q[3]= _q(3);
/*attitude rates for control state */
ctrl_state.roll_rate= _lp_roll_rate.apply(_rates(0));
ctrl_state.pitch_rate= _lp_pitch_rate.apply(_rates(1));
ctrl_state.yaw_rate= _lp_yaw_rate.apply(_rates(2));
/*Airspeed - take airspeed measurement directly here as no wind is estimated */
if(PX4_ISFINITE(_airspeed.indicated_airspeed_m_s) && hrt_absolute_time()- _airspeed.timestamp < 1e6
&& _airspeed.timestamp > 0) {
ctrl_state.airspeed= _airspeed.indicated_airspeed_m_s;
ctrl_state.airspeed_valid= true;
}else {
ctrl_state.airspeed_valid= false;
}
/*the instance count is not used here */
intctrl_inst;
/*publish to control state topic */
orb_publish_auto(ORB_ID(control_state),&_ctrl_state_pub, &ctrl_state, &ctrl_inst, ORB_PRIO_HIGH);
}
{
structestimator_status_s est = {};
est.timestamp= sensors.timestamp;
est.vibe[0]= _voter_accel.get_vibration_offset(est.timestamp, 0);
est.vibe[1]= _voter_accel.get_vibration_offset(est.timestamp, 1);
est.vibe[2]= _voter_accel.get_vibration_offset(est.timestamp, 2);
/*the instance count is not used here */
intest_inst;
/*publish to control state topic */
orb_publish_auto(ORB_ID(estimator_status),&_est_state_pub, &est, &est_inst, ORB_PRIO_HIGH);
}
}
}在rc.mc_apps中，我们可以选择运行了哪个姿态估计和控制程序，不懂的话请看http://blog.csdn.net/czyv587/article/details/51397213最下面
默认的是：
姿态估计 Attitude_estimator_q
位置估计 position_estimator_inav
姿态控制 mc_att_control
位置控制 mc_pos_control
extern "C" __EXPORT int mc_att_control_main(intargc, char *argv[])跳转到
int mc_att_control_main(intargc, char *argv[])
{
if (argc < 2) {
warnx("usage:mc_att_control {start|stop|status}");
return 1;
}
if (!strcmp(argv[1],"start")) {
if (mc_att_control::g_control!= nullptr) {
warnx("alreadyrunning");
return 1;
}
mc_att_control::g_control =new MulticopterAttitudeControl;
if (mc_att_control::g_control== nullptr) {
warnx("allocfailed");
return 1;
}
if (OK !=mc_att_control::g_control->start()) {
deletemc_att_control::g_control;
mc_att_control::g_control= nullptr;
warnx("startfailed");
return 1;
}
return 0;
}
if (!strcmp(argv[1], "stop")){
if (mc_att_control::g_control== nullptr) {
warnx("notrunning");
return 1;
}
deletemc_att_control::g_control;
mc_att_control::g_control =nullptr;
return 0;
}
if (!strcmp(argv[1],"status")) {
if(mc_att_control::g_control) {
warnx("running");
return 0;
} else {
warnx("notrunning");
return 1;
}
}
warnx("unrecognizedcommand");
return 1;
}该函数是通过if (!strcmp(argv[1], "xxx"))选择要如何处理，我们接着看if (!strcmp(argv[1], "start"))
其中，mc_att_control::g_control = new MulticopterAttitudeControl;，new关键词应该不陌生吧，类似于C语言中的malloc，对变量进行内存分配的，即对姿态控制过程中使用到的变量赋初值。
跳转到start()
int
MulticopterAttitudeControl::start()
{
ASSERT(_control_task== -1);
/* start the task*/
_control_task =px4_task_spawn_cmd("mc_att_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
1500,
(px4_main_t)&MulticopterAttitudeControl::task_main_trampoline,
nullptr);
if (_control_task< 0) {
warn("taskstart failed");
return-errno;
}
return OK;
}跳转到task_main_trampoline ()
void
MulticopterAttitudeControl::task_main_trampoline(intargc, char *argv[])
{
mc_att_control::g_control->task_main();
}跳转到task_main()，由于此函数内容较多，因此直接拆开分析，最后再简短的总结下
void
MulticopterAttitudeControl::task_main()
{
/*
* do subscriptions
*/
_v_att_sp_sub =orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_v_rates_sp_sub =orb_subscribe(ORB_ID(vehicle_rates_setpoint));
_ctrl_state_sub =orb_subscribe(ORB_ID(control_state));
_v_control_mode_sub =orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub =orb_subscribe(ORB_ID(parameter_update));
_manual_control_sp_sub =orb_subscribe(ORB_ID(manual_control_setpoint));
_armed_sub =orb_subscribe(ORB_ID(actuator_armed));
_vehicle_status_sub =orb_subscribe(ORB_ID(vehicle_status));
_motor_limits_sub =orb_subscribe(ORB_ID(multirotor_motor_limits));订阅各种信息
structcontrol_state_s _ctrl_state; /**< control state */
structvehicle_attitude_setpoint_s _v_att_sp; /**< vehicleattitude setpoint */
structvehicle_rates_setpoint_s _v_rates_sp; /**< vehicle rates setpoint */
structmanual_control_setpoint_s _manual_control_sp; /**< manual control setpoint */
struct vehicle_control_mode_s _v_control_mode; /**< vehicle control mode */
structactuator_controls_s _actuators; /**< actuatorcontrols */
structactuator_armed_s _armed; /**< actuatorarming status */
structvehicle_status_s _vehicle_status; /**< vehicle status */
structmultirotor_motor_limits_s _motor_limits; /**< motor limits */
structmc_att_ctrl_status_s _controller_status;/**< controller status */
#ifdef __cplusplus
struct __EXPORT vehicle_attitude_setpoint_s {
#else
struct vehicle_attitude_setpoint_s {
#endif
uint64_ttimestamp;
floatroll_body;
floatpitch_body;
float yaw_body;
floatyaw_sp_move_rate;
floatR_body[9];
bool R_valid;
float q_d[4];
bool q_d_valid;
float q_e[4];
bool q_e_valid;
float thrust;
boolroll_reset_integral;
boolpitch_reset_integral;
boolyaw_reset_integral;
boolfw_control_yaw;
booldisable_mc_yaw_control;
boolapply_flaps;
#ifdef __cplusplus
#endif
};
#ifdef __cplusplus
struct __EXPORT vehicle_rates_setpoint_s {
#else
struct vehicle_rates_setpoint_s {
#endif
uint64_ttimestamp;
float roll;
float pitch;
float yaw;
float thrust;
#ifdef __cplusplus
#endif
};
#ifdef __cplusplus
struct __EXPORT control_state_s {
#else
struct control_state_s {
#endif
uint64_ttimestamp;
float x_acc;
float y_acc;
float z_acc;
float x_vel;
float y_vel;
float z_vel;
float x_pos;
float y_pos;
float z_pos;
float airspeed;
boolairspeed_valid;
floatvel_variance[3];
floatpos_variance[3];
float q[4];
floatroll_rate;
float pitch_rate;
float yaw_rate;
floathorz_acc_mag;
#ifdef __cplusplus
#endif
};
#ifdef __cplusplus
struct __EXPORT vehicle_control_mode_s {
#else
struct vehicle_control_mode_s {
#endif
uint64_ttimestamp;
boolflag_armed;
bool flag_external_manual_override_ok;
boolflag_system_hil_enabled;
boolflag_control_manual_enabled;
boolflag_control_auto_enabled;
boolflag_control_offboard_enabled;
boolflag_control_rates_enabled;
boolflag_control_attitude_enabled;
bool flag_control_force_enabled;
boolflag_control_velocity_enabled;
boolflag_control_position_enabled;
boolflag_control_altitude_enabled;
boolflag_control_climb_rate_enabled;
boolflag_control_termination_enabled;
#ifdef __cplusplus
#endif
};
#ifdef __cplusplus
struct __EXPORT parameter_update_s {
#else
struct parameter_update_s {
#endif
uint64_ttimestamp;
bool saved;
#ifdef __cplusplus
#endif
};
#ifdef __cplusplus
struct __EXPORT manual_control_setpoint_s {
#else
struct manual_control_setpoint_s {
#endif
uint64_ttimestamp;
float x;
float y;
float z;
float r;
float flaps;
float aux1;
float aux2;
float aux3;
float aux4;
float aux5;
uint8_tmode_switch;
uint8_treturn_switch;
uint8_trattitude_switch;
uint8_tposctl_switch;
uint8_tloiter_switch;
uint8_tacro_switch;
uint8_toffboard_switch;
uint8_tkill_switch;
int8_tmode_slot;
#ifdef __cplusplus
static constuint8_t SWITCH_POS_NONE = 0;
static constuint8_t SWITCH_POS_ON = 1;
static constuint8_t SWITCH_POS_MIDDLE = 2;
static constuint8_t SWITCH_POS_OFF = 3;
static constint8_t MODE_SLOT_NONE = -1;
static constint8_t MODE_SLOT_1 = 0;
static constint8_t MODE_SLOT_2 = 1;
static constint8_t MODE_SLOT_3 = 2;
static constint8_t MODE_SLOT_4 = 3;
static constint8_t MODE_SLOT_5 = 4;
static constint8_t MODE_SLOT_6 = 5;
static constint8_t MODE_SLOT_MAX = 6;
#endif
};
#ifdef __cplusplus
struct __EXPORT actuator_armed_s {
#else
struct actuator_armed_s {
#endif
uint64_ttimestamp;
bool armed;
bool prearmed;
boolready_to_arm;
bool lockdown;
boolforce_failsafe;
boolin_esc_calibration_mode;
#ifdef __cplusplus
#endif
};
#ifdef __cplusplus
struct __EXPORT vehicle_status_s {
#else
struct vehicle_status_s {
#endif
uint16_tcounter;
uint64_ttimestamp;
uint8_tmain_state;
uint8_tmain_state_prev;
uint8_tnav_state;
uint8_tarming_state;
uint8_thil_state;
bool failsafe;
boolcalibration_enabled;
int32_tsystem_type;
uint32_tsystem_id;
uint32_tcomponent_id;
boolis_rotary_wing;
bool is_vtol;
boolvtol_fw_permanent_stab;
boolin_transition_mode;
boolcondition_battery_voltage_valid;
boolcondition_system_in_air_restore;
boolcondition_system_sensors_initialized;
boolcondition_system_prearm_error_reported;
boolcondition_system_hotplug_timeout;
bool condition_system_returned_to_home;
boolcondition_auto_mission_available;
boolcondition_global_position_valid;
boolcondition_home_position_valid;
boolcondition_local_position_valid;
boolcondition_local_altitude_valid;
boolcondition_airspeed_valid;
boolcondition_landed;
boolcondition_power_input_valid;
floatavionics_power_rail_voltage;
boolusb_connected;
boolrc_signal_found_once;
boolrc_signal_lost;
uint64_trc_signal_lost_timestamp;
boolrc_signal_lost_cmd;
boolrc_input_blocked;
uint8_trc_input_mode;
booldata_link_lost;
booldata_link_lost_cmd;
uint8_tdata_link_lost_counter;
boolengine_failure;
boolengine_failure_cmd;
boolvtol_transition_failure;
boolvtol_transition_failure_cmd;
boolgps_failure;
bool gps_failure_cmd;
boolmission_failure;
boolbarometer_failure;
booloffboard_control_signal_found_once;
booloffboard_control_signal_lost;
booloffboard_control_signal_weak;
uint64_toffboard_control_signal_lost_interval;
booloffboard_control_set_by_command;
uint32_tonboard_control_sensors_present;
uint32_tonboard_control_sensors_enabled;
uint32_tonboard_control_sensors_health;
float load;
floatbattery_voltage;
floatbattery_current;
floatbattery_remaining;
floatbattery_discharged_mah;
uint32_tbattery_cell_count;
uint8_tbattery_warning;
uint16_tdrop_rate_comm;
uint16_terrors_comm;
uint16_terrors_count1;
uint16_terrors_count2;
uint16_terrors_count3;
uint16_terrors_count4;
boolcircuit_breaker_engaged_power_check;
bool circuit_breaker_engaged_airspd_check;
boolcircuit_breaker_engaged_enginefailure_check;
boolcircuit_breaker_engaged_gpsfailure_check;
bool cb_usb;
#ifdef __cplusplus
static constuint8_t MAIN_STATE_MANUAL = 0;
static constuint8_t MAIN_STATE_ALTCTL = 1;
static constuint8_t MAIN_STATE_POSCTL = 2;
static constuint8_t MAIN_STATE_AUTO_MISSION = 3;
static constuint8_t MAIN_STATE_AUTO_LOITER = 4;
static constuint8_t MAIN_STATE_AUTO_RTL = 5;
static constuint8_t MAIN_STATE_ACRO = 6;
static constuint8_t MAIN_STATE_OFFBOARD = 7;
static constuint8_t MAIN_STATE_STAB = 8;
static constuint8_t MAIN_STATE_RATTITUDE = 9;
static constuint8_t MAIN_STATE_AUTO_TAKEOFF = 10;
static constuint8_t MAIN_STATE_AUTO_LAND = 11;
static constuint8_t MAIN_STATE_AUTO_FOLLOW_TARGET = 12;
static constuint8_t MAIN_STATE_MAX = 13;
static constuint8_t ARMING_STATE_INIT = 0;
static constuint8_t ARMING_STATE_STANDBY = 1;
static constuint8_t ARMING_STATE_ARMED = 2;
static constuint8_t ARMING_STATE_ARMED_ERROR = 3;
static constuint8_t ARMING_STATE_STANDBY_ERROR = 4;
static constuint8_t ARMING_STATE_REBOOT = 5;
static constuint8_t ARMING_STATE_IN_AIR_RESTORE = 6;
static constuint8_t ARMING_STATE_MAX = 7;
static constuint8_t HIL_STATE_OFF = 0;
static constuint8_t HIL_STATE_ON = 1;
static constuint8_t NAVIGATION_STATE_MANUAL = 0;
static constuint8_t NAVIGATION_STATE_ALTCTL = 1;
static constuint8_t NAVIGATION_STATE_POSCTL = 2;
static constuint8_t NAVIGATION_STATE_AUTO_MISSION = 3;
static const uint8_tNAVIGATION_STATE_AUTO_LOITER = 4;
static constuint8_t NAVIGATION_STATE_AUTO_RTL = 5;
static constuint8_t NAVIGATION_STATE_AUTO_RCRECOVER = 6;
static constuint8_t NAVIGATION_STATE_AUTO_RTGS = 7;
static constuint8_t NAVIGATION_STATE_AUTO_LANDENGFAIL = 8;
static constuint8_t NAVIGATION_STATE_AUTO_LANDGPSFAIL = 9;
static constuint8_t NAVIGATION_STATE_ACRO = 10;
static constuint8_t NAVIGATION_STATE_UNUSED = 11;
static constuint8_t NAVIGATION_STATE_DESCEND = 12;
static constuint8_t NAVIGATION_STATE_TERMINATION = 13;
static constuint8_t NAVIGATION_STATE_OFFBOARD = 14;
static constuint8_t NAVIGATION_STATE_STAB = 15;
static constuint8_t NAVIGATION_STATE_RATTITUDE = 16;
static constuint8_t NAVIGATION_STATE_AUTO_TAKEOFF = 17;
static constuint8_t NAVIGATION_STATE_AUTO_LAND = 18;
static constuint8_t NAVIGATION_STATE_AUTO_FOLLOW_TARGET = 19;
static constuint8_t NAVIGATION_STATE_MAX = 20;
static constuint8_t VEHICLE_MODE_FLAG_SAFETY_ARMED = 128;
static constuint8_t VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64;
static constuint8_t VEHICLE_MODE_FLAG_HIL_ENABLED = 32;
static constuint8_t VEHICLE_MODE_FLAG_STABILIZED_ENABLED = 16;
static constuint8_t VEHICLE_MODE_FLAG_GUIDED_ENABLED = 8;
static constuint8_t VEHICLE_MODE_FLAG_AUTO_ENABLED = 4;
static constuint8_t VEHICLE_MODE_FLAG_TEST_ENABLED = 2;
static constuint8_t VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1;
static constuint8_t VEHICLE_TYPE_GENERIC = 0;
static constuint8_t VEHICLE_TYPE_FIXED_WING = 1;
static const uint8_tVEHICLE_TYPE_QUADROTOR = 2;
static constuint8_t VEHICLE_TYPE_COAXIAL = 3;
static constuint8_t VEHICLE_TYPE_HELICOPTER = 4;
static constuint8_t VEHICLE_TYPE_ANTENNA_TRACKER = 5;
static constuint8_t VEHICLE_TYPE_GCS = 6;
static constuint8_t VEHICLE_TYPE_AIRSHIP = 7;
static constuint8_t VEHICLE_TYPE_FREE_BALLOON = 8;
static constuint8_t VEHICLE_TYPE_ROCKET = 9;
static constuint8_t VEHICLE_TYPE_GROUND_ROVER = 10;
static constuint8_t VEHICLE_TYPE_SURFACE_BOAT = 11;
static constuint8_t VEHICLE_TYPE_SUBMARINE = 12;
static constuint8_t VEHICLE_TYPE_HEXAROTOR = 13;
static constuint8_t VEHICLE_TYPE_OCTOROTOR = 14;
static constuint8_t VEHICLE_TYPE_TRICOPTER = 15;
static constuint8_t VEHICLE_TYPE_FLAPPING_WING = 16;
static constuint8_t VEHICLE_TYPE_KITE = 17;
static constuint8_t VEHICLE_TYPE_ONBOARD_CONTROLLER = 18;
static constuint8_t VEHICLE_TYPE_VTOL_DUOROTOR = 19;
static constuint8_t VEHICLE_TYPE_VTOL_QUADROTOR = 20;
static constuint8_t VEHICLE_TYPE_VTOL_HEXAROTOR = 21;
static constuint8_t VEHICLE_TYPE_VTOL_OCTOROTOR = 22;
static constuint8_t VEHICLE_TYPE_ENUM_END = 23;
static constuint8_t VEHICLE_VTOL_STATE_UNDEFINED = 0;
static constuint8_t VEHICLE_VTOL_STATE_TRANSITION_TO_FW = 1;
static constuint8_t VEHICLE_VTOL_STATE_TRANSITION_TO_MC = 2;
static constuint8_t VEHICLE_VTOL_STATE_MC = 3;
static constuint8_t VEHICLE_VTOL_STATE_FW = 4;
static constuint8_t VEHICLE_BATTERY_WARNING_NONE = 0;
static constuint8_t VEHICLE_BATTERY_WARNING_LOW = 1;
static const uint8_tVEHICLE_BATTERY_WARNING_CRITICAL = 2;
static constuint8_t RC_IN_MODE_DEFAULT = 0;
static constuint8_t RC_IN_MODE_OFF = 1;
static constuint8_t RC_IN_MODE_GENERATED = 2;
#endif
};
#ifdef __cplusplus
struct __EXPORT multirotor_motor_limits_s {
#else
struct multirotor_motor_limits_s {
#endif
uint8_tlower_limit;
uint8_tupper_limit;
uint8_t yaw;
uint8_treserved;
#ifdef __cplusplus
#endif
}; /* initialize parameters cache */
parameters_update();跳转到parameters_update();
int
MulticopterAttitudeControl::parameters_update()
{
float v;
float roll_tc,pitch_tc;
param_get(_params_handles.roll_tc,&roll_tc);
param_get(_params_handles.pitch_tc,&pitch_tc);
/* roll gains*/
param_get(_params_handles.roll_p,&v);
_params.att_p(0)= v * (ATTITUDE_TC_DEFAULT / roll_tc);
param_get(_params_handles.roll_rate_p,&v);
_params.rate_p(0)= v * (ATTITUDE_TC_DEFAULT / roll_tc);
param_get(_params_handles.roll_rate_i,&v);
_params.rate_i(0)= v;
param_get(_params_handles.roll_rate_d,&v);
_params.rate_d(0)= v * (ATTITUDE_TC_DEFAULT / roll_tc);
param_get(_params_handles.roll_rate_ff,&v);
_params.rate_ff(0)= v;
/* pitch gains*/
param_get(_params_handles.pitch_p,&v);
_params.att_p(1)= v * (ATTITUDE_TC_DEFAULT / pitch_tc);
param_get(_params_handles.pitch_rate_p,&v);
_params.rate_p(1)= v * (ATTITUDE_TC_DEFAULT / pitch_tc);
param_get(_params_handles.pitch_rate_i,&v);
_params.rate_i(1)= v;
param_get(_params_handles.pitch_rate_d,&v);
_params.rate_d(1)= v * (ATTITUDE_TC_DEFAULT / pitch_tc);
param_get(_params_handles.pitch_rate_ff,&v);
_params.rate_ff(1)= v;
/* yaw gains */
param_get(_params_handles.yaw_p,&v);
_params.att_p(2)= v;
param_get(_params_handles.yaw_rate_p,&v);
_params.rate_p(2)= v;
param_get(_params_handles.yaw_rate_i,&v);
_params.rate_i(2)= v;
param_get(_params_handles.yaw_rate_d,&v);
_params.rate_d(2)= v;
param_get(_params_handles.yaw_rate_ff,&v);
_params.rate_ff(2)= v;
param_get(_params_handles.yaw_ff,&_params.yaw_ff);
/* angular ratelimits */
param_get(_params_handles.roll_rate_max,&_params.roll_rate_max);
_params.mc_rate_max(0)= math::radians(_params.roll_rate_max);
param_get(_params_handles.pitch_rate_max,&_params.pitch_rate_max);
_params.mc_rate_max(1)= math::radians(_params.pitch_rate_max);
param_get(_params_handles.yaw_rate_max,&_params.yaw_rate_max);
_params.mc_rate_max(2)= math::radians(_params.yaw_rate_max);
/* auto angularrate limits */
param_get(_params_handles.roll_rate_max,&_params.roll_rate_max);
_params.auto_rate_max(0)= math::radians(_params.roll_rate_max);
param_get(_params_handles.pitch_rate_max,&_params.pitch_rate_max);
_params.auto_rate_max(1)= math::radians(_params.pitch_rate_max);
param_get(_params_handles.yaw_auto_max,&_params.yaw_auto_max);
_params.auto_rate_max(2)= math::radians(_params.yaw_auto_max);
/* manual ratecontrol scale and auto mode roll/pitch rate limits */
param_get(_params_handles.acro_roll_max,&v);
_params.acro_rate_max(0)= math::radians(v);
param_get(_params_handles.acro_pitch_max,&v);
_params.acro_rate_max(1)= math::radians(v);
param_get(_params_handles.acro_yaw_max,&v);
_params.acro_rate_max(2)= math::radians(v);
/* stickdeflection needed in rattitude mode to control rates not angles */
param_get(_params_handles.rattitude_thres,&_params.rattitude_thres);
param_get(_params_handles.vtol_type,&_params.vtol_type);
int tmp;
param_get(_params_handles.vtol_opt_recovery_enabled,&tmp);
_params.vtol_opt_recovery_enabled= (bool)tmp;
param_get(_params_handles.vtol_wv_yaw_rate_scale,&_params.vtol_wv_yaw_rate_scale);
_actuators_0_circuit_breaker_enabled= circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY);
return OK;
}跳转到
int
param_get(param_t param, void *val)
{
int result =-1;
param_lock();
const void *v =param_get_value_ptr(param);
if (val !=NULL) {
memcpy(val,v, param_size(param));
result= 0;
}
param_unlock();
return result;
}怎么获取数据的，又是一系列的函数，跟下去的话太多了，脑袋要晕了，先就理解为以某种方式初始化相关的参数，具体哪些参数，看参数的名字应该大概能知道
/* wakeup source: vehicle attitude */
px4_pollfd_struct_t fds[1];
fds[0].fd = _ctrl_state_sub;
fds[0].events = POLLIN;NuttX任务使能
while (!_task_should_exit) {循环开始
/* wait for up to 100ms fordata */
int pret =px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
/* timed out - periodic checkfor _task_should_exit */
if (pret == 0) {
continue;
}
/* this is undesirable butnot much we can do - might want to flag unhappy status */
if (pret < 0) {
warn("mc attctrl: poll error %d, %d", pret, errno);
/* sleep a bitbefore next try */
usleep(100000);
continue;
}
perf_begin(_loop_perf);首先是px4_poll()配置阻塞时间100ms（uORB模型的函数API）。然后是打开MAVLINK协议，记录数据。如果poll失败，直接使用关键词continue从头开始运行（注意while和continue的组合使用）。其中的usleep(10000)函数属于线程级睡眠函数，使当前线程挂起。原文解释为：
“Theusleep() function will cause the calling thread to besuspended from executionuntil either the number of real-time microsecondsspecified by the argument'usec' has elapsed or a signal is delivered to thecalling thread。”
上面最后一个perf_begin(_loop_perf)，是一个空函数，带perf开头的都是空函数，它的作用主要是“Emptyfunction calls forroscompatibility”。
/* run controller on attitudechanges */
if (fds[0].revents &POLLIN) { //判断姿态控制器的控制任务是否已经使能
// pollevent_t events; /* The inputevent flags */
// pollevent_t revents; /* Theoutput event flags */
static uint64_tlast_run = 0;
float dt =(hrt_absolute_time() - last_run) / 1000000.0f;
last_run =hrt_absolute_time();
/* guard against toosmall (< 2ms) and too large (> 20ms) dt's */
if (dt < 0.002f) {
dt =0.002f;
} else if (dt >0.02f) {
dt = 0.02f;
}
/* copy attitude andcontrol state topics */
orb_copy(ORB_ID(control_state),_ctrl_state_sub, &_ctrl_state);看看control_state是什么
struct log_CTS_s control_state;
struct log_CTS_s {
float vx_body;
float vy_body;
float vz_body;
float airspeed;
floatroll_rate;
floatpitch_rate;
float yaw_rate;
};也就是跟新飞行器的姿态
/* check for updatesin other topics */
parameter_update_poll();
vehicle_control_mode_poll();
arming_status_poll();
vehicle_manual_poll();
vehicle_status_poll();
vehicle_motor_limits_poll();跳转到parameter_update_poll();
void
MulticopterAttitudeControl::parameter_update_poll()
{
bool updated;
/* Check ifparameters have changed */
orb_check(_params_sub,&updated);
if (updated) {
structparameter_update_s param_update;
orb_copy(ORB_ID(parameter_update),_params_sub, ¶m_update);
parameters_update();
}
}记得刚开始订阅的信息吗？
/*
* do subscriptions
*/
_v_att_sp_sub =orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_v_rates_sp_sub= orb_subscribe(ORB_ID(vehicle_rates_setpoint));
_ctrl_state_sub= orb_subscribe(ORB_ID(control_state));
_v_control_mode_sub= orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_control_sp_sub= orb_subscribe(ORB_ID(manual_control_setpoint));
_armed_sub =orb_subscribe(ORB_ID(actuator_armed));
_vehicle_status_sub= orb_subscribe(ORB_ID(vehicle_status));
_motor_limits_sub= orb_subscribe(ORB_ID(multirotor_motor_limits));
之后的几个poll是跟新这些数据，具体什么数据，之前列出来了 /* Check if we arein rattitude mode and the pilot is above the threshold on pitch
* or roll (yaw can rotate 360 in normal attcontrol). If both are true don't
* even bother running the attitude controllers*/
if(_vehicle_status.main_state == vehicle_status_s::MAIN_STATE_RATTITUDE) {
if(fabsf(_manual_control_sp.y) > _params.rattitude_thres ||
fabsf(_manual_control_sp.x) >_params.rattitude_thres) {
_v_control_mode.flag_control_attitude_enabled= false;
}
}然后：飞行模式判断是否是MAIN_STATE_RATTITUD模式，该模式是一种新的飞行模式，只控制角速度，不控制角度，俗称半自稳模式（小舵量自稳大舵量手动），主要用在setpoint中，航点飞行。根据介绍，这个模式只有在pitch和roll都设置为Rattitude模式时才有意义，如果yaw也设置了该模式，那么就会自动被手动模式替代了。所以代码中只做了x、y阈值的检测。官方介绍：
RATTITUDE The pilot's inputs are passed as roll, pitch, andyaw rate commands to the autopilot if they are greater than the mode'sthreshold. If not the inputs are passed as roll and pitch angle commands and ayaw rate command. Throttle is passed directly to the output mixer.
if(_v_control_mode.flag_control_attitude_enabled) {确定飞行模式是不是_v_control_mode.flag_control_attitude_enabled(姿态控制)，这个就是最开始_v_control_mode_sub= orb_subscribe(ORB_ID(vehicle_control_mode));订阅，接下来vehicle_control_mode_poll();获取的
if(_ts_opt_recovery == nullptr) {
//the tailsitter recovery instance has notbeen created, thus, the vehicle
//is not a tailsitter, do normal attitude control
control_attitude(dt);整个控制部分最重要的就是control_attitude(dt);(姿态控制)和control_attitude_rates(dt);(速度控制)
跳转到control_attitude(dt);
该函数内部做了很多的处理，控制理论基本都是在这个里面体现的，所以需要深入研究理解它才可以进一步的研究后续的算法。它的内部会通过算法处理获得控制量（目标姿态Target），即_rates_sp，一个vector<3>变量，以便后续控制使用。
理论知识真不少，好难懂，先贴上summer的解释：
/**
* Attitude controller.
* Input:'vehicle_attitude_setpoint' topics (depending on mode)
* Output: '_rates_sp'vector, '_thrust_sp'
*/
void
MulticopterAttitudeControl::control_attitude(float dt)
{
vehicle_attitude_setpoint_poll();跳转到vehicle_attitude_setpoint_poll();
void
MulticopterAttitudeControl::vehicle_attitude_setpoint_poll()
{
/* check ifthere is a new setpoint */
boolupdated;
orb_check(_v_att_sp_sub,&updated);
if(updated) {
orb_copy(ORB_ID(vehicle_attitude_setpoint),_v_att_sp_sub, &_v_att_sp);
}
}
即获取下面的这些数据
#ifdef __cplusplus
struct __EXPORT vehicle_attitude_setpoint_s {
#else
struct vehicle_attitude_setpoint_s {
#endif
uint64_ttimestamp;
floatroll_body;
floatpitch_body;
floatyaw_body;
floatyaw_sp_move_rate;
floatR_body[9];
bool R_valid;
floatq_d[4];
boolq_d_valid;
floatq_e[4];
boolq_e_valid;
floatthrust;
boolroll_reset_integral;
boolpitch_reset_integral;
boolyaw_reset_integral;
boolfw_control_yaw;
booldisable_mc_yaw_control;
boolapply_flaps;
#ifdef __cplusplus
#endif
}; _thrust_sp =_v_att_sp.thrust;//把油门控制量赋值给控制变量
/* constructattitude setpoint rotation matrix */
math::Matrix<3,3> R_sp;
R_sp.set(_v_att_sp.R_body);
构建姿态旋转矩阵，应该类似于变量定义与初始化
/* get currentrotation matrix from control state quaternions */
math::Quaternionq_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);
math::Matrix<3,3> R = q_att.to_dcm();
通过math库构建四元数；获取DCM的函数原型
/* all inputdata is ready, run controller itself */
好难啊，什么鬼啊，这个注释还是看懂了，之前的都是输入数据，接下来是控制器的设计了
/* try to movethrust vector shortest way, because yaw response is slower than roll/pitch */
math::Vector<3>R_z(R(0, 2), R(1, 2), R(2, 2));
math::Vector<3>R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));
这个地方旋转应该按照ZYX来进行的
当前姿态的z轴和目标姿态的z轴的误差大小（即需要旋转的角度）并旋转到b系（即先对齐Z轴）
/* axis andsin(angle) of desired rotation */
math::Vector<3>e_R = R.transposed() * (R_z % R_sp_z);R_z%R_sp_z叉积，还记得这个么?在mahony算法中已经出现过一次了，就是求取误差的，本来应该z轴相互重合的，如果不是0就作为误差项。然后再左乘旋转矩阵旋转到b系。
跳转到transposed()
Matrix3 Matrix3::transposed(void) const
{
returnMatrix3(Vector3(a.x, b.x, c.x),
Vector3(a.y, b.y, c.y),
Vector3(a.z, b.z, c.z));
} /* calculateangle error */
float e_R_z_sin= e_R.length();
float e_R_z_cos= R_z * R_sp_z;计算姿态角度误差（姿态误差），一个数学知识背景：由公式a×b=︱a︱︱b︱sinθ，a•b=︱a︱︱b︱cosθ，这里的R_z和R_sp_z都是单位向量，模值为1，因此误差向量e_R（a×b叉积就是误差）的模就是sinθ，点积就是cosθ。
/* calculateweight for yaw control */
float yaw_w =R_sp(2, 2) * R_sp(2, 2);计算yaw的权重
第一行的这个权重纯粹是因为如果不转动roll-pitch的话那应该是1，而如果转动的话，那个权重会平方倍衰减
/* calculaterotation matrix after roll/pitch only rotation */
math::Matrix<3,3> R_rp;
if (e_R_z_sin> 0.0f) {
/*get axis-angle representation */
floate_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos);
math::Vector<3>e_R_z_axis = e_R / e_R_z_sin;
e_R =e_R_z_axis * e_R_z_angle;
/*cross product matrix for e_R_axis */
math::Matrix<3,3> e_R_cp;
e_R_cp.zero();
e_R_cp(0,1) = -e_R_z_axis(2);
e_R_cp(0,2) = e_R_z_axis(1);
e_R_cp(1,0) = e_R_z_axis(2);
e_R_cp(1,2) = -e_R_z_axis(0);
e_R_cp(2,0) = -e_R_z_axis(1);
e_R_cp(2,1) = e_R_z_axis(0);
/*rotation matrix for roll/pitch only rotation */
R_rp= R * (_I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));
} else {
/*zero roll/pitch rotation */
R_rp= R;
}因为多轴的yaw响应一般比roll/pitch慢了接近一倍，因此将两者解耦(需要理解解耦的目的)，先补偿roll-pitch的变化，计算R_rp
首先需要明确的就是上述处理过程中的DCM量都是通过欧拉角来表示的，这个主要就是考虑在控制时需要明确具体的欧拉角的大小，还有就是算法的解算过程是通过矩阵微分方程推导得到的（参考《惯性技术_邓正隆》_P148-P152以及《惯性导航_秦永元》_P342），并且在《惯性技术_邓正隆》_P154页介绍了姿态矩阵的实时解算方法。再判断两个z轴是否存在误差（e_R_z_sin> 0.0f），若存在误差则通过反正切求出该误差角度值（atan2f(e_R_z_sin,e_R_z_cos)）；然后归一化e_R_z_axis(e_R /e_R_z_sin该步计算主要就是利用e_R_z_sin=e_R.length()，往上看就是了，不会这么快就忘记了吧？！)。然后就是e_R =e_R_z_axis* e_R_z_angle了（主要就是为了误差向量用角度量表示）。
/* R_rp andR_sp has the same Z axis, calculate yaw error */
math::Vector<3>R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0));
math::Vector<3>R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));
e_R(2) =atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w;计算yaw的误差，该误差是roll_pitch获取的z轴和目标姿态的z轴的误差
该部分同样是根据向量的叉积和点积求出误差角度的正弦和余弦，再反正切求出角度，之前也有这部分介绍
if (e_R_z_cos< 0.0f) {上面介绍的是在小角度变化时，如果是大角度变化时（大于90°，可能性比较小，还是集中在上面的算法吧）使用如何方法处理。
/*for large thrust vector rotations use another rotation method:
* calculate angle and axis for R -> R_sprotation directly */
math::Quaternionq;
q.from_dcm(R.transposed()* R_sp);
math::Vector<3>e_R_d = q.imag();
e_R_d.normalize();
e_R_d*= 2.0f * atan2f(e_R_d.length(), q(0));
/*use fusion of Z axis based rotation and direct rotation */
floatdirect_w = e_R_z_cos * e_R_z_cos * yaw_w;
e_R =e_R * (1.0f - direct_w) + e_R_d * direct_w;
}主要就是由DCM获取四元数；然后把四元数的虚部取出赋值给e_R_d(e_R_d = q.imag());然后对其进行归一化处理；最后2行是先求出互补系数，再通过互补方式求取e_R。
/* calculateangular rates setpoint */
_rates_sp =_params.att_p.emult(e_R);跳转到emult(e_R);
Matrix emult(const Matrix &other) const
{
Matrix res;
constMatrix &self = *this;
for (size_ti = 0; i < M; i++) {
for(size_t j = 0; j < N; j++) {
res(i , j) = self(i, j)*other(i, j);
}
}
return res;
} 所以_rates_sp = _params.att_p.emult(e_R)这句话的意思就是用att_p的每一个元素和e_R中对应位置的每一个元素相乘，结果赋值给_rates_sp角速度变量
计算角速度变化的大小
/* limit rates*/
for (int i = 0;i < 3; i++) {
if(_v_control_mode.flag_control_velocity_enabled &&!_v_control_mode.flag_control_manual_enabled) {
_rates_sp(i)= math::constrain(_rates_sp(i), -_params.auto_rate_max(i),_params.auto_rate_max(i));
}else {
_rates_sp(i)= math::constrain(_rates_sp(i), -_params.mc_rate_max(i),_params.mc_rate_max(i));
}
}并对其进行约束
/* weather-vanemode, dampen yaw rate */
if(_v_att_sp.disable_mc_yaw_control == true &&_v_control_mode.flag_control_velocity_enabled &&!_v_control_mode.flag_control_manual_enabled) {
floatwv_yaw_rate_max = _params.auto_rate_max(2) * _params.vtol_wv_yaw_rate_scale;
_rates_sp(2)= math::constrain(_rates_sp(2), -wv_yaw_rate_max, wv_yaw_rate_max);
//prevent integrator winding up in weathervane mode
_rates_int(2)= 0.0f;
}
/* feed forwardyaw setpoint rate */
_rates_sp(2) +=_v_att_sp.yaw_sp_move_rate * yaw_w * _params.yaw_ff;因为yaw响应较慢，再加入一个前馈控制
/* weather-vanemode, scale down yaw rate */
if(_v_att_sp.disable_mc_yaw_control == true && _v_control_mode.flag_control_velocity_enabled&& !_v_control_mode.flag_control_manual_enabled) {
floatwv_yaw_rate_max = _params.auto_rate_max(2) * _params.vtol_wv_yaw_rate_scale;
_rates_sp(2)= math::constrain(_rates_sp(2), -wv_yaw_rate_max, wv_yaw_rate_max);
//prevent integrator winding up in weathervane mode
_rates_int(2)= 0.0f;
}
} } else {
vehicle_attitude_setpoint_poll();
_thrust_sp= _v_att_sp.thrust;
math::Quaternionq(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);
math::Quaternionq_sp(&_v_att_sp.q_d[0]);
_ts_opt_recovery->setAttGains(_params.att_p,_params.yaw_ff);
_ts_opt_recovery->calcOptimalRates(q,q_sp, _v_att_sp.yaw_sp_move_rate, _rates_sp);
/*limit rates */
for(int i = 0; i < 3; i++) {
_rates_sp(i)= math::constrain(_rates_sp(i), -_params.mc_rate_max(i),_params.mc_rate_max(i));
}
}
/* publishattitude rates setpoint */
_v_rates_sp.roll= _rates_sp(0);
_v_rates_sp.pitch= _rates_sp(1);
_v_rates_sp.yaw= _rates_sp(2);
_v_rates_sp.thrust= _thrust_sp;
_v_rates_sp.timestamp= hrt_absolute_time();
if(_v_rates_sp_pub != nullptr) {
orb_publish(_rates_sp_id,_v_rates_sp_pub, &_v_rates_sp);
} else if(_rates_sp_id) {
_v_rates_sp_pub= orb_advertise(_rates_sp_id, &_v_rates_sp);
}通过control_attitude(dt)经过一系列的算法处理过以后获取的目标内环角速度值，并通过uORB模型发布出去，包含roll-pitch-yaw、油门量和时间戳
现在最不明了的就是这个数据发布出去以后在哪订阅了该数据呢或者说给谁用呢？？？自己发布，自己订阅，生生不息息
细节说明：在task_main()的开头处就是订阅各种topics，其中就有一个_v_rates_sp_sub =orb_subscribe(ORB_ID(vehicle_rates_setpoint))订阅过程（735_linenumber），它就是在该算法执行到最后时发布的控制量数据“_v_rates_sp”（822），也就是按照前讲述的理论，自己订阅自己发布的topic，以实现循环控制。其中需要注意的就是发布时用的ID和订阅时用的不一致所迷惑了，其实它俩是一样的;因为在上述处理过程中是把ORB_ID(vehicle_rates_setpoint)赋值给_rates_sp_id它的（567），赋值过程在发布topic之前，即在vehicle_status_poll()函数内部（794）。
_v_rates_sp是控制量，那么这个控制量怎么给pwm呢？？
//}
} else {
到此是姿态控制使能条件判断的“非”部分
/* attitudecontroller disabled, poll rates setpoint topic */
if(_v_control_mode.flag_control_manual_enabled) {
判断是不是手动模式
/*manual rates control - ACRO mode */
_rates_sp= math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x,
_manual_control_sp.r).emult(_params.acro_rate_max);
_thrust_sp= math::min(_manual_control_sp.z, MANUAL_THROTTLE_MAX_MULTICOPTER);
/*publish attitude rates setpoint */
_v_rates_sp.roll= _rates_sp(0);
_v_rates_sp.pitch= _rates_sp(1);
_v_rates_sp.yaw= _rates_sp(2);
_v_rates_sp.thrust= _thrust_sp;
_v_rates_sp.timestamp= hrt_absolute_time();
if(_v_rates_sp_pub != nullptr) {
orb_publish(_rates_sp_id,_v_rates_sp_pub, &_v_rates_sp);
}else if (_rates_sp_id) {
_v_rates_sp_pub= orb_advertise(_rates_sp_id, &_v_rates_sp);
}
} else {
/*attitude controller disabled, poll rates setpoint topic */
vehicle_rates_setpoint_poll();
_rates_sp(0)= _v_rates_sp.roll;
_rates_sp(1)= _v_rates_sp.pitch;
_rates_sp(2)= _v_rates_sp.yaw;
_thrust_sp= _v_rates_sp.thrust;
}
}
至此是另外一种控制模式了，不再是姿态控制模式了，而是速度控制模式
由此可以看出姿态控制和速度控制是2种模式，并不是级联关系
if(_v_control_mode.flag_control_rates_enabled) {
control_attitude_rates(dt);
跳转到control_attitude_rates(dt)
Void MulticopterAttitudeControl::control_attitude_rates(floatdt)
{
/* resetintegral if disarmed */
if(!_armed.armed || !_vehicle_status.is_rotary_wing) {
_rates_int.zero();
}
/* current bodyangular rates */
math::Vector<3>rates;
rates(0) =_ctrl_state.roll_rate;
rates(1) =_ctrl_state.pitch_rate;
rates(2) =_ctrl_state.yaw_rate;
通过_ctrl_state数据结构把需要的有效数据赋值给rates
/* angularrates error */
math::Vector<3>rates_err = _rates_sp - rates;//目标姿态-当前姿态
_att_control =_params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) /dt + _rates_int + _params.rate_ff.emult(_rates_sp - _rates_sp_prev) / dt;
_rates_sp_prev= _rates_sp;
_rates_prev =rates;
/* updateintegral only if not saturated on low limit and if motor commands are notsaturated */
if (_thrust_sp> MIN_TAKEOFF_THRUST && !_motor_limits.lower_limit &&!_motor_limits.upper_limit ) {
for(int i = 0; i < 3; i++) {
if(fabsf(_att_control(i)) < _thrust_sp) {
floatrate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt;
if(PX4_ISFINITE(rate_i) && rate_i > -RATES_I_LIMIT && rate_i< RATES_I_LIMIT &&
_att_control(i) > -RATES_I_LIMIT&& _att_control(i) < RATES_I_LIMIT) {
_rates_int(i)= rate_i;
}
}
}
}
}
/* publishactuator controls */发布控制量
_actuators.control[0]= (PX4_ISFINITE(_att_control(0))) ? _att_control(0) : 0.0f;
_actuators.control[1]= (PX4_ISFINITE(_att_control(1))) ? _att_control(1) : 0.0f;
_actuators.control[2]= (PX4_ISFINITE(_att_control(2))) ? _att_control(2) : 0.0f;
_actuators.control[3]= (PX4_ISFINITE(_thrust_sp)) ? _thrust_sp : 0.0f;
_actuators.timestamp= hrt_absolute_time();
_actuators.timestamp_sample= _ctrl_state.timestamp;
_controller_status.roll_rate_integ= _rates_int(0);
_controller_status.pitch_rate_integ= _rates_int(1);
_controller_status.yaw_rate_integ= _rates_int(2);
_controller_status.timestamp= hrt_absolute_time();
if(!_actuators_0_circuit_breaker_enabled) {
if(_actuators_0_pub != nullptr) {
orb_publish(_actuators_id,_actuators_0_pub, &_actuators);
perf_end(_controller_latency_perf);
}else if (_actuators_id) {
_actuators_0_pub= orb_advertise(_actuators_id, &_actuators);
}
}
/* publishcontroller status */
if(_controller_status_pub != nullptr) {
orb_publish(ORB_ID(mc_att_ctrl_status),_controller_status_pub, &_controller_status);
} else {
_controller_status_pub= orb_advertise(ORB_ID(mc_att_ctrl_status), &_controller_status);
}
}
至此速度控制模式结束
}
perf_end(_loop_perf);
}
循环结束
_control_task = -1;
return;
}其实在mc_att_control里面就完全涵盖了姿态控制的内环和外环（即角速度控制、角度控制）。主要就是attitudecontrol和attituderate control两个部分，前者是控制角度后者是控制角速度并把控制量输入给mixer。在控制过程中是通过控制电机的速度以实现多旋翼的整体的rpy的速度，通过这个速度随时间的累加实现角度控制。
attitude_control 输入是体轴矩阵R和期望的体轴矩阵Rsp，角度环只是一个P控制，算出来之后输出的是期望的角速度值rate_sp（这一段已经完成了所需要的角度变化，并将角度的变化值转换到了需要的角速度值）。并且把加速度值直接输出给attituderate control，再经过角速度环的pid控制，输出值直接就给mixer，然后控制电机输出了。
关于这些，主要还是需要理解这个控制过程：一方面是通过姿态解算部分获取的实时的姿态信息，并通过uORB模型把姿态信息发布出去；姿态控制部分订阅姿态解算得到的姿态信息。然后通过attitudecontrol获取目标姿态和当前姿态的角度差值并经过算法处理得到对应的角速度值，并把这个角速度值输出给attituderate control 最终获取到需求的控制量。输出给mixer。但是关于上述还是有一个迷惑的地方，就是在attitudecontrol这个里面输出的是根据目标姿态计算的角速度值，然后再和attituderate control 里面通过uORB获取的当前的角速度值做差得出角速度差值。。。。本身对这个比较懵逼。其实attitudecontrol输出是需要达到这个误差角度时所需要的角速度值，用这个值与当前的角速度值做差，求出现在需要的角速度值而已。这个就是为什么控制角速度的原因，进而达到控制角度的效果。
以上三段来自summer，其实现在从程序中并没有看出是这个逻辑来控制的。
自己思考：
代码太长、算法复杂，但是根据经验应该可以理出一条思路：
根据pixhawk姿态算法可以得到当前准确的欧拉角和角速度，即飞行器当前的姿态信息
根据控制期望可以通过一系列算法得到想要的角速度和角度
通过控制器的设计即姿态控制或者速度控制，一定可以得带控制量
将此控制量经过一系列变换最终以pwm形式电机
接下来要干的就是了解通过什么方式得到当前准确的姿态信息，通过什么算法得到期望姿态信息，通过什么算法得到控制量，经过什么样的变化得到最终pwm
好多都没看懂，先记到这里吧。
如果您觉得此文对您的发展有用，请随意打赏。
您的鼓励将是笔者书写高质量文章的最大动力^_^!!
