/*
三个轴向主要的稳定功能。
*/
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();
// 如果在2秒内没有运行速率控制器，那么重置积分器和重置方向控制。
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) {
// 脚本程序在过去的50毫秒内没有调用set_target_throttle_rate_rpy函数。
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();
// 是否在过去的3S内接收到外部油门操纵指令？
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;
bool using_rate_controller = false;
// 是否在过去的3S内接收到外部偏航操纵指令？
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;
// 在调整偏航速率控制器时，可以增加协调转弯的偏航速率，以使飞行更加简单。
const float coordination_yaw_rate = degrees(GRAVITY_MSS * tanf(radians(nav_roll_cd*0.01f))/MAX(aparm.airspeed_min,smoothed_airspeed));
commanded_rudder = yawController.get_rate_out(yaw_rate+coordination_yaw_rate, speed_scaler, false);
using_rate_controller = true;
} 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);
if (!using_rate_controller) {
/*
如果没有运行偏航速率控制器，我们需要重置速率。
*/
yawController.reset_rate_PID();
}
}
/*
计算地面转向的偏航控制，
需要根据特定的航向计算。
*/
void Plane::calc_nav_yaw_course(void)
{
// 在地面上保持特定的导航航向，用于自动起飞和着陆。
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);
}
/*
计算地面转向的偏航控制
计算地面转向的偏航控制需要考虑无人机的转向能力以及目标转弯半径。以下是一个示例的计算方法：
1) 确定目标转弯半径：根据需要进行转弯的路段或转弯点的限制，确定所需的转弯半径。
2）根据目标转弯半径计算期望偏航速率：
根据无人机的动力学特性和转弯半径，计算出期望的偏航速率。
这可以通过简化的公式或者使用更复杂的无人机动力学模型来计算。
例如，对于一个给定的转弯半径R和无人机的轨迹宽度W，可以使用下面的公式计算出期望偏航速率：
期望偏航速率 = atan(V^2 / (R * g)) * (180 / π)，
其中，V是无人机的速度，g是重力加速度。
3）根据当前转弯半径计算实际偏航速率：根据无人机的当前转弯半径和无人机速度，
计算出实际的偏航速率。可以使用无人机的传感器数据或者通过运动估计算法来获取。
4）计算偏航控制指令：通过比较期望偏航速率和实际偏航速率的差异，
使用控制算法（如PID控制）计算出偏航控制指令。
这个控制指令可以是一个转向角度或者一个转向助力的量。
*/
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;
}
// 如果在地面转向时，连续一秒没有进行转向操作，则清除已锁定的航向。
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();
// 是否在过去的3S内接收到外部横滚操纵指令？
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();
// 是否在过去的3S内接收到外部横滚操纵指令？
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` 是以弧度表示的，取值范围在 -π 到 π 之间（默认为 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;
/*将误差加入到 AC_PID 中，可能的改进是使用 update_all 代替。
在传统的 PID 控制器中，一般会将误差（error）作为输入，并通过计算得到相应的控制输出。
在这种情况下，你可以将误差推送到 AC_PID 控制器中，然后调用其相应的更新函数来计算输出。
然而，如果你想要进行更多的改进，可以考虑使用 update_all 方法。
update_all 方法会将所有的 PID 控制器参数一起更新，而不仅仅是单个误差。
这样可以更灵活地进行控制器参数的调整和优化，并使得控制器的响应更加精确和稳定。
具体的改进方式取决于你的具体需求和应用场景。你可以考虑以下几点来决定是否使用 update_all 方法：
1）需要同时更新多个 PID 控制器的参数。
2）控制器的性能需要更高的精度和响应速度。
3）控制器的参数需要频繁地进行调整和优化。*/
g2.guidedHeading.update_error(error, delta);
// 将误差加入AC_PID中，可能的改进是改用更新所有参数
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();
}
/*
调整 `nav_pitch_cd` 以适应 `STAB_PITCH_DOWN_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` 并限制 `nav_roll_cd`，
以确保负载因子不会使得飞机速度降至不可持续的范围。
负载因子（load factor）是指飞机所承受的力，与飞机的速度和姿态有关。
为了确保飞机速度不会降至不可持续的范围，你可以通过限制 `nav_roll_cd` 来控制负载因子。
具体的计算方式和限制条件会根据具体的飞行控制系统和性能需求而有所不同。
需要考虑以下几点来计算新的 `aerodynamic_load_factor` 和限制 `nav_roll_cd`：
1. 可持续空速范围：确定飞机所能承受的最低速度阈值，确保飞机处于安全的飞行状态。
2. 飞机性能：考虑飞机的最大负载因子和最大承重能力，避免超过飞机的设计限制。
3. 控制策略：根据飞行控制系统的要求和稳定性需求，确定合适的负载因子和航向控制偏角。
综上所述，你需要根据具体情况计算新的 `aerodynamic_load_factor` 并限制 `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) {
/*由于空速低于最小空速，限制滚转（roll）角度为 25 度。
当飞机空速低于最小空速时，需要采取措施来防止飞机失速。
一种常用的方法是限制滚转角度，以减小升力和降低飞机的失速风险。
根据你提供的信息，建议将滚转角度限制为 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) {
// the demanded nav_roll would take us past the aerodymamic
// load limit. Limit our roll to a bank angle that will keep
// the load within what the airframe can handle. We always
// allow at least 25 degrees of roll however, to ensure the
// aircraft can be maneuvered with a bad airspeed estimate. At
// 25 degrees the load factor is 1.1 (10%)
//要求的导航滚转码将超过空气动力负载限制。限制我们的滚转角度，
//以保持负载在飞机可以承受的范围内。但我们总是允许至少25度的滚转，
// 以确保即使空速估计不准也能操纵飞机。在25度的滚转角度下，负载系数为1.1(10％)
/*如果要进行的导航滚转（nav_roll）会超过空气动力学负载极限，
则限制滚转角度以保持负载在飞机结构可以承受的范围内。
但为了确保飞机在空速估计有误的情况下仍能进行操纵，
始终允许至少 25 度的滚转。在 25 度时，负载因子为 1.1（即 10%）。
根据实际要求，在限制空气动力学负载的前提下，可以将最大滚转角度限制为能够使负载保持在飞机结构可接受范围内的值。
在这种情况下，你提到要允许至少 25 度的滚转以增加机动性。
然而，在具体计算这个限制时，还需要考虑更多因素，
例如飞机的设计特性、结构强度和飞行控制系统的要求。
建议咨询专业的飞机设计师或相关领域的专家来确定最适合飞机的滚转角度限制。*/
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);
}
}
扫一扫
972
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
