apm-rover主程序分析

apm源码下载地址：下载

apm-rover主函数

    void fun() __attribute__ ((noinline));//fun函数不能作为inline函数优化

    // Radio setup:
    // APM INPUT (Rec = receiver)
    // Rec ch1: Steering
    // Rec ch2: not used
    // Rec ch3: Throttle
    // Rec ch4: not used
    // Rec ch5: not used
    // Rec ch6: not used
    // Rec ch7: Option channel to 2 position switch
    // Rec ch8: Mode channel to 6 position switch
    // APM OUTPUT
    // Ch1: Wheel servo (direction)
    // Ch2: not used
    // Ch3: to the motor ESC
    // Ch4: not used

        { read_radio,             20ms,   1ms },
        { ahrs_update,            20ms,   6.400ms },
        { read_sonars,            20ms,   2.000 },
        { update_current_mode,    20ms,   1.500 },
        { set_servos,             20ms,   1.500 },
        { update_GPS_50Hz,        20ms,   2.500 },
        { update_GPS_10Hz,        100ms,   2.500 },
        { update_alt,             100ms,   3.400 },
        { navigate,               100ms,   1.600 },
        { update_compass,         100ms,   2.000 },
        { update_commands,        100ms,   1.000 },
        { update_logging1,        100ms,   1.000 },
        { update_logging2,        100ms,   1.000 },
        { gcs_retry_deferred,     20ms,   1.000 },
        { gcs_update,             20ms,   1.700 },
        { gcs_data_stream_send,   20ms,   3.000 },
        { read_control_switch,   300ms,   1.000 },
        { read_trim_switch,       100ms,   1.000 },
        { read_battery,           100ms,   1.000 },
        { read_receiver_rssi,     100ms,   1.000 },
        { update_events,          20ms,   1.000 },
        { check_usb_mux,         300ms,   1.000 },
        { mount_update,           20ms,    0.600 },
        { gcs_failsafe_check,     100ms,    0.600 },
        { compass_accumulate,     20ms,    0.900 },
        { update_notify,          20ms,    0.300 },
        { one_second_loop,       1s,   3.000 },
    #if FRSKY_TELEM_ENABLED == ENABLED
        { telemetry_send,        200ms,    0.100 }


    /*
     *  send a message on both GCS links
     */
    static void gcs_send_message(enum ap_message id)
    {
        for (uint8_t i=0; i<num_gcs; i++) {
            if (gcs[i].initialised) {
                gcs[i].send_message(id);
            }
        }
    }

    //一秒事件：
    // send a heartbeat
    gcs_send_message(MSG_HEARTBEAT);

    // allow orientation change at runtime to aid config
    ahrs.set_orientation(); //方向

    /*
      allow for runtime change of control channel ordering
     */
    set_control_channels();

    // save compass offsets once a minute

    static void update_GPS_50Hz(void);


    static void update_current_mode(void)
    { 
        switch (control_mode){
        case AUTO:
        case RTL:
        case GUIDED:  
        /*
        set the in_reverse flag
        reset the throttle integrator if this changes in_reverse
        */
       /*
        设置 in_reverse标志位，复位油门积分参数
        */
            set_reverse(false); //

            calc_lateral_acceleration(); //Calculate desired turn angles
            //calculate steering angle given lateral_acceleration
            calc_nav_steer();   
            calc_throttle(g.speed_cruise);
            break;

        case STEERING: {
            /*
              in steering mode we control lateral acceleration
              directly. We first calculate the maximum lateral
              acceleration at full steering lock for this speed. That is
              V^2/R where R is the radius of turn. We get the radius of
              turn from half the STEER2SRV_P.
             */
            float max_g_force = ground_speed * ground_speed / steerController.get_turn_radius();

            // constrain to user set TURN_MAX_G
            max_g_force = constrain_float(max_g_force, 0.1f, g.turn_max_g * GRAVITY_MSS);

            lateral_acceleration = max_g_force * (channel_steer->pwm_to_angle()/4500.0f);
            calc_nav_steer();

            // and throttle gives speed in proportion to cruise speed, up
            // to 50% throttle, then uses nudging above that.
            float target_speed = channel_throttle->pwm_to_angle() * 0.01 * 2 * g.speed_cruise;