ardupilot/Ardupilot V4.3.4 源码解析：Ardupilot.cpp
/*
本程序是免费软件：你可以根据免费软件基金会发布的GNU通用公共许
可证的条款，即许可证的第3版，或（看你的选择）任何更高的版本，重新发布和/或修改它。
本程序的发布是希望它能有用，但没有任何保证；甚至没有明确的质保
或第三方保证。 更多细节请参见GNU通用公共许可证。
你应该已经收到了一份与本程序一起的GNU通用公共许可证的副本。
如果没有，请参阅
*/
#include "Plane.h"
#define SCHED_TASK(func, rate_hz, max_time_micros, priority) SCHED_TASK_CLASS(Plane, &plane, func, rate_hz, max_time_micros, priority)
#define FAST_TASK(func) FAST_TASK_CLASS(Plane, &plane, func)
/*
参数表 - 所有常规任务都应列在这里。
这个表中的所有条目按优先级排序。
这个表与嵌套于每个子程序中的表交错排列，以确定任务运行的顺序。
提供方便的方法SCHED_TASK和SCHED_TASK_CLASS来建立这个结构中的条目：
SCHED_TASK 函数结构和参数意义：
- 要调用静态函数的名称
- 调用该函数的速率（赫兹）。
- 该函数运行的预期时间（微秒）。
- 优先级（0到255，数字越小意味着优先级越高）。
SCHED_TASK_CLASS 函数结构和参数意义：
- 要调用的方法的类名
- 调用该方法的实例
- 要在该实例上调用的方法
- 调用该方法的速率（赫兹）。
- 该方法运行的预期时间（微秒）。
- 优先级（0到255，数字越小意味着优先级越高）
*/
const AP_Scheduler::Task Plane::scheduler_tasks[] = {
// FAST_TASK条目在每个循环上运行，即使这意味着该循环允许超过了其分配的时间
FAST_TASK(ahrs_update),
FAST_TASK(update_control_mode),
FAST_TASK(stabilize),
FAST_TASK(set_servos),
// 要调用静态函数的名称 调用该函数的速率(Hz) 函数运行的预期时间（微秒） 优先级
SCHED_TASK(read_radio, 50, 100, 6),
SCHED_TASK(check_short_failsafe, 50, 100, 9),
SCHED_TASK(update_speed_height, 50, 200, 12),
SCHED_TASK(update_throttle_hover, 100, 90, 24),
SCHED_TASK(read_control_switch, 7, 100, 27),
SCHED_TASK(update_GPS_50Hz, 50, 300, 30),
SCHED_TASK(update_GPS_10Hz, 10, 400, 33),
SCHED_TASK(navigate, 10, 150, 36),
SCHED_TASK(update_compass, 10, 200, 39),
SCHED_TASK(calc_airspeed_errors, 10, 100, 42),
SCHED_TASK(update_alt, 10, 200, 45),
SCHED_TASK(adjust_altitude_target, 10, 200, 48),
#if ADVANCED_FAILSAFE == ENABLED
SCHED_TASK(afs_fs_check, 10, 100, 51),
#endif
SCHED_TASK(ekf_check, 10, 75, 54),
SCHED_TASK_CLASS(GCS, (GCS*)&plane._gcs, update_receive, 300, 500, 57),
SCHED_TASK_CLASS(GCS, (GCS*)&plane._gcs, update_send, 300, 750, 60),
SCHED_TASK_CLASS(AP_ServoRelayEvents, &plane.ServoRelayEvents, update_events, 50, 150, 63),
SCHED_TASK_CLASS(AP_BattMonitor, &plane.battery, read, 10, 300, 66),
SCHED_TASK_CLASS(AP_Baro, &plane.barometer, accumulate, 50, 150, 69),
SCHED_TASK_CLASS(AP_Notify, &plane.notify, update, 50, 300, 72),
SCHED_TASK(read_rangefinder, 50, 100, 78),
#if AP_ICENGINE_ENABLED
SCHED_TASK_CLASS(AP_ICEngine, &plane.g2.ice_control, update, 10, 100, 81),
#endif
#if AP_OPTICALFLOW_ENABLED
SCHED_TASK_CLASS(AP_OpticalFlow, &plane.optflow, update, 50, 50, 87),
#endif
SCHED_TASK(one_second_loop, 1, 400, 90),
SCHED_TASK(three_hz_loop, 3, 75, 93),
SCHED_TASK(check_long_failsafe, 3, 400, 96),
SCHED_TASK_CLASS(AP_RPM, &plane.rpm_sensor, update, 10, 100, 99),
#if AP_AIRSPEED_AUTOCAL_ENABLE
SCHED_TASK(airspeed_ratio_update, 1, 100, 102),
#endif // AP_AIRSPEED_AUTOCAL_ENABLE
#if HAL_MOUNT_ENABLED
SCHED_TASK_CLASS(AP_Mount, &plane.camera_mount, update, 50, 100, 105),
#endif // HAL_MOUNT_ENABLED
#if CAMERA == ENABLED
SCHED_TASK_CLASS(AP_Camera, &plane.camera, update, 50, 100, 108),
#endif // CAMERA == ENABLED
SCHED_TASK_CLASS(AP_Scheduler, &plane.scheduler, update_logging, 0.2, 100, 111),
SCHED_TASK(compass_save, 0.1, 200, 114),
SCHED_TASK(Log_Write_FullRate, 400, 300, 117),
SCHED_TASK(update_logging10, 10, 300, 120),
SCHED_TASK(update_logging25, 25, 300, 123),
#if HAL_SOARING_ENABLED
SCHED_TASK(update_soaring, 50, 400, 126),
#endif
SCHED_TASK(parachute_check, 10, 200, 129),
#if AP_TERRAIN_AVAILABLE
SCHED_TASK_CLASS(AP_Terrain, &plane.terrain, update, 10, 200, 132),
#endif // AP_TERRAIN_AVAILABLE
SCHED_TASK(update_is_flying_5Hz, 5, 100, 135),
#if LOGGING_ENABLED == ENABLED
SCHED_TASK_CLASS(AP_Logger, &plane.logger, periodic_tasks, 50, 400, 138),
#endif
SCHED_TASK_CLASS(AP_InertialSensor, &plane.ins, periodic, 50, 50, 141),
#if HAL_ADSB_ENABLED
SCHED_TASK(avoidance_adsb_update, 10, 100, 144),
#endif
SCHED_TASK_CLASS(RC_Channels, (RC_Channels*)&plane.g2.rc_channels, read_aux_all, 10, 200, 147),
#if HAL_BUTTON_ENABLED
SCHED_TASK_CLASS(AP_Button, &plane.button, update, 5, 100, 150),
#endif
#if STATS_ENABLED == ENABLED
SCHED_TASK_CLASS(AP_Stats, &plane.g2.stats, update, 1, 100, 153),
#endif
#if GRIPPER_ENABLED == ENABLED
SCHED_TASK_CLASS(AP_Gripper, &plane.g2.gripper, update, 10, 75, 156),
#endif
#if LANDING_GEAR_ENABLED == ENABLED
SCHED_TASK(landing_gear_update, 5, 50, 159),
#endif
};
void Plane::get_scheduler_tasks(const AP_Scheduler::Task *&tasks,
uint8_t &task_count,
uint32_t &log_bit)
{
tasks = &scheduler_tasks[0];
task_count = ARRAY_SIZE(scheduler_tasks);
log_bit = MASK_LOG_PM;
}
#if HAL_QUADPLANE_ENABLED
constexpr int8_t Plane::_failsafe_priorities[7];
#else
constexpr int8_t Plane::_failsafe_priorities[6];
#endif
// 更新姿态传感器
void Plane::ahrs_update()
{
arming.update_soft_armed();
ahrs.update();
if (should_log(MASK_LOG_IMU)) {
AP::ins().Write_IMU();
}
// 根据设定的滚转角、俯仰角度，计算出按比例的滚转、俯仰限制
roll_limit_cd = aparm.roll_limit_cd;
pitch_limit_min_cd = aparm.pitch_limit_min_cd;
bool rotate_limits = true;
#if HAL_QUADPLANE_ENABLED
if (quadplane.tailsitter.active()) {
rotate_limits = false;
}
#endif
if (rotate_limits) {
roll_limit_cd *= ahrs.cos_pitch();
pitch_limit_min_cd *= fabsf(ahrs.cos_roll());
}
// 更新、汇总用于地面转向和自动起飞的陀螺仪数据。
// 通过DCM.c转换矩阵与陀螺仪矢量的点积给出了大地系的偏航率
steer_state.locked_course_err += ahrs.get_yaw_rate_earth() * G_Dt;
steer_state.locked_course_err = wrap_PI(steer_state.locked_course_err);
#if HAL_QUADPLANE_ENABLED
// 检查是否有来自EKF的重置偏航数据
quadplane.check_yaw_reset();
// 更新垂起无人机的惯性导航系统
quadplane.inertial_nav.update();
#endif
if (should_log(MASK_LOG_VIDEO_STABILISATION)) {
ahrs.write_video_stabilisation();
}
}
/*
以50Hz的频率更新速度/高度控制器
*/
void Plane::update_speed_height(void)
{
if (control_mode->does_auto_throttle()) {
// 以50Hz频率调用TECS更新
//请注意，无论油门是否被抑制，都将调用这个功能，因为需要在起50Hz飞检测中运行。
TECS_controller.update_50hz();
}
#if HAL_QUADPLANE_ENABLED
if (quadplane.in_vtol_mode() ||
quadplane.in_assisted_flight()) {
quadplane.update_throttle_mix();
}
#endif
}
/*
读取和更新罗盘数据
*/
void Plane::update_compass(void)
{
compass.read();
}
/*
以10Hz的频率进行日志记录
*/
void Plane::update_logging10(void)
{
bool log_faster = (should_log(MASK_LOG_ATTITUDE_FULLRATE) || should_log(MASK_LOG_ATTITUDE_FAST));
if (should_log(MASK_LOG_ATTITUDE_MED) && !log_faster) {
Log_Write_Attitude();
ahrs.Write_AOA_SSA();
} else if (log_faster) {
ahrs.Write_AOA_SSA();
}
}
/*
以25Hz的频率进行日志记录
*/
void Plane::update_logging25(void)
{
// MASK LOG ATTITUDE FULLRATE以400Hz记录，MASK LOG ATTITUDE FAST以25Hz记录，MASK LOG ATTIUDE MED以10Hz记录
// 以最快速率记录
bool log_faster = should_log(MASK_LOG_ATTITUDE_FULLRATE);
if (should_log(MASK_LOG_ATTITUDE_FAST) && !log_faster) {
Log_Write_Attitude();
}
if (should_log(MASK_LOG_CTUN)) {
Log_Write_Control_Tuning();
AP::ins().write_notch_log_messages();
#if HAL_GYROFFT_ENABLED
gyro_fft.write_log_messages();
#endif
}
if (should_log(MASK_LOG_NTUN)) {
Log_Write_Nav_Tuning();
Log_Write_Guided();
}
if (should_log(MASK_LOG_RC))
Log_Write_RC();
if (should_log(MASK_LOG_IMU))
AP::ins().Write_Vibration();
}
/*
检查AFS的故障安全检测
*/
#if ADVANCED_FAILSAFE == ENABLED
void Plane::afs_fs_check(void)
{
// perform AFS failsafe checks进行AFS故障安全检查
#if AP_FENCE_ENABLED
const bool fence_breached = fence.get_breaches() != 0;
#else
const bool fence_breached = false;
#endif
afs.check(fence_breached, failsafe.AFS_last_valid_rc_ms);
}
#endif
#if HAL_WITH_IO_MCU
#include
extern AP_IOMCU iomcu;
#endif
void Plane::one_second_loop()
{
// 发射机检测逻辑 自主开发的程序 用于飞控主系统的机械开关启动
bool debug = true;
if (plane.g2.launch_detector_pin != -1) { // 启用了发射检测功能
hal.gpio->pinMode(plane.g2.launch_detector_pin, HAL_GPIO_INPUT); // 发射机检测引脚设置为GPIO输入模式
hal.gpio->write(plane.g2.launch_detector_pin, 1); // 发射检测引脚添加内部上拉
if (hal.gpio->read(plane.g2.launch_detector_pin) == plane.g2.launch_detector_polarity) { // 处于已触发状态
if (AP_HAL::millis() - plane.launch_detector_pin_last_inactive_time_ms < ((uint32_t)plane.g2.launch_detector_delay_ms)) { // 虽然已经触发，但是没有达到延时时间
debug = false;
}
} else { // 处于未触发状态
plane.launch_detector_pin_last_inactive_time_ms = AP_HAL::millis();
debug = false;
}
}
if(debug)
gcs().send_text(MAV_SEVERITY_CRITICAL, "Activate!");
else
gcs().send_text(MAV_SEVERITY_CRITICAL, "Inactivate!");
// 使之有可能在运行时改变控制频道的顺序
set_control_channels();
#if HAL_WITH_IO_MCU
iomcu.setup_mixing(&rcmap, g.override_channel.get(), g.mixing_gain, g2.manual_rc_mask);
#endif
#if HAL_ADSB_ENABLED
adsb.set_stall_speed_cm(aparm.airspeed_min * 100); // convert m/s to cm/s将m/s转化为cm/s
adsb.set_max_speed(aparm.airspeed_max);
#endif
if (g2.flight_options & FlightOptions::ENABLE_DEFAULT_AIRSPEED) {
// 使用最小和最大空速的平均值作为默认空速值以消除空速测量误差
ahrs.writeDefaultAirSpeed((float)((aparm.airspeed_min + aparm.airspeed_max)/2),
(float)((aparm.airspeed_max - aparm.airspeed_min)/2));
}
// 同步MAVLink系统ID
mavlink_system.sysid = g.sysid_this_mav;
SRV_Channels::enable_aux_servos();
// 更新通知标志
AP_Notify::flags.pre_arm_check = arming.pre_arm_checks(false);
AP_Notify::flags.pre_arm_gps_check = true;
AP_Notify::flags.armed = arming.is_armed() || arming.arming_required() == AP_Arming::Required::NO;
#if AP_TERRAIN_AVAILABLE
if (should_log(MASK_LOG_GPS)) {
terrain.log_terrain_data();
}
#endif
// 如果未解锁且GPS位置发生变化时，则更新home点位置。
// 每5秒更新一次
if (!arming.is_armed() &&
gps.last_message_time_ms() - last_home_update_ms > 5000 &&
gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
last_home_update_ms = gps.last_message_time_ms();
update_home();
// 重置着陆高度的修正
landing.alt_offset = 0;
}
// 通过调用构造函数和调度器的方法，确保了启动时G_Dt的正确性。
if (!is_equal(1.0f/scheduler.get_loop_rate_hz(), scheduler.get_loop_period_s()) ||
!is_equal(G_Dt, scheduler.get_loop_period_s())) {
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
}
}
void Plane::three_hz_loop()
{
#if AP_FENCE_ENABLED
fence_check();
#endif
}
void Plane::compass_save()
{
if (AP::compass().available() &&
compass.get_learn_type() >= Compass::LEARN_INTERNAL &&
!hal.util->get_soft_armed()) {
/*
只有在上锁时才保存偏移量
*/
compass.save_offsets();
}
}
#if AP_AIRSPEED_AUTOCAL_ENABLE
/*
每秒更新一次空速校准率
*/
void Plane::airspeed_ratio_update(void)
{
if (!airspeed.enabled() ||
gps.status() < AP_GPS::GPS_OK_FIX_3D ||
gps.ground_speed() < 4) {
// 不移动时不进行校准
return;
}
if (airspeed.get_airspeed() < aparm.airspeed_min &&
gps.ground_speed() < (uint32_t)aparm.airspeed_min) {
// 当飞行速度低于最低空速时，不要进行校准。
// 我们同时检查空速和地面速度，避免空速太低的情况
return;
}
if (labs(ahrs.roll_sensor) > roll_limit_cd ||
ahrs.pitch_sensor > aparm.pitch_limit_max_cd ||
ahrs.pitch_sensor < pitch_limit_min_cd) {
// 当无人机姿态超出正常飞行允许范围时，不要校准。
return;
}
const Vector3f &vg = gps.velocity();
airspeed.update_calibration(vg, aparm.airspeed_max);
}
#endif //
/*
读取卫星定位数据并更新位置
*/
void Plane::update_GPS_50Hz(void)
{
gps.update();
update_current_loc();
}
/*
读取更新GPS位置 - 10Hz更新
*/
void Plane::update_GPS_10Hz(void)
{
static uint32_t last_gps_msg_ms;
if (gps.last_message_time_ms() != last_gps_msg_ms && gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
last_gps_msg_ms = gps.last_message_time_ms();
if (ground_start_count > 1) {
ground_start_count--;
} else if (ground_start_count == 1) {
// 我们倒数N个良好的GPS固定点，确保高度数据就更准确了
// -------------------------------------
if (current_loc.lat == 0 && current_loc.lng == 0) {
ground_start_count = 5;
} else if (!hal.util->was_watchdog_reset()) {
if (!set_home_persistently(gps.location())) {
}
next_WP_loc = prev_WP_loc = home;
ground_start_count = 0;
}
}
// 更新风向评估
ahrs.estimate_wind();
} else if (gps.status() < AP_GPS::GPS_OK_FIX_3D && ground_start_count != 0) {
// 若卫星数据丢失3D定位，则重新开始
ground_start_count = 5;
}
calc_gndspeed_undershoot();
}
/*
主控模式依赖的更新代码
*/
void Plane::update_control_mode(void)
{
if (control_mode != &mode_auto) {
// 起飞和降落时锁定航向
steer_state.hold_course_cd = -1;
}
update_fly_forward();
control_mode->update();
}
void Plane::update_fly_forward(void)
{
// 确保作为纯固定翼模式飞行时是向前飞的。
// 这有助于EKF得出更好的状态评估，因为它可以做出更强的假设
#if HAL_QUADPLANE_ENABLED
if (quadplane.available() &&
quadplane.tailsitter.is_in_fw_flight()) {
ahrs.set_fly_forward(true);
return;
}
if (quadplane.in_vtol_mode() ||
quadplane.in_assisted_flight()) {
ahrs.set_fly_forward(false);
return;
}
#endif
if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) {
ahrs.set_fly_forward(landing.is_flying_forward());
return;
}
ahrs.set_fly_forward(true);
}
/*
设置飞行阶段
*/
void Plane::set_flight_stage(AP_Vehicle::FixedWing::FlightStage fs)
{
if (fs == flight_stage) {
return;
}
landing.handle_flight_stage_change(fs == AP_Vehicle::FixedWing::FLIGHT_LAND);
if (fs == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) {
gcs().send_text(MAV_SEVERITY_NOTICE, "Landing aborted, climbing to %dm",
int(auto_state.takeoff_altitude_rel_cm/100));
}
flight_stage = fs;
Log_Write_Status();
}
void Plane::update_alt()
{
barometer.update();
#if HAL_QUADPLANE_ENABLED
if (quadplane.available()) {
quadplane.motors->set_air_density_ratio(barometer.get_air_density_ratio());
}
#endif
// 计算下沉率。
float sink_rate;
Vector3f vel;
if (ahrs.get_velocity_NED(vel)) {
sink_rate = vel.z;
} else if (gps.status() >= AP_GPS::GPS_OK_FIX_3D && gps.have_vertical_velocity()) {
sink_rate = gps.velocity().z;
} else {
sink_rate = -barometer.get_climb_rate();
}
// 通过低通滤波器消除下沉率中的噪音
auto_state.sink_rate = 0.8f * auto_state.sink_rate + 0.2f*sink_rate;
#if PARACHUTE == ENABLED
parachute.set_sink_rate(auto_state.sink_rate);
#endif
update_flight_stage();
if (control_mode->does_auto_throttle() && !throttle_suppressed) {
float distance_beyond_land_wp = 0;
if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND && current_loc.past_interval_finish_line(prev_WP_loc, next_WP_loc)) {
distance_beyond_land_wp = current_loc.get_distance(next_WP_loc);
}
tecs_target_alt_cm = relative_target_altitude_cm();
if (control_mode == &mode_rtl && !rtl.done_climb && (g2.rtl_climb_min > 0 || (plane.g2.flight_options & FlightOptions::CLIMB_BEFORE_TURN))) {
// 确保我们在RTL中进行初始爬升。我们在要求的高度上额外增加10米，以推动TECS快速爬升。
tecs_target_alt_cm = MAX(tecs_target_alt_cm, prev_WP_loc.alt - home.alt) + (g2.rtl_climb_min+10)*100;
}
TECS_controller.update_pitch_throttle(tecs_target_alt_cm,
target_airspeed_cm,
flight_stage,
distance_beyond_land_wp,
get_takeoff_pitch_min_cd(),
throttle_nudge,
tecs_hgt_afe(),
aerodynamic_load_factor);
}
}
/*
重新计算飞行阶段
*/
void Plane::update_flight_stage(void)
{
// 更新速度和高度控制器的状态
if (control_mode->does_auto_throttle() && !throttle_suppressed) {
if (control_mode == &mode_auto) {
#if HAL_QUADPLANE_ENABLED
if (quadplane.in_vtol_auto()) {
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_VTOL);
return;
}
#endif
if (auto_state.takeoff_complete == false) {
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_TAKEOFF);
return;
} else if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND) {
if (landing.is_commanded_go_around() || flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) {
// 终止模式是棘手的，它必须在执行NAV_LAND时完成时才被允许。
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND);
} else if (landing.get_abort_throttle_enable() && get_throttle_input() >= 90 &&
landing.request_go_around()) {
gcs().send_text(MAV_SEVERITY_INFO,"Landing aborted via throttle");
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND);
} else {
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_LAND);
}
return;
}
#if HAL_QUADPLANE_ENABLED
if (quadplane.in_assisted_flight()) {
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_VTOL);
return;
}
#endif
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_NORMAL);
} else if (control_mode != &mode_takeoff) {
// 如果不是在自动模式下，则假定在正常操作，以实现TECS的正常运行。
// 这可以防止TECS在着陆、中止着陆或起飞过程中，从自动模式切换到例如FBWB时，被卡在错误的阶段。
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_NORMAL);
}
return;
}
#if HAL_QUADPLANE_ENABLED
if (quadplane.in_vtol_mode() ||
quadplane.in_assisted_flight()) {
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_VTOL);
return;
}
#endif
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_NORMAL);
}
/*
如果着陆解除延迟被启用（非零），检查是否有着陆，然后在时间结束后自动解锁。
当着陆库准备解锁时,只从AP_Landing调用。
*/
void Plane::disarm_if_autoland_complete()
{
if (landing.get_disarm_delay() > 0 &&
!is_flying() &&
arming.arming_required() != AP_Arming::Required::NO &&
arming.is_armed()) {
/* 已经启用了自动解锁。看看是否已经过了足够的时间 */
if (millis() - auto_state.last_flying_ms >= landing.get_disarm_delay()*1000UL) {
if (arming.disarm(AP_Arming::Method::AUTOLANDED)) {
gcs().send_text(MAV_SEVERITY_INFO,"Auto disarmed");
}
}
}
}
/*
传递给TECS的现场海拔高度
*/
float Plane::tecs_hgt_afe(void)
{
/*
将场内海拔以上的高度传递为着陆时距离地面的高度。
这意味着TECS得到了测距仪的信息，从而可以知道突变在何时发生。
*/
float hgt_afe;
if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) {
hgt_afe = height_above_target();
hgt_afe -= rangefinder_correction();
} else {
// 在正常飞行时，我们将hgt_afe作为相对高度传递给家位置。
hgt_afe = relative_altitude;
}
return hgt_afe;
}
// 飞行器特定的航点信息帮助工具
bool Plane::get_wp_distance_m(float &distance) const
{
// 见 GCS_MAVLINK_Plane::send_nav_controller_output()
if (control_mode == &mode_manual) {
return false;
}
#if HAL_QUADPLANE_ENABLED
if (quadplane.in_vtol_mode()) {
distance = quadplane.using_wp_nav() ? quadplane.wp_nav->get_wp_distance_to_destination() * 0.01 : 0;
return true;
}
#endif
distance = auto_state.wp_distance;
return true;
}
bool Plane::get_wp_bearing_deg(float &bearing) const
{
// 见 GCS_MAVLINK_Plane::send_nav_controller_output()
if (control_mode == &mode_manual) {
return false;
}
#if HAL_QUADPLANE_ENABLED
if (quadplane.in_vtol_mode()) {
bearing = quadplane.using_wp_nav() ? quadplane.wp_nav->get_wp_bearing_to_destination() : 0;
return true;
}
#endif
bearing = nav_controller->target_bearing_cd() * 0.01;
return true;
}
bool Plane::get_wp_crosstrack_error_m(float &xtrack_error) const
{
// 见 GCS_MAVLINK_Plane::send_nav_controller_output()
if (control_mode == &mode_manual) {
return false;
}
#if HAL_QUADPLANE_ENABLED
if (quadplane.in_vtol_mode()) {
xtrack_error = quadplane.using_wp_nav() ? quadplane.wp_nav->crosstrack_error() : 0;
return true;
}
#endif
xtrack_error = nav_controller->crosstrack_error();
return true;
}
#if AP_SCRIPTING_ENABLED
// 设置目标位置（供脚本使用）
bool Plane::set_target_location(const Location &target_loc)
{
Location loc{target_loc};
if (plane.control_mode != &plane.mode_guided) {
// 只在引导模式下接受位置更新
return false;
}
// 如有需要，可添加首页的备选方案
if (loc.relative_alt) {
loc.alt += plane.home.alt;
loc.relative_alt = 0;
}
plane.set_guided_WP(loc);
return true;
}
// 设置目标位置（供脚本使用）
bool Plane::get_target_location(Location& target_loc)
{
switch (control_mode->mode_number()) {
case Mode::Number::RTL:
case Mode::Number::AVOID_ADSB:
case Mode::Number::GUIDED:
case Mode::Number::AUTO:
case Mode::Number::LOITER:
case Mode::Number::TAKEOFF:
#if HAL_QUADPLANE_ENABLED
case Mode::Number::QLOITER:
case Mode::Number::QLAND:
case Mode::Number::QRTL:
#endif
target_loc = next_WP_loc;
return true;
break;
default:
break;
}
return false;
}
/*
更新目标位置()在所有自动导航模式下都能工作
*/
bool Plane::update_target_location(const Location &old_loc, const Location &new_loc)
{
if (!old_loc.same_latlon_as(next_WP_loc)) {
return false;
}
ftype alt_diff;
if (!old_loc.get_alt_distance(next_WP_loc, alt_diff) ||
!is_zero(alt_diff)) {
return false;
}
next_WP_loc = new_loc;
next_WP_loc.change_alt_frame(old_loc.get_alt_frame());
return true;
}
// 允许在VTOL中进行速度匹配
bool Plane::set_velocity_match(const Vector2f &velocity)
{
#if HAL_QUADPLANE_ENABLED
if (quadplane.in_vtol_mode() || quadplane.in_vtol_land_sequence()) {
quadplane.poscontrol.velocity_match = velocity;
quadplane.poscontrol.last_velocity_match_ms = AP_HAL::millis();
return true;
}
#endif
return false;
}
#endif // AP_SCRIPTING_ENABLED
// 在非VTOL模式下为TRIM_PITCH_CD纠正AHRS俯仰，并在VTOL模式下返回VTOL视图
void Plane::get_osd_roll_pitch_rad(float &roll, float &pitch) const
{
#if HAL_QUADPLANE_ENABLED
if (quadplane.show_vtol_view()) {
pitch = quadplane.ahrs_view->pitch;
roll = quadplane.ahrs_view->roll;
return;
}
#endif
pitch = ahrs.pitch;
roll = ahrs.roll;
if (!(g2.flight_options & FlightOptions::OSD_REMOVE_TRIM_PITCH_CD)) { // correct for TRIM_PITCH_CD 更正TRIM_PITCH_CD
pitch -= g.pitch_trim_cd * 0.01 * DEG_TO_RAD;
}
}
/*
更新当前位置
*/
void Plane::update_current_loc(void)
{
have_position = plane.ahrs.get_location(plane.current_loc);
// 重新计算相对海拔
ahrs.get_relative_position_D_home(plane.relative_altitude);
relative_altitude *= -1.0f;
}
AP_HAL_MAIN_CALLBACKS(&plane);