ardupilot/Ardupilot V4.3.4 源码解析：Plane.h(2)_calc_nav_roll

/*Outback Challenge失控保护支持

    Outback Challenge是一项国际性的无人机竞赛活动，旨在推动无人机技术和应用的发展。

    该挑战赛通常涉及在澳大利亚的乡村地区进行，参赛者需要利用无人机完成一系列实际任务，

    如搜索和救援、精确投递、地形探测等。这个挑战赛激发了无人机行业的创新，

    并促使参与者在复杂环境和条件下展示其无人机飞行和应用的能力。

    */

#if ADVANCED_FAILSAFE == ENABLED

    AP_AdvancedFailsafe_Plane afs;

#endif

    /*

      meta data to support counting the number of circles in a loiter

      计算盘旋圈数

    */

    struct {

        //先前的目标方位，用于更新sum_cd

        int32_t old_target_bearing_cd;

        //在环绕飞行中所需的总旋转量,用于盘旋转弯命令.

        int32_t total_cd;

       

        //到目前为止盘旋完成的总角度

        int32_t sum_cd;

        //盘旋的方向。顺时针为1，逆时针为-1

        int8_t direction;

        //当盘旋并涉及高度时，当至少达到一次时，此标志位为true

        bool reached_target_alt;

        // 检查那些可能由上升气流造成永不停降的环绕飞行场景。

        bool unable_to_acheive_target_alt;

        // 盘旋开始时间。以毫秒为单位。

        uint32_t start_time_ms;

 

        //盘旋循环圈开始时的高度。用于检测每圈的高度变化。

        //仅当sum_cd> 36000时有效

        int32_t start_lap_alt_cm;

        int32_t next_sum_lap_cd;

        // Loiter Time指令中应该在环绕飞行中停留的时间量，单位为毫秒。

        uint32_t time_max_ms;

    } loiter;

 

    //条件命令

    //条件命令中使用的值（例如延迟，改变高度等）

    //例如在延迟命令中，condition_start记录了延迟的起始时间

    int32_t condition_value;

 

    //用于检查条件命令状态的起始值

    //例如，在延迟命令中，condition_start记录了延迟的起始时间

    uint32_t condition_start;

    //条件命令中使用的值。例如，改变高度的速率

    int16_t condition_rate;

 

    //3D位置矢量

    //在AP_Common中定义的位置结构

    const struct Location &home = ahrs.get_home();

    //上一个航点的位置。用于跟踪和高度斜坡计算

    Location prev_WP_loc {};

    // 飞机的当前位置

    struct Location current_loc {};

   //当前航点的位置。用于高度斜坡，跟踪和盘旋计算

    Location next_WP_loc {};

    // Altitude control 高度控制

    struct {

        //海平面上方的目标高度，单位厘米。用于气压

        //高度导航

        int32_t amsl_cm;

        //上一个航点和当前航点之间的高度差，单位厘米。用于滑行坡度处理

        int32_t offset_cm;

#if AP_TERRAIN_AVAILABLE

       //是否采用跟随地形？

        bool terrain_following;

        //地形上方的目标高度（使用地形跟随时有效）

        int32_t terrain_alt_cm;

        //用于高度误差报告的补偿值

        float lookahead;

#endif

        //FBWB/CRUISE模式下高度控制的最后输入

        float last_elevator_input;

        //上次检查高度的时间

        uint32_t last_elev_check_us;

    } target_altitude {};

    float relative_altitude;

    //循环性能监控

    AP::PerfInfo perf_info;

    struct {

        uint32_t last_trim_check;

        uint32_t last_trim_save;

    } auto_trim;

    struct {

        bool done_climb;

    } rtl;

 

    // 上次解除装备时更新家位置的时间

    uint32_t last_home_update_ms;

    //相机/天线安装跟踪和稳定性

#if HAL_MOUNT_ENABLED

    AP_Mount camera_mount;

#endif

    //解锁/上锁管理类

    AP_Arming_Plane arming;

    AP_Param param_loader {var_info};

    static const AP_Scheduler::Task scheduler_tasks[];

    static const AP_Param::Info var_info[];

    //舵机解锁计时器

    uint32_t rudder_arm_timer;

#if HAL_QUADPLANE_ENABLED

    //四旋翼飞机的辅助

    QuadPlane quadplane{ahrs};

#endif

    //发射器调谐辅助

    AP_Tuning_Plane tuning;

    static const struct LogStructure log_structure[];

    //差动推力舵混合增益（0-1）

    float rudder_dt;

    //滑翔模式更改计时器

    uint32_t soaring_mode_timer_ms;

    //非自动模式禁用地形，由RC选项开关设置

    bool non_auto_terrain_disable;

    bool terrain_disabled();

#if AP_TERRAIN_AVAILABLE

    bool terrain_enabled_in_current_mode() const;

    bool terrain_enabled_in_mode(Mode::Number num) const;

    enum class terrain_bitmask {

        ALL             = 1U << 0,

        FLY_BY_WIRE_B   = 1U << 1,

        CRUISE          = 1U << 2,

        AUTO            = 1U << 3,

        RTL             = 1U << 4,

        AVOID_ADSB      = 1U << 5,

        GUIDED          = 1U << 6,

        LOITER          = 1U << 7,

        CIRCLE          = 1U << 8,

        QRTL            = 1U << 9,

        QLAND           = 1U << 10,

        QLOITER         = 1U << 11,

    };

    struct TerrainLookupTable{

       Mode::Number mode_num;

       terrain_bitmask bitmask;

    };

    static const TerrainLookupTable Terrain_lookup[];

#endif

    // Attitude.cpp

    void adjust_nav_pitch_throttle(void);

    void update_load_factor(void);

    void adjust_altitude_target();

    void setup_glide_slope(void);

    int32_t get_RTL_altitude_cm() const;

    float relative_ground_altitude(bool use_rangefinder_if_available);

    void set_target_altitude_current(void);

    void set_target_altitude_current_adjusted(void);

    void set_target_altitude_location(const Location &loc);

    int32_t relative_target_altitude_cm(void);

    void change_target_altitude(int32_t change_cm);

    void set_target_altitude_proportion(const Location &loc, float proportion);

    void constrain_target_altitude_location(const Location &loc1, const Location &loc2);

    int32_t calc_altitude_error_cm(void);

    void check_fbwb_minimum_altitude(void);

    void reset_offset_altitude(void);

    void set_offset_altitude_location(const Location &start_loc, const Location &destination_loc);

    bool above_location_current(const Location &loc);

    void setup_terrain_target_alt(Location &loc) const;

    int32_t adjusted_altitude_cm(void);

    int32_t adjusted_relative_altitude_cm(void);

    float mission_alt_offset(void);

    float height_above_target(void);

    float lookahead_adjustment(void);

    float rangefinder_correction(void);

    void rangefinder_height_update(void);

    void rangefinder_terrain_correction(float &height);

    void stabilize();

    void calc_throttle();

    void calc_nav_roll();

    void calc_nav_pitch();

    float calc_speed_scaler(void);

    float get_speed_scaler(void) const { return surface_speed_scaler; }

    bool stick_mixing_enabled(void);

    void stabilize_roll(float speed_scaler);

    float stabilize_roll_get_roll_out(float speed_scaler);

    void stabilize_pitch(float speed_scaler);

    float stabilize_pitch_get_pitch_out(float speed_scaler);

    void stabilize_stick_mixing_direct();

    void stabilize_stick_mixing_fbw();

    void stabilize_yaw(float speed_scaler);

    void stabilize_training(float speed_scaler);

    void stabilize_acro(float speed_scaler);

    void calc_nav_yaw_coordinated(float speed_scaler);

    void calc_nav_yaw_course(void);

    void calc_nav_yaw_ground(void);

    // Log.cpp

    uint32_t last_log_fast_ms;

    void Log_Write_FullRate(void);

    void Log_Write_Attitude(void);

    void Log_Write_Control_Tuning();

    void Log_Write_OFG_Guided();

    void Log_Write_Guided(void);

    void Log_Write_Nav_Tuning();

    void Log_Write_Status();

    void Log_Write_RC(void);

    void Log_Write_Vehicle_Startup_Messages();

    void Log_Write_AETR();

    void Log_Write_MavCmdI(const mavlink_command_int_t &packet);

    void log_init();

    // Parameters.cpp

    void load_parameters(void) override;

    // commands_logic.cpp

    void set_next_WP(const struct Location &loc);

    void do_RTL(int32_t alt);

    bool verify_takeoff();

    bool verify_loiter_unlim(const AP_Mission::Mission_Command &cmd);

    bool verify_loiter_time();

    bool verify_loiter_turns(const AP_Mission::Mission_Command &cmd);

    bool verify_loiter_to_alt(const AP_Mission::Mission_Command &cmd);

    bool verify_RTL();

    bool verify_continue_and_change_alt();

    bool verify_wait_delay();

    bool verify_within_distance();

    bool verify_altitude_wait(const AP_Mission::Mission_Command &cmd);

    void do_loiter_at_location();

    bool verify_loiter_heading(bool init);

    void exit_mission_callback();

    bool start_command(const AP_Mission::Mission_Command& cmd);

    bool verify_command(const AP_Mission::Mission_Command& cmd);

    void do_takeoff(const AP_Mission::Mission_Command& cmd);

    void do_nav_wp(const AP_Mission::Mission_Command& cmd);

    void do_land(const AP_Mission::Mission_Command& cmd);

#if HAL_QUADPLANE_ENABLED

    void do_landing_vtol_approach(const AP_Mission::Mission_Command& cmd);

#endif

    void loiter_set_direction_wp(const AP_Mission::Mission_Command& cmd);

    void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd);

    void do_loiter_turns(const AP_Mission::Mission_Command& cmd);

    void do_loiter_time(const AP_Mission::Mission_Command& cmd);

    void do_altitude_wait(const AP_Mission::Mission_Command& cmd);

    void do_continue_and_change_alt(const AP_Mission::Mission_Command& cmd);

    void do_loiter_to_alt(const AP_Mission::Mission_Command& cmd);

    void do_vtol_takeoff(const AP_Mission::Mission_Command& cmd);

    void do_vtol_land(const AP_Mission::Mission_Command& cmd);

    bool verify_nav_wp(const AP_Mission::Mission_Command& cmd);

#if HAL_QUADPLANE_ENABLED

    bool verify_landing_vtol_approach(const AP_Mission::Mission_Command& cmd);

#endif

    void do_wait_delay(const AP_Mission::Mission_Command& cmd);

    void do_within_distance(const AP_Mission::Mission_Command& cmd);

    bool do_change_speed(const AP_Mission::Mission_Command& cmd);

    void do_set_home(const AP_Mission::Mission_Command& cmd);

    bool start_command_callback(const AP_Mission::Mission_Command &cmd);

    bool verify_command_callback(const AP_Mission::Mission_Command& cmd);

    float get_wp_radius() const;

    void do_nav_delay(const AP_Mission::Mission_Command& cmd);

    bool verify_nav_delay(const AP_Mission::Mission_Command& cmd);

    // Delay the next navigation command延迟下一个导航命令

    struct {

        uint32_t time_max_ms;

        uint32_t time_start_ms;

    } nav_delay;

   

#if AP_SCRIPTING_ENABLED

    // nav scripting support

    void do_nav_script_time(const AP_Mission::Mission_Command& cmd);

    bool verify_nav_script_time(const AP_Mission::Mission_Command& cmd);

#endif

    // commands.cpp

    void set_guided_WP(const Location &loc);

    // update home position. Return true if update done

    bool update_home();

    // update current_loc

    void update_current_loc(void);

    // set home location and store it persistently:

    bool set_home_persistently(const Location &loc) WARN_IF_UNUSED;

    // quadplane.cpp

#if HAL_QUADPLANE_ENABLED

    bool verify_vtol_takeoff(const AP_Mission::Mission_Command &cmd);

    bool verify_vtol_land(const AP_Mission::Mission_Command &cmd);

#endif

    // control_modes.cpp

    void read_control_switch();

    uint8_t readSwitch(void) const;

    void reset_control_switch();

    void autotune_start(void);

    void autotune_restore(void);

    void autotune_enable(bool enable);

    bool fly_inverted(void);

    uint8_t get_mode() const override { return (uint8_t)control_mode->mode_number(); }

    Mode *mode_from_mode_num(const enum Mode::Number num);

    // events.cpp

    void failsafe_short_on_event(enum failsafe_state fstype, ModeReason reason);

    void failsafe_long_on_event(enum failsafe_state fstype, ModeReason reason);

    void failsafe_short_off_event(ModeReason reason);

    void failsafe_long_off_event(ModeReason reason);

    void handle_battery_failsafe(const char* type_str, const int8_t action);

    bool failsafe_in_landing_sequence() const;  // returns true if the vehicle is in landing sequence.  Intended only for use in failsafe code.

#if AP_FENCE_ENABLED

    // fence.cpp

    void fence_check();

    bool fence_stickmixing() const;

    bool in_fence_recovery() const;

#endif

    // ArduPlane.cpp

    void disarm_if_autoland_complete();

    void get_osd_roll_pitch_rad(float &roll, float &pitch) const override;

    float tecs_hgt_afe(void);

    void efi_update(void);

    void get_scheduler_tasks(const AP_Scheduler::Task *&tasks,

                             uint8_t &task_count,

                             uint32_t &log_bit) override;

    void ahrs_update();

    void update_speed_height(void);

    void update_GPS_50Hz(void);

    void update_GPS_10Hz(void);

    void update_compass(void);

    void update_alt(void);

#if ADVANCED_FAILSAFE == ENABLED

    void afs_fs_check(void);

#endif

    void one_second_loop(void);

    void three_hz_loop(void);

#if AP_AIRSPEED_AUTOCAL_ENABLE

    void airspeed_ratio_update(void);

#endif

    void compass_save(void);

    void update_logging10(void);

    void update_logging25(void);

    void update_control_mode(void);

    void update_fly_forward(void);

    void update_flight_stage();

    void set_flight_stage(AP_Vehicle::FixedWing::FlightStage fs);

    // navigation.cpp

    void loiter_angle_reset(void);

    void loiter_angle_update(void);

    void navigate();

    void calc_airspeed_errors();

    float mode_auto_target_airspeed_cm();

    void calc_gndspeed_undershoot();

    void update_loiter(uint16_t radius);

    void update_loiter_update_nav(uint16_t radius);

    void update_cruise();

    void update_fbwb_speed_height(void);

    void setup_turn_angle(void);

    bool reached_loiter_target(void);

    // radio.cpp

    void set_control_channels(void) override;

    void init_rc_in();

    void init_rc_out_main();

    void init_rc_out_aux();

    void rudder_arm_disarm_check();

    void read_radio();

    int16_t rudder_input(void);

    void control_failsafe();

    void trim_radio();

    bool rc_throttle_value_ok(void) const;

    bool rc_failsafe_active(void) const;

    // sensors.cpp

    void read_rangefinder(void);

    // system.cpp

    void init_ardupilot() override;

    void startup_ground(void);

    bool set_mode(Mode& new_mode, const ModeReason reason);

    bool set_mode(const uint8_t mode, const ModeReason reason) override;

    bool set_mode_by_number(const Mode::Number new_mode_number, const ModeReason reason);

    void check_long_failsafe();

    void check_short_failsafe();

    void startup_INS_ground(void);

    bool should_log(uint32_t mask);

    int8_t throttle_percentage(void);

    void notify_mode(const Mode& mode);

    // takeoff.cpp

    bool auto_takeoff_check(void);

    void takeoff_calc_roll(void);

    void takeoff_calc_pitch(void);

    int8_t takeoff_tail_hold(void);

    int16_t get_takeoff_pitch_min_cd(void);

    void landing_gear_update(void);

    // avoidance_adsb.cpp

    void avoidance_adsb_update(void);

    // servos.cpp

    void set_servos_idle(void);

    void set_servos();

    void set_servos_manual_passthrough(void);

    void set_servos_controlled(void);

    void set_servos_old_elevons(void);

    void set_servos_flaps(void);

    void set_landing_gear(void);

    void dspoiler_update(void);

    void airbrake_update(void);

    void landing_neutral_control_surface_servos(void);

    void servos_output(void);

    void servos_auto_trim(void);

    void servos_twin_engine_mix();

    void force_flare();

    void throttle_voltage_comp(int8_t &min_throttle, int8_t &max_throttle) const;

    void throttle_watt_limiter(int8_t &min_throttle, int8_t &max_throttle);

    void throttle_slew_limit(SRV_Channel::Aux_servo_function_t func);

    bool suppress_throttle(void);

    void update_throttle_hover();

    void channel_function_mixer(SRV_Channel::Aux_servo_function_t func1_in, SRV_Channel::Aux_servo_function_t func2_in,

                                SRV_Channel::Aux_servo_function_t func1_out, SRV_Channel::Aux_servo_function_t func2_out) const;

    void flaperon_update();

    // is_flying.cpp

    void update_is_flying_5Hz(void);

    void crash_detection_update(void);

    bool in_preLaunch_flight_stage(void);

    bool is_flying(void);

    // parachute.cpp

    void parachute_check();

#if PARACHUTE == ENABLED

    void do_parachute(const AP_Mission::Mission_Command& cmd);

    void parachute_release();

    bool parachute_manual_release();

#endif

    // soaring.cpp

#if HAL_SOARING_ENABLED

    void update_soaring();

#endif

    // RC_Channel.cpp

    bool emergency_landing;

    // vehicle specific waypoint info helpers

    bool get_wp_distance_m(float &distance) const override;

    bool get_wp_bearing_deg(float &bearing) const override;

    bool get_wp_crosstrack_error_m(float &xtrack_error) const override;

    // reverse_thrust.cpp

    bool reversed_throttle;

    bool have_reverse_throttle_rc_option;

    bool allow_reverse_thrust(void) const;

    bool have_reverse_thrust(void) const;

    float get_throttle_input(bool no_deadzone=false) const;

    float get_adjusted_throttle_input(bool no_deadzone=false) const;

#if AP_SCRIPTING_ENABLED

    // 支持NAV_SCRIPT_TIME任务命令

    bool nav_scripting_active(void) const;

    bool nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2) override;

    void nav_script_time_done(uint16_t id) override;

 

    // 命令油门百分比和滚转、俯仰、偏航目标速率。用于脚本控制器

    void set_target_throttle_rate_rpy(float throttle_pct, float roll_rate_dps, float pitch_rate_dps, float yaw_rate_dps) override;

    bool nav_scripting_enable(uint8_t mode) override;

#endif

    enum Failsafe_Action {

        Failsafe_Action_None      = 0,

        Failsafe_Action_RTL       = 1,

        Failsafe_Action_Land      = 2,

        Failsafe_Action_Terminate = 3,

#if HAL_QUADPLANE_ENABLED

        Failsafe_Action_QLand     = 4,

#endif

        Failsafe_Action_Parachute = 5,

#if HAL_QUADPLANE_ENABLED

        Failsafe_Action_Loiter_alt_QLand = 6,

#endif

    };

    // 优先级列表，优先级从高到低排序

    static constexpr int8_t _failsafe_priorities[] = {

                                                      Failsafe_Action_Terminate,

                                                      Failsafe_Action_Parachute,

#if HAL_QUADPLANE_ENABLED

                                                      Failsafe_Action_QLand,

#endif

                                                      Failsafe_Action_Land,

                                                      Failsafe_Action_RTL,

                                                      Failsafe_Action_None,

                                                      -1 // the priority list must end with a sentinel of -1

                                                     };

    static_assert(_failsafe_priorities[ARRAY_SIZE(_failsafe_priorities) - 1] == -1,

                  "_failsafe_priorities is missing the sentinel");

 

    //ekf_check.cpp中执行丢失导航的EKF检查

    // 这些是特定于VTOL操作的

    void ekf_check();

    bool ekf_over_threshold();

    void failsafe_ekf_event();

    void failsafe_ekf_off_event(void);

    enum class CrowMode {

        NORMAL,

        PROGRESSIVE,

        CROW_DISABLED,

    };

    enum class ThrFailsafe {

        Disabled    = 0,

        Enabled     = 1,

        EnabledNoFS = 2

    };

    CrowMode crow_mode = CrowMode::NORMAL;

    enum class FlareMode {

        FLARE_DISABLED = 0,

        ENABLED_NO_PITCH_TARGET,

        ENABLED_PITCH_TARGET

    };

    FlareMode flare_mode;

    bool throttle_at_zero(void) const;

    // 数据配置处理

    float roll_in_expo(bool use_dz) const;

    float pitch_in_expo(bool use_dz) const;

    float rudder_in_expo(bool use_dz) const;

    // 进入上一个模式的原因

    ModeReason previous_mode_reason = ModeReason::UNKNOWN;

   

    int32_t tecs_target_alt_cm;

public:

    void failsafe_check(void);

#if AP_SCRIPTING_ENABLED

    bool set_target_location(const Location& target_loc) override;

    bool get_target_location(Location& target_loc) override;

    bool update_target_location(const Location &old_loc, const Location &new_loc) override;

    bool set_velocity_match(const Vector2f &velocity) override;

#endif // AP_SCRIPTING_ENABLED

};

extern Plane plane;

using AP_HAL::millis;

using AP_HAL::micros;