ardupilot/arduplane/attitude.cpp 姿态控制解析

#include "Plane.h"

/*
计算速度控制通道的控制率缩放系数。
这适用于PID，以随速度改变PID的比例。
在高速时，速度改变范围小，而在低速时，速度改变范围大。
*/
float Plane::calc_speed_scaler(void)
{
float aspeed, speed_scaler;
if (ahrs.airspeed_estimate(aspeed)) {
if (aspeed > auto_state.highest_airspeed && hal.util->get_soft_armed()) {
auto_state.highest_airspeed = aspeed;
}
// 确保在完全配置的空速上进行缩放
const float airspeed_min = MAX(aparm.airspeed_min, MIN_AIRSPEED_MIN);
const float scale_min = MIN(0.5, g.scaling_speed / (2.0 * aparm.airspeed_max));
const float scale_max = MAX(2.0, g.scaling_speed / (0.7 * airspeed_min));
if (aspeed > 0.0001f) {
speed_scaler = g.scaling_speed / aspeed;
} else {
speed_scaler = scale_max;
}
speed_scaler = constrain_float(speed_scaler, scale_min, scale_max);

#if HAL_QUADPLANE_ENABLED
if (quadplane.in_vtol_mode() && hal.util->get_soft_armed()) {
// 当处于垂直起降模式时，限制地面低速移动以防止不稳定
float threshold = airspeed_min * 0.5;
if (aspeed < threshold) {
float new_scaler = linear_interpolate(0.001, g.scaling_speed / threshold, aspeed, 0, threshold);
speed_scaler = MIN(speed_scaler, new_scaler);

// 逐渐减小积分系数，以防止积分器在低速时出现过大的情况
rollController.decay_I();
pitchController.decay_I();
yawController.decay_I();
}
}
#endif
} else if (hal.util->get_soft_armed()) {
// 使用油门输出缩放速度面
float throttle_out = MAX(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle), 1);
speed_scaler = sqrtf(THROTTLE_CRUISE / throttle_out);
// 由于没有真实的速度信息，这里受到了更严格的限制
speed_scaler = constrain_float(speed_scaler, 0.6f, 1.67f);
} else {
// 无速度估计且没有速度测量装置时，使用单位缩放
speed_scaler = 1;
}
if (!plane.ahrs.airspeed_sensor_enabled() &&
(plane.g2.flight_options & FlightOptions::SURPRESS_TKOFF_SCALING) &&
(plane.flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF)) {
//在自动起飞的爬升阶段，由于空速估计不准确的问题，在没有使用空速传感器的情况下，缩放被抑制
return MIN(speed_scaler, 1.0f) ;
}
return speed_scaler;
}

/*
如果当前设置和模式应允许驾驶杆混合，则返回true
*/
bool Plane::stick_mixing_enabled(void)
{
if (!rc().has_valid_input()) {
// 在没有有效RC输入的情况下，禁止摇杆混合
return false;
}
#if AP_FENCE_ENABLED
const bool stickmixing = fence_stickmixing();
#else
const bool stickmixing = true;
#endif
#if HAL_QUADPLANE_ENABLED
if (control_mode == &mode_qrtl &&
quadplane.poscontrol.get_state() >= QuadPlane::QPOS_POSITION1) {
// 用户可能正在重新定位
return false;
}
if (quadplane.in_vtol_land_poscontrol()) {
// 用户可能正在重新定位
return false;
}
#endif
if (control_mode->does_auto_throttle() && plane.control_mode->does_auto_navigation()) {
// 处于自动模式。检查摇杆混合标志
if (g.stick_mixing != StickMixing::NONE &&
g.stick_mixing != StickMixing::VTOL_YAW &&
stickmixing) {
return true;
} else {
return false;
}
}

if (failsafe.rc_failsafe && g.fs_action_short == FS_ACTION_SHORT_FBWA) {
// 不在FBWA模式下进行摇杆混合
return false;
}

// 非自动模式。始终进行摇杆混合
return true;
}

这是主要的滚转稳定功能。
它需要前置设置的nav_roll计算滚转伺服输出，
以尝试将飞机稳定在给定的滚转。
*/
void Plane::stabilize_roll(float speed_scaler)
{
if (fly_inverted()) {

// 倒行情况。
// 在该飞行状态下，需要处理roll_sensor的极点和nav_roll的极点干扰问题，如果该问题处理不当，会给控制器解算带来混乱。
// 处理这一问题的最简单方法是确保两者从零开始朝同一方向前进。
nav_roll_cd += 18000;
if (ahrs.roll_sensor < 0) nav_roll_cd -= 36000;
}

const float roll_out = stabilize_roll_get_roll_out(speed_scaler);
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, roll_out);
}

float Plane::stabilize_roll_get_roll_out(float speed_scaler)
{
#if HAL_QUADPLANE_ENABLED
if (!quadplane.use_fw_attitude_controllers()) {
// 使用垂直起降控制律进行控制，以确保一致性
const auto &pid_info = quadplane.attitude_control->get_rate_roll_pid().get_pid_info();
const float roll_out = rollController.get_rate_out(degrees(pid_info.target), speed_scaler);
/*
当从固定翼控制转向垂直起降控制时，需要减小积分器，以防止两个控制器之间的相对积分器对消
*/
rollController.decay_I();
return roll_out;
}
#endif

bool disable_integrator = false;
if (control_mode == &mode_stabilize && channel_roll->get_control_in() != 0) {
disable_integrator = true;
}
return rollController.get_servo_out(nav_roll_cd - ahrs.roll_sensor, speed_scaler, disable_integrator,
ground_mode && !(plane.g2.flight_options & FlightOptions::DISABLE_GROUND_PID_SUPPRESSION));
}

/*
这是主要的俯仰稳定功能。
它使用先前设置的nav_pitch并计算servo_out值，
以尝试将飞机稳定在给定姿态。
*/
void Plane::stabilize_pitch(float speed_scaler)
{
int8_t force_elevator = takeoff_tail_hold();
if (force_elevator != 0) {

//起飞时压尾翼。只需将百分比转换为[-4500 4500]范围内的角度
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, 45*force_elevator);
return;
}

const float pitch_out = stabilize_pitch_get_pitch_out(speed_scaler);
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pitch_out);
}

float Plane::stabilize_pitch_get_pitch_out(float speed_scaler)
{
#if HAL_QUADPLANE_ENABLED
if (!quadplane.use_fw_attitude_controllers()) {
// 使用垂直起降控制律进行控制，以确保一致性
const auto &pid_info = quadplane.attitude_control->get_rate_pitch_pid().get_pid_info();
const int32_t pitch_out = pitchController.get_rate_out(degrees(pid_info.target), speed_scaler);
/*
* 当从固定翼控制转向垂直起降控制时，我们需要衰减积分器，以防止两个控制器之间的对消
*/
pitchController.decay_I();
return pitch_out;
}
#endif
//如果设置了LANDING_FLARE RCx_OPTION开关，并且在FW模式下，手动油门，油门空转，
//并且设置了飞行选项FORCE_FLARE_ATTITUDE，则将俯仰设置为LAND_pitch_CD

#if HAL_QUADPLANE_ENABLED
const bool quadplane_in_transition = quadplane.in_transition();
#else
const bool quadplane_in_transition = false;
#endif

int32_t demanded_pitch = nav_pitch_cd + g.pitch_trim_cd + SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) * g.kff_throttle_to_pitch;
bool disable_integrator = false;
if (control_mode == &mode_stabilize && channel_pitch->get_control_in() != 0) {
disable_integrator = true;
}
/* 下列情形下，强制着陆俯仰值：
-飞行高度突变
-零推力时的油门杆
-固定翼非自动油门模式
*/
if (!quadplane_in_transition &&
!control_mode->is_vtol_mode() &&
!control_mode->does_auto_throttle() &&
flare_mode == FlareMode::ENABLED_PITCH_TARGET &&
throttle_at_zero()) {
demanded_pitch = landing.get_pitch_cd();
}

return pitchController.get_servo_out(demanded_pitch - ahrs.pitch_sensor, speed_scaler, disable_integrator,
ground_mode && !(plane.g2.flight_options & FlightOptions::DISABLE_GROUND_PID_SUPPRESSION));
}

/*
这使得用户可以在稳定模式下控制飞机
*/
void Plane::stabilize_stick_mixing_direct()
{
if (!stick_mixing_enabled() ||
control_mode == &mode_acro ||
control_mode == &mode_fbwa ||
control_mode == &mode_autotune ||
control_mode == &mode_fbwb ||
control_mode == &mode_cruise ||
#if HAL_QUADPLANE_ENABLED
control_mode == &mode_qstabilize ||
control_mode == &mode_qhover ||
control_mode == &mode_qloiter ||
control_mode == &mode_qland ||
control_mode == &mode_qrtl ||
control_mode == &mode_qacro ||
#if QAUTOTUNE_ENABLED
control_mode == &mode_qautotune ||
#endif
#endif
control_mode == &mode_training) {
return;
}
float aileron = SRV_Channels::get_output_scaled(SRV_Channel::k_aileron);
aileron = channel_roll->stick_mixing(aileron);
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, aileron);

if ((control_mode == &mode_loiter) && (plane.g2.flight_options & FlightOptions::ENABLE_LOITER_ALT_CONTROL)) {
// 定高模式使用的是基于俯仰杆的高度控制，这里不要使用
return;
}

float elevator = SRV_Channels::get_output_scaled(SRV_Channel::k_elevator);
elevator = channel_pitch->stick_mixing(elevator);
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, elevator);
}

/*
这使得用户可以使用FBW控制方式在稳定模式下控制飞机
*/
void Plane::stabilize_stick_mixing_fbw()
{
if (!stick_mixing_enabled() ||
control_mode == &mode_acro ||
control_mode == &mode_fbwa ||
control_mode == &mode_autotune ||
control_mode == &mode_fbwb ||
control_mode == &mode_cruise ||
#if HAL_QUADPLANE_ENABLED
control_mode == &mode_qstabilize ||
control_mode == &mode_qhover ||
control_mode == &mode_qloiter ||
control_mode == &mode_qland ||
control_mode == &mode_qacro ||
#if QAUTOTUNE_ENABLED
control_mode == &mode_qautotune ||
#endif
#endif // HAL_QUADPLANE_ENABLED
control_mode == &mode_training) {
return;
}

//做FBW控制模式的摇杆操纵混合。我们不会线性地对待它。
//然而。对于高达最大值一半的输入，我们对nav_roll和nav_pitch使用线性加法。
//高于此值，它变为非线性，最终达到最大值的2倍，以确保用户可以使用棒混合将平面指向任何方向。
float roll_input = channel_roll->norm_input();
if (roll_input > 0.5f) {
roll_input = (3*roll_input - 1);
} else if (roll_input < -0.5f) {
roll_input = (3*roll_input + 1);
}
nav_roll_cd += roll_input * roll_limit_cd;
nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd);

if ((control_mode == &mode_loiter) && (plane.g2.flight_options & FlightOptions::ENABLE_LOITER_ALT_CONTROL)) {
// 定高模式使用的是基于俯仰杆的高度控制，这里不再使用
return;
}

float pitch_input = channel_pitch->norm_input();
if (pitch_input > 0.5f) {
pitch_input = (3*pitch_input - 1);
} else if (pitch_input < -0.5f) {
pitch_input = (3*pitch_input + 1);
}
if (fly_inverted()) {
pitch_input = -pitch_input;
}
if (pitch_input > 0) {
nav_pitch_cd += pitch_input * aparm.pitch_limit_max_cd;
} else {
nav_pitch_cd += -(pitch_input * pitch_limit_min_cd);
}
nav_pitch_cd = constrain_int32(nav_pitch_cd, pitch_limit_min_cd, aparm.pitch_limit_max_cd.get());
}

/*
稳定航向轴。有三种操作模式：
-用地面转向保持特定航向
-地面转向速率控制
-协调飞行偏航控制
*/
void Plane::stabilize_yaw(float speed_scaler)
{
if (landing.is_flaring()) {
// 突发情况启用地面转向
steering_control.ground_steering = true;
} else {
// 当没有输入控制并且我们处于 GROUND_STEER_ALT以下时，使用地面转向
steering_control.ground_steering = (channel_roll->get_control_in() == 0 &&
fabsf(relative_altitude) < g.ground_steer_alt);
if (!landing.is_ground_steering_allowed()) {
// 着陆进近时不使用地面转向
steering_control.ground_steering = false;
}
}

/*
首先计算steering_control。
地面航向控制为前轮或尾轮转向。
当执行拉平（当机翼保持水平时）或在FBWA模式下保持航向时（当我们低于GROUND_STEER_ALT时），我们对方向舵使用“航向保持”模式
*/
if (landing.is_flaring() ||
(steer_state.hold_course_cd != -1 && steering_control.ground_steering)) {
calc_nav_yaw_course();
} else if (steering_control.ground_steering) {
calc_nav_yaw_ground();
}

/*
计算 steering_control.方向控制采用方向舵
*/
calc_nav_yaw_coordinated(speed_scaler);
}

/*
训练模式的一种特殊稳定函数
*/
void Plane::stabilize_training(float speed_scaler)
{
const float rexpo = roll_in_expo(false);
const float pexpo = pitch_in_expo(false);
if (training_manual_roll) {
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, rexpo);
} else {
// 计算那些需要保持
stabilize_roll(speed_scaler);
if ((nav_roll_cd > 0 && rexpo < SRV_Channels::get_output_scaled(SRV_Channel::k_aileron)) ||
(nav_roll_cd < 0 && rexpo > SRV_Channels::get_output_scaled(SRV_Channel::k_aileron))) {
// 允许退出滚转
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, rexpo);
}
}

if (training_manual_pitch) {
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pexpo);
} else {
stabilize_pitch(speed_scaler);
if ((nav_pitch_cd > 0 && pexpo < SRV_Channels::get_output_scaled(SRV_Channel::k_elevator)) ||
(nav_pitch_cd < 0 && pexpo > SRV_Channels::get_output_scaled(SRV_Channel::k_elevator))) {
// 允许返回水平
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pexpo);
}
}

stabilize_yaw(speed_scaler);
}

/*
这是ACRO模式稳定功能。它可以在滚转和俯仰轴上实现速率稳定。
*/
void Plane::stabilize_acro(float speed_scaler)
{
const float rexpo = roll_in_expo(true);
const float pexpo = pitch_in_expo(true);
float roll_rate = (rexpo/SERVO_MAX) * g.acro_roll_rate;
float pitch_rate = (pexpo/SERVO_MAX) * g.acro_pitch_rate;

/*
检查俯仰中立点特殊的滚转保持
*/
if (g.acro_locking && is_zero(roll_rate)) {
/*
当没有滚动杆输入时，将进入“滚动锁定”模式，并保持杆释放时的滚动。
*/
if (!acro_state.locked_roll) {
acro_state.locked_roll = true;
acro_state.locked_roll_err = 0;
} else {
acro_state.locked_roll_err += ahrs.get_gyro().x * G_Dt;
}
int32_t roll_error_cd = -ToDeg(acro_state.locked_roll_err)*100;
nav_roll_cd = ahrs.roll_sensor + roll_error_cd;
// 尝试将积分角度误差减小到零。我们将“stabilze”设置为true，这将禁用滚转积分器
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, rollController.get_servo_out(roll_error_cd,
speed_scaler,
true, false));
} else {
/*
副翼操纵杆非零，使用纯速率控制，直到用户松开操纵杆
*/
acro_state.locked_roll = false;
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, rollController.get_rate_out(roll_rate, speed_scaler));
}

if (g.acro_locking && is_zero(pitch_rate)) {
/*
用户有零俯仰摇杆输入，在摇杆相对输入点锁定俯仰值。
*/
if (!acro_state.locked_pitch) {
acro_state.locked_pitch = true;
acro_state.locked_pitch_cd = ahrs.pitch_sensor;
}
//试着保持锁定的俯仰角。请注意，这里启用了俯仰积分器，这有助于倒飞
nav_pitch_cd = acro_state.locked_pitch_cd;
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pitchController.get_servo_out(nav_pitch_cd - ahrs.pitch_sensor,
speed_scaler,
false, false));
} else {
/*
具有非零俯仰输入，使用纯速率控制器。
*/
acro_state.locked_pitch = false;
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pitchController.get_rate_out(pitch_rate, speed_scaler));
}

steering_control.steering = rudder_input();

if (g.acro_yaw_rate > 0 && yawController.rate_control_enabled()) {
// 通过ACRO_yaw_rate缩放偏航率控制偏航率
const float rudd_expo = rudder_in_expo(true);
const float yaw_rate = (rudd_expo/SERVO_MAX) * g.acro_yaw_rate;
steering_control.steering = steering_control.rudder = yawController.get_rate_out(yaw_rate, speed_scaler, false);
} else if (plane.g2.flight_options & FlightOptions::ACRO_YAW_DAMPER) {
// 采用航向控制器
calc_nav_yaw_coordinated(speed_scaler);
} else {
/*
手动方向舵
*/
steering_control.rudder = steering_control.steering;
}
}

/*
3个轴向的主稳定功能
*/
void Plane::stabilize()
{
if (control_mode == &mode_manual) {
// 重置操纵控制
steer_state.locked_course = false;
steer_state.locked_course_err = 0;
return;
}
float speed_scaler = get_speed_scaler();

uint32_t now = AP_HAL::millis();
bool allow_stick_mixing = true;
#if HAL_QUADPLANE_ENABLED
if (quadplane.available()) {
quadplane.transition->set_FW_roll_pitch(nav_pitch_cd, nav_roll_cd, allow_stick_mixing);
}
#endif

if (now - last_stabilize_ms > 2000) {
// 如果2秒钟没有运行速率控制器，那么重置积分器
rollController.reset_I();
pitchController.reset_I();
yawController.reset_I();

// 重置操纵控制
steer_state.locked_course = false;
steer_state.locked_course_err = 0;
}
last_stabilize_ms = now;

if (control_mode == &mode_training) {
stabilize_training(speed_scaler);
#if AP_SCRIPTING_ENABLED
} else if ((control_mode == &mode_auto &&
mission.get_current_nav_cmd().id == MAV_CMD_NAV_SCRIPT_TIME) || (nav_scripting.enabled && nav_scripting.current_ms > 0)) {
// 脚本控制滚转和俯仰速度以及油门
const float aileron = rollController.get_rate_out(nav_scripting.roll_rate_dps, speed_scaler);
const float elevator = pitchController.get_rate_out(nav_scripting.pitch_rate_dps, speed_scaler);
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, aileron);
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, elevator);
if (yawController.rate_control_enabled()) {
const float rudder = yawController.get_rate_out(nav_scripting.yaw_rate_dps, speed_scaler, false);
steering_control.rudder = rudder;
}
if (AP_HAL::millis() - nav_scripting.current_ms > 50) { //set_target_throttle_rate_rpy has not been called from script in last 50ms
nav_scripting.current_ms = 0;
}
#endif
} else if (control_mode == &mode_acro) {
stabilize_acro(speed_scaler);
#if HAL_QUADPLANE_ENABLED
} else if (control_mode->is_vtol_mode() && !quadplane.tailsitter.in_vtol_transition(now)) {
//为该模式运行特定的控制器
plane.control_mode->run();

// 用舵面进行增稳
if (plane.control_mode->mode_number() == Mode::Number::QACRO) {
plane.stabilize_acro(speed_scaler);
} else {
plane.stabilize_roll(speed_scaler);
plane.stabilize_pitch(speed_scaler);
}
#endif
} else {
if (allow_stick_mixing && g.stick_mixing == StickMixing::FBW && control_mode != &mode_stabilize) {
stabilize_stick_mixing_fbw();
}
stabilize_roll(speed_scaler);
stabilize_pitch(speed_scaler);
if (allow_stick_mixing && (g.stick_mixing == StickMixing::DIRECT || control_mode == &mode_stabilize)) {
stabilize_stick_mixing_direct();
}
stabilize_yaw(speed_scaler);
}

/*
判断是否应该将姿态控制器积分器归零。
*/
if (is_zero(get_throttle_input()) &&
fabsf(relative_altitude) < 5.0f &&
fabsf(barometer.get_climb_rate()) < 0.5f &&
ahrs.groundspeed() < 3) {
// 当高度很低，没有爬升率，没有油门，地面速度很低时，
// 将姿态控制器积分器归零。
// 这可以防止积分器在起飞前堆积。
rollController.reset_I();
pitchController.reset_I();
yawController.reset_I();

// 如果移动非常慢，操纵摇杆积分器也归零
if (ahrs.groundspeed() < 1) {
steerController.reset_I();
}
}
}

void Plane::calc_throttle()
{
if (aparm.throttle_cruise <= 1) {
// 用户操纵零油门
//这可能是任务完成时想要关闭发动机进行降落伞着陆的过程。
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0);
return;
}

float commanded_throttle = TECS_controller.get_throttle_demand();

// 是否在最后3秒内收到了引导油门的外部消息
if (control_mode->is_guided_mode() &&
plane.guided_state.last_forced_throttle_ms > 0 &&
millis() - plane.guided_state.last_forced_throttle_ms < 3000) {
commanded_throttle = plane.guided_state.forced_throttle;
}

SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, commanded_throttle);
}

/*****************************************
*计算期望的滚转/俯仰/偏航角
*****************************************/

/*
协调飞行时计算偏航控制输出
*/
void Plane::calc_nav_yaw_coordinated(float speed_scaler)
{
bool disable_integrator = false;
int16_t rudder_in = rudder_input();

int16_t commanded_rudder;

// 在最后3秒内是否收到引导航向的外部消息
if (control_mode->is_guided_mode() &&
plane.guided_state.last_forced_rpy_ms.z > 0 &&
millis() - plane.guided_state.last_forced_rpy_ms.z < 3000) {
commanded_rudder = plane.guided_state.forced_rpy_cd.z;
} else if (control_mode == &mode_autotune && g.acro_yaw_rate > 0 && yawController.rate_control_enabled()) {
// 正在使用航向率控制进行自动协调控制
const float rudd_expo = rudder_in_expo(true);
const float yaw_rate = (rudd_expo/SERVO_MAX) * g.acro_yaw_rate;
commanded_rudder = yawController.get_rate_out(yaw_rate, speed_scaler, false);
} else {
if (control_mode == &mode_stabilize && rudder_in != 0) {
disable_integrator = true;
}

commanded_rudder = yawController.get_servo_out(speed_scaler, disable_integrator);

// 添加方向舵控制到滚转通道
commanded_rudder += SRV_Channels::get_output_scaled(SRV_Channel::k_aileron) * g.kff_rudder_mix;
commanded_rudder += rudder_in;
}

steering_control.rudder = constrain_int16(commanded_rudder, -4500, 4500);
}

/*
calculate yaw control for ground steering with specific course
*/
void Plane::calc_nav_yaw_course(void)
{
// holding a specific navigation course on the ground. Used in
// auto-takeoff and landing
int32_t bearing_error_cd = nav_controller->bearing_error_cd();
steering_control.steering = steerController.get_steering_out_angle_error(bearing_error_cd);
if (stick_mixing_enabled()) {
steering_control.steering = channel_rudder->stick_mixing(steering_control.steering);
}
steering_control.steering = constrain_int16(steering_control.steering, -4500, 4500);
}

/*
计算地面转向的航向控制
*/
void Plane::calc_nav_yaw_ground(void)
{
if (gps.ground_speed() < 1 &&
is_zero(get_throttle_input()) &&
flight_stage != AP_Vehicle::FixedWing::FLIGHT_TAKEOFF &&
flight_stage != AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) {
// 静止时用手动进行方向舵控制
steer_state.locked_course = false;
steer_state.locked_course_err = 0;
steering_control.steering = rudder_input();
return;
}

// 如果持续1秒钟没有转向，那么清除锁定的航向通道
const uint32_t now_ms = AP_HAL::millis();
if (now_ms - steer_state.last_steer_ms > 1000) {
steer_state.locked_course = false;
}
steer_state.last_steer_ms = now_ms;

float steer_rate = (rudder_input()/4500.0f) * g.ground_steer_dps;
if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF ||
flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) {
steer_rate = 0;
}
if (!is_zero(steer_rate)) {
// 操纵员正在给方向舵输入
steer_state.locked_course = false;
} else if (!steer_state.locked_course) {
// 飞行员已松开舵杆，否则仍锁定航向
steer_state.locked_course = true;
if (flight_stage != AP_Vehicle::FixedWing::FLIGHT_TAKEOFF &&
flight_stage != AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) {
steer_state.locked_course_err = 0;
}
}

if (!steer_state.locked_course) {
// 以操纵员指定的速率使用速率控制器
steering_control.steering = steerController.get_steering_out_rate(steer_rate);
} else {
// 在求和误差上使用误差控制器
int32_t yaw_error_cd = -ToDeg(steer_state.locked_course_err)*100;
steering_control.steering = steerController.get_steering_out_angle_error(yaw_error_cd);
}
steering_control.steering = constrain_int16(steering_control.steering, -4500, 4500);
}

/*
从速度高度控制器计算新的nav_pitch_cd
*/
void Plane::calc_nav_pitch()
{
// 计算无人机的俯仰角
// --------------------------------
int32_t commanded_pitch = TECS_controller.get_pitch_demand();

// 最后3秒内是否收到外部的滚转引导消息
if (control_mode->is_guided_mode() &&
plane.guided_state.last_forced_rpy_ms.y > 0 &&
millis() - plane.guided_state.last_forced_rpy_ms.y < 3000) {
commanded_pitch = plane.guided_state.forced_rpy_cd.y;
}

nav_pitch_cd = constrain_int32(commanded_pitch, pitch_limit_min_cd, aparm.pitch_limit_max_cd.get());
}

/*
从导航控制器计算新的nav_roll_cd值
*/
void Plane::calc_nav_roll()
{
int32_t commanded_roll = nav_controller->nav_roll_cd();

// 最后3秒内是否收到外部的滚转引导消息
if (control_mode->is_guided_mode() &&
plane.guided_state.last_forced_rpy_ms.x > 0 &&
millis() - plane.guided_state.last_forced_rpy_ms.x < 3000) {
commanded_roll = plane.guided_state.forced_rpy_cd.x;
#if OFFBOARD_GUIDED == ENABLED
// guided_state.target_heading 是介于-pi和pi之间的弧度（默认值为-4）
} else if ((control_mode == &mode_guided) && (guided_state.target_heading_type != GUIDED_HEADING_NONE) ) {
uint32_t tnow = AP_HAL::millis();
float delta = (tnow - guided_state.target_heading_time_ms) * 1e-3f;
guided_state.target_heading_time_ms = tnow;

float error = 0.0f;
if (guided_state.target_heading_type == GUIDED_HEADING_HEADING) {
error = wrap_PI(guided_state.target_heading - AP::ahrs().yaw);
} else {
Vector2f groundspeed = AP::ahrs().groundspeed_vector();
error = wrap_PI(guided_state.target_heading - atan2f(-groundspeed.y, -groundspeed.x) + M_PI);
}

float bank_limit = degrees(atanf(guided_state.target_heading_accel_limit/GRAVITY_MSS)) * 1e2f;

g2.guidedHeading.update_error(error); // 将错误推送到AC_PID中，可能的改进是改用update_all
g2.guidedHeading.set_dt(delta);

float i = g2.guidedHeading.get_i(); // 获取积分器
if (((is_negative(error) && !guided_state.target_heading_limit_low) || (is_positive(error) && !guided_state.target_heading_limit_high))) {
i = g2.guidedHeading.get_i();
}

float desired = g2.guidedHeading.get_p() + i + g2.guidedHeading.get_d();
guided_state.target_heading_limit_low = (desired <= -bank_limit);
guided_state.target_heading_limit_high = (desired >= bank_limit);
commanded_roll = constrain_float(desired, -bank_limit, bank_limit);
#endif // OFFBOARD_GUIDED == ENABLED
}

nav_roll_cd = constrain_int32(commanded_roll, -roll_limit_cd, roll_limit_cd);
update_load_factor();
}

/*
为STAB_pitch_DOWN_cd调整nav_pitch_cd值。
这是为了使在FBWA模式下保持空速更容易
因为飞机在低油门时会自动俯仰一点。
它使FBWA着陆更容易，而不会失速。
*/
void Plane::adjust_nav_pitch_throttle(void)
{
int8_t throttle = throttle_percentage();
if (throttle >= 0 && throttle < aparm.throttle_cruise && flight_stage != AP_Vehicle::FixedWing::FLIGHT_VTOL) {
float p = (aparm.throttle_cruise - throttle) / (float)aparm.throttle_cruise;
nav_pitch_cd -= g.stab_pitch_down * 100.0f * p;
}
}

/*
计算新的 aerodynamic_load_factor 和 limit nav_roll_cd
以确保无人机的空速不低于失速速度。
*/
void Plane::update_load_factor(void)
{
float demanded_roll = fabsf(nav_roll_cd*0.01f);
if (demanded_roll > 85) {
// 限制为85度以防止数值错误
demanded_roll = 85;
}
aerodynamic_load_factor = 1.0f / safe_sqrt(cosf(radians(demanded_roll)));

#if HAL_QUADPLANE_ENABLED
if (quadplane.available() && quadplane.transition->set_FW_roll_limit(roll_limit_cd)) {
nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd);
return;
}
#endif

if (!aparm.stall_prevention) {
// 熄火预防禁用
return;
}
if (fly_inverted()) {
// 倒飞时无滚转限制
return;
}
#if HAL_QUADPLANE_ENABLED
if (quadplane.tailsitter.active()) {
// 盘旋时时无限制
return;
}
#endif

float max_load_factor = smoothed_airspeed / MAX(aparm.airspeed_min, 1);
if (max_load_factor <= 1) {
// 空速低于最低空速，则将滚转限制为25度
nav_roll_cd = constrain_int32(nav_roll_cd, -2500, 2500);
roll_limit_cd = MIN(roll_limit_cd, 2500);
} else if (max_load_factor < aerodynamic_load_factor) {
// 要求nav_roll 将使我们超过空气动力学负荷极限
// 将滚转角限制在一个滚转范围内内，使载荷保持在机身能够承受的范围内。
// 然而，我们总是允许至少25度的侧倾，以确保飞机能够以糟糕的估计空速进行操纵。
// 25度时，负载系数为1.1（10%）
int32_t roll_limit = degrees(acosf(sq(1.0f / max_load_factor)))*100;
if (roll_limit < 2500) {
roll_limit = 2500;
}
nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit, roll_limit);
roll_limit_cd = MIN(roll_limit_cd, roll_limit);
}
}

ardupilot/arduplane/attitude.cpp 姿态控制解析相关推荐

ArduPilot — ArduPlane架构概述
前言最近有着手 Ardupilot 开源代码中 Arduplane 的相关项目,将官方网址关于 Arduplane 的架构介绍(Plane Architecture Overview - Dev ...
ArduPilot飞行前检查——PreArm解析
ArduPilot飞行前检查主要包括两个部分: 1. 初始化中遥控器输入检查: 2. 1Hz解锁前检查. 附: 显示地面站信息参考文章:Ardupilot Pre-Arm安全检查程序分析 1. 初 ...
c语言中.h文件和.cpp文件解析
理论上来说cpp文件与头文件里的内容,只要是C语言所支持的,无论写什么都可以的,比如你在头文件中写函数体实现,任何一个cpp文件包含此头文件就可以将这个函数编译成目标文件的一部分(编译是以cpp文件为 ...
SSD6中Exercise4 (substitute.cpp) 答案解析
今天终于把Exercise4搞定了,昨天大约优化了0.38秒,今天优化了0.52秒,跨越了一大步. 在我们未加任何修饰执行此代码时,其执行后所用时间如图(摘抄主要): Function Callee ...
飞控Pixhawk——APM代码学习——ardupilot/ArduCopter文件夹
1.afs_copter.cpp/afs_copter.h 高级失效保护 2.AP_Arming.cpp/AP_Arming.h 解锁 3.AP_Rally.cpp/AP_Rally.h 集结地 4. ...
浅谈APM系列-----update_flight_mode（ModeAltHold）
update_flight_mode(ModeAltHold) 这里只看ModeAltHold. 位置:X:\ardupilot\ArduCopter\mode.cpp // update_fligh ...
Ardupilot姿态控制详解（完结篇）
本文主要是对过去写的一些APM中姿态控制函数博文的汇总. 按照之前写的博文以及个人认为的学习顺序,给出APM姿态控制器的学习顺序如下: APM姿态旋转理论基础 Ardupilot姿态控制器 PID控制 ...
ArduPilot飞控AOCODARC-H7DUAL固件编译
ArduPilot飞控AOCODARC-H743DUAL固件编译 1. 编译目标(AOCODARC-H743DUAL) 2. 硬件支持包(APM-AOCODARC-H743DUAL) 3. 编译步骤 ...
ArduPilot 编译选项
备注: 可以自己添加 -O0 -g3 -ggdb3 飞控板类型:fmuv3 ArduPilot C语言编译选项 [ '/opt/gcc-arm-none-eabi-5_4-2016q3/bin/arm ...

ardupilot/arduplane/attitude.cpp 姿态控制解析

ardupilot/arduplane/attitude.cpp 姿态控制解析相关推荐

最新文章

热门文章