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; iif (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;
            set_reverse(target_speed < 0);
            if (in_reverse) {
                target_speed = constrain_float(target_speed, -g.speed_cruise, 0);
            } else {
                target_speed = constrain_float(target_speed, 0, g.speed_cruise);
            }
            calc_throttle(target_speed);
            break;
        }

        case LEARNING:
        case MANUAL:
            /*
              in both MANUAL and LEARNING we pass through the
              controls. Setting servo_out here actually doesn't matter, as
              we set the exact value in set_servos(), but it helps for
              logging
             */
            channel_throttle->servo_out = channel_throttle->control_in;
            channel_steer->servo_out = channel_steer->pwm_to_angle();

            // mark us as in_reverse when using a negative throttle to
            // stop AHRS getting off
            set_reverse(channel_throttle->servo_out < 0);
            break;

        case HOLD:
            // hold position - stop motors and center steering
            channel_throttle->servo_out = 0;
            channel_steer->servo_out = 0;
            set_reverse(false);
            break;

        case INITIALISING:
            break;
        }
    }


    // This function is called at regular intervals (typically 10Hz).
    virtual void update_waypoint(const struct Location &prev_WP, const struct Location &next_WP) = 0;

两秒钟没有接收到心跳，即认为失联。

steering.c文件

    // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-

    /*****************************************
    * Throttle slew limit
    *****************************************/
    //在set_servos函数中调用
    static void throttle_slew_limit(int16_t last_throttle)
    {
        // if slew limit rate is set to zero then do not slew limit
        //如果转向的速率设置为0，则不要设置转向限制
        //参数THR_SLEWRATE：maximum percentage change in throttle per second.
        // A setting of 10 means to not change the throttle by more than 10% of the full throttle range in one second. 
        //A value of zero means no limit.
        //在一秒种，油门的最大变化百分比,默认值是0
        if (g.throttle_slewrate && last_throttle != 0) {                   
            // limit throttle change by the given percentage per second
            //通过每秒的百分比限制油门的变化，G_Dt = 0.02,：
            //初始化函数setup.pde时： 
            //channel_steer->radio_max = channel_steer->radio_in;
            //channel_throttle->radio_max = channel_throttle->radio_in;理论上后面的乘数值为0.2
            float temp = g.throttle_slewrate * G_Dt * 0.01f * fabsf(channel_throttle->radio_max - channel_throttle->radio_min);
            //G_Dt:This is the time between calls to the DCM algorithm and is
            // the Integration time for the gyros.It is 0.02s. 
            // allow a minimum change of 1 PWM per cycle
            if (temp < 1) {
                temp = 1;
            }
            //算出来的输出油门应该比之前的油门小temp值，比之前的油门大temp值。
            channel_throttle->radio_out = constrain_int16(channel_throttle->radio_out, last_throttle - temp, last_throttle + temp);
        }
    }

    /*
      check for triggering of start of auto mode
      检验自动模式的触发是否启动，calc_throttle函数调用
      当设置模式时，调用了set_mode函数，函数中当不处于自动模式时，
      auto_triggered置0
     */
    static bool auto_check_trigger(void)
    {
        // only applies to AUTO mode
        //如果不处于自动模式，则返回真。
        if (control_mode != AUTO) {
            return true;
        }
        // check for user pressing the auto trigger to off
        //auto_triggered是自动模式的标志位，如果处于自动模式，并且触发引脚为1，则把auto_triggered置为假，返回假
        //AUTO_TRIGGER_PIN:pin number to use to trigger start of auto mode. If set to -1 then
        // don’t use a trigger, otherwise this is a pin number which if held 
        //low in auto mode will start the motor, and otherwise will force the
        // throttle off. This can be used in combination with INITIAL_MODE to 
        //give a ‘press button to start’ rover with no receiver.
        if (auto_triggered && g.auto_trigger_pin != -1 && check_digital_pin(g.auto_trigger_pin) == 1) {
            gcs_send_text_P(SEVERITY_LOW, PSTR("AUTO triggered off"));
            auto_triggered = false;
            return false; 
        }

        // if already triggered, then return true, so you don't
        // need to hold the switch down
        //如果已经触发自动模式了，则没必要按下开关了，并且返回真。
        if (auto_triggered) {
            return true;
        }
        //如果触发引脚没有触发，并且快速启动标志位为0，则没有触发需要设置，auto_triggered设置为真，返回真。
        //AUTO_KICKSTART:X acceleration in meters/second/second to use to 
        //trigger the motor start in auto mode. If set to zero then auto 
        //throttle starts immediately when the mode switch happens, otherwise
        // the rover waits for the X acceleration to go above this value 
        //before it will start the motor
        if (g.auto_trigger_pin == -1 && g.auto_kickstart == 0.0f) {
            // no trigger configured - let's go!
            auto_triggered = true;
            return true;
        }
        //如果之前触发引脚参数值不为-1，同时重新读取引脚后为0
        //，auto_triggered设置为真，返回真。
        if (g.auto_trigger_pin != -1 && check_digital_pin(g.auto_trigger_pin) == 0) {
            gcs_send_text_P(SEVERITY_LOW, PSTR("Triggered AUTO with pin"));
            auto_triggered = true;
            return true;            
        }
        //如果kickstart不为0，并且读取的x轴加速度值比它大，则auto_triggered置为1
        //返回真。
        if (g.auto_kickstart != 0.0f) {
            float xaccel = ins.get_accel().x;
            if (xaccel >= g.auto_kickstart) {
                gcs_send_text_fmt(PSTR("Triggered AUTO xaccel=%.1f"), xaccel);
                auto_triggered = true;
                return true;            
            }
        }
        return false;   
    }

    /*
      work out if we are going to use pivot steering
     */
     //如果我们使用原地转向，调用此函数。被calc_throttle函数调用
     //如果方位偏差大于转向角，则使用原地转向。
    static bool use_pivot_steering(void)
    {
        //如果模式为auto，rtl，guided，initialising，滑动转向标志位设置为1，同时原地转向角不为0
        //参数:SKID_STEER_OUT:Set this to 1 for skid steering controlled
        // rovers (tank track style). When enabled, servo1 is used for the
        // left track control, servo3 is used for right track control
        //SONAR_TURN_ANGLE:The course deviation in degrees to apply while
        // avoiding an obstacle detected with the sonar. A positive number 
        //means to turn right, and a negative angle means to turn left.不像这个参数。
        if (control_mode >= AUTO && g.skid_steer_out && g.pivot_turn_angle != 0) {
            //target_bearing_cd函数
            // return the target bearing in centi-degrees. This is the bearing
            // from the vehicles current position to the target waypoint. This
            // should be calculated in the update_*() functions below.
            //返回值是目标方位（厘米度），范围是-18000~18000，
            //这是载具的位置到目标航点的方位，在update_*()函数中被计算。
            //bearing_error是方位偏差，范围是-180到180。
            int16_t bearing_error = wrap_180_cd(nav_controller->target_bearing_cd() - ahrs.yaw_sensor) / 100;
            if (abs(bearing_error) > g.pivot_turn_angle) {
                return true; //如果偏差大于原地转向角，则返回真。
            }
        }
        return false;
    }


    /*
      calculate the throtte for auto-throttle modes
     */
     //计算自动油门模式下的油门
    static void calc_throttle(float target_speed)
    {  
        //如果自动触发按钮没有按下，则油门舵机的输出等于参数油门最小值
        if (!auto_check_trigger()) {
            channel_throttle->servo_out = g.throttle_min.get();
            return;
        }
        //油门基数=目标速度/参数：巡航速度 *　参数：巡航油门
        //油门目标 = 油门基数 + 油门微调 throttle_nudge是油门杆的中间位置
        //CRUISE_SPEED:The target speed in auto missions.5m/s
        //CRUISE_THROTTLE:The base throttle percentage to use in auto mode.
        // The CRUISE_SPEED parameter controls the target speed, but the rover
        // starts with the CRUISE_THROTTLE setting as the initial estimate for
        // how much throttle is needed to achieve that speed. It then adjusts 
        //the throttle based on how fast the rover is actually going. 50%
        float throttle_base = (fabsf(target_speed) / g.speed_cruise) * g.throttle_cruise;
        //throttle_nudge:0-(throttle_max - throttle_cruise) : throttle nudge
        // in Auto mode using top 1/2 of throttle stick travel,我认为大概为0.
        int throttle_target = throttle_base + throttle_nudge;  
        /*
          reduce target speed in proportion to turning rate, up to the
          SPEED_TURN_GAIN percentage.
        */
        //SPEED_TURN_GAIN:The percentage to reduce the throttle while turning.
        // If this is 100% then the target speed is not reduced while turning. 
        //If this is 50% then the target speed is reduced in proportion to the
        // turn rate, with a reduction of 50% when the steering is maximally 
        //deflected. 默认值：50%
        //根据转向速度相应的减少目标速度，steer_rate = 横向加速度/（参数：最大转向加速度 * 9.8)
        float steer_rate = fabsf(lateral_acceleration / (g.turn_max_g*GRAVITY_MSS));
        steer_rate = constrain_float(steer_rate, 0.0, 1.0);

        // use g.speed_turn_gain for a 90 degree turn, and in proportion
        // for other turn angles
        //next_navigation_leg_cd下一个航点的角度，
        int32_t turn_angle = wrap_180_cd(next_navigation_leg_cd - ahrs.yaw_sensor);
        //返回值在0-1之间
        float speed_turn_ratio = constrain_float(fabsf(turn_angle / 9000.0f), 0, 1);
        float speed_turn_reduction = (100 - g.speed_turn_gain) * speed_turn_ratio * 0.01f;  //减少的百分比
        float reduction = 1.0 - steer_rate*speed_turn_reduction;

        if (control_mode >= AUTO && wp_distance <= g.speed_turn_dist) {
            // in auto-modes we reduce speed when approaching waypoints
            float reduction2 = 1.0 - speed_turn_reduction;
            if (reduction2 < reduction) {
                reduction = reduction2; //只取根据转向角度确定的百分比
            }
        }

        // reduce the target speed by the reduction factor
        target_speed *= reduction;
        groundspeed_error = fabsf(target_speed) - ground_speed; 

        throttle = throttle_target + (g.pidSpeedThrottle.get_pid(groundspeed_error * 100) / 100);

        // also reduce the throttle by the reduction factor. This gives a
        // much faster response in turns
        throttle *= reduction;

        if (in_reverse) {
            channel_throttle->servo_out = constrain_int16(-throttle, -g.throttle_max, -g.throttle_min);
        } else {
            channel_throttle->servo_out = constrain_int16(throttle, g.throttle_min, g.throttle_max);
        }

        if (!in_reverse && g.braking_percent != 0 && groundspeed_error < -g.braking_speederr) {
            // the user has asked to use reverse throttle to brake. Apply
            // it in proportion to the ground speed error, but only when
            // our ground speed error is more than BRAKING_SPEEDERR.
            //
            // We use a linear gain, with 0 gain at a ground speed error
            // of braking_speederr, and 100% gain when groundspeed_error
            // is 2*braking_speederr
            float brake_gain = constrain_float(((-groundspeed_error)-g.braking_speederr)/g.braking_speederr, 0, 1);
            int16_t braking_throttle = g.throttle_max * (g.braking_percent * 0.01f) * brake_gain;
            channel_throttle->servo_out = constrain_int16(-braking_throttle, -g.throttle_max, -g.throttle_min);

            // temporarily set us in reverse to allow the PWM setting to
            // go negative
            set_reverse(true);
        }

        if (use_pivot_steering()) {
            channel_throttle->servo_out = 0;
        }
    }

    /*****************************************
     * Calculate desired turn angles (in medium freq loop)
     *****************************************/

    static void calc_lateral_acceleration()
    {
        switch (control_mode) {
        case AUTO:
            nav_controller->update_waypoint(prev_WP, next_WP);
            break;

        case RTL:
        case GUIDED:
        case STEERING:
            nav_controller->update_waypoint(current_loc, next_WP);
            break;
        default:
            return;
        }

        // Calculate the required turn of the wheels

        // negative error = left turn
        // positive error = right turn
        lateral_acceleration = nav_controller->lateral_acceleration();
        if (use_pivot_steering()) {
            int16_t bearing_error = wrap_180_cd(nav_controller->target_bearing_cd() - ahrs.yaw_sensor) / 100;
            if (bearing_error > 0) {
                lateral_acceleration = g.turn_max_g*GRAVITY_MSS;
            } else {
                lateral_acceleration = -g.turn_max_g*GRAVITY_MSS;
            }
        }
    }

    /*
      calculate steering angle given lateral_acceleration
     */
    static void calc_nav_steer()
    {
        // add in obstacle avoidance
        lateral_acceleration += (obstacle.turn_angle/45.0f) * g.turn_max_g;

        // constrain to max G force
        lateral_acceleration = constrain_float(lateral_acceleration, -g.turn_max_g*GRAVITY_MSS, g.turn_max_g*GRAVITY_MSS);

        channel_steer->servo_out = steerController.get_steering_out_lat_accel(lateral_acceleration);
    }

    /*****************************************
    * Set the flight control servos based on the current calculated values
    *****************************************/
    static void set_servos(void)
    {
        static int16_t last_throttle;

        // support a separate steering channel
        RC_Channel_aux::set_servo_out(RC_Channel_aux::k_steering, channel_steer->pwm_to_angle_dz(0));

        if (control_mode == MANUAL || control_mode == LEARNING) {
            // do a direct pass through of radio values
            channel_steer->radio_out       = channel_steer->read();
            channel_throttle->radio_out    = channel_throttle->read();
            if (failsafe.bits & FAILSAFE_EVENT_THROTTLE) {
                // suppress throttle if in failsafe and manual
                channel_throttle->radio_out = channel_throttle->radio_trim;
            }
        } else {       
            channel_steer->calc_pwm();
            if (in_reverse) {
                channel_throttle->servo_out = constrain_int16(channel_throttle->servo_out, 
                                                              -g.throttle_max,
                                                              -g.throttle_min);
            } else {
                channel_throttle->servo_out = constrain_int16(channel_throttle->servo_out, 
                                                              g.throttle_min.get(), 
                                                              g.throttle_max.get());
            }

            if ((failsafe.bits & FAILSAFE_EVENT_THROTTLE) && control_mode < AUTO) {
                // suppress throttle if in failsafe
                channel_throttle->servo_out = 0;
            }

            // convert 0 to 100% into PWM
            channel_throttle->calc_pwm();

            // limit throttle movement speed
            throttle_slew_limit(last_throttle);
        }

        // record last throttle before we apply skid steering
        last_throttle = channel_throttle->radio_out;

        if (g.skid_steer_out) {
            // convert the two radio_out values to skid steering values
            /*
              mixing rule:
              steering = motor1 - motor2
              throttle = 0.5*(motor1 + motor2)
              motor1 = throttle + 0.5*steering
              motor2 = throttle - 0.5*steering
            */          
            float steering_scaled = channel_steer->norm_output();
            float throttle_scaled = channel_throttle->norm_output();
            float motor1 = throttle_scaled + 0.5*steering_scaled;
            float motor2 = throttle_scaled - 0.5*steering_scaled;
            channel_steer->servo_out = 4500*motor1;
            channel_throttle->servo_out = 100*motor2;
            channel_steer->calc_pwm();
            channel_throttle->calc_pwm();
        }


    #if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS
        // send values to the PWM timers for output
        // ----------------------------------------
        channel_steer->output(); 
        channel_throttle->output();
        RC_Channel_aux::output_ch_all();
    #endif
    }

求ground_speed

    if (!using_EKF()) {
        return AP_AHRS_DCM::groundspeed_vector();
    }
    Vector3f velocity;
    if (ahrs.get_velocity_NED(velocity)) {
        ground_speed = pythagorous2(velocity.x, velocity.y);
    }

其中ahrs.get_velocity_NED(velocity)调用get_velocity_NED(Vector3f &vec)

    // return a ground velocity in meters/second, North/East/Down
    // order. Must only be called if have_inertial_nav() is true
    bool AP_AHRS_NavEKF::get_velocity_NED(Vector3f &vec) const
    {
        if (using_EKF()) {
            EKF.getVelNED(vec);
            return true;
        }
        return false;
    }

getVelNED调用：

    // return NED velocity in m/s
    void NavEKF::getVelNED(Vector3f &vel) const
    {
        vel = state.velocity;
    }

    ResetVelocity(void)
    {
        //如果是静止的，则都初始化为0
        if (staticMode) {
             state.velocity.zero();
             state.vel1.zero();
             state.vel2.zero();
        } else if (_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_3D) {
            // read the GPS
            readGpsData();
            // Set vertical GPS velocity to 0 if mode > 0 (assume 0 if no VZ measurement)
            if (_fusionModeGPS >= 1) {
                velNED[2] = 0.0f;
            }
            // reset filter velocity states
            state.velocity = velNED;
            state.vel1 = velNED;
            state.vel2 = velNED;
            // reset stored velocity states to prevent subsequent GPS measurements from being rejected
            for (uint8_t i=0; i<=49; i++){
                storedStates[i].velocity = velNED;
            }
        }
    }


    // update the quaternion, velocity and position states using IMU measurements
    void NavEKF::UpdateStrapdownEquationsNED()

    // calculate the predicted state covariance matrix
    void NavEKF::CovariancePrediction()

    // fuse selected position, velocity and height measurements, checking dat for consistency
    // provide a static mode that allows maintenance of the attitude reference without GPS provided the vehicle is not accelerating
    // check innovation consistency of velocity states calculated using IMU1 and IMU2 and calculate the optimal weighting of accel data
    void NavEKF::FuseVelPosNED()

    // fuse true airspeed measurements
    void NavEKF::FuseAirspeed()

    // fuse sythetic sideslip measurement of zero
    void NavEKF::FuseSideslip()

其中groundspeed_vector()调用groundspeed_vector(void)

    // Generate estimate of ground speed vector using GPS
    if (gotGPS) {
        float cog = radians(_gps.ground_course_cd()*0.01f);
        gndVelGPS = Vector2f(cosf(cog), sinf(cog)) * _gps.ground_speed();
    }

问题：这个Vector2f(cosf(cog), sinf(cog)) 理论上不是等于1吗

纠正航向偏差

void AP_AHRS_DCM::drift_correction_yaw(void)

使用罗盘或者GPS计算航向偏差

是否使用罗盘
1. 当没有gps时，或者不能假定车沿着x轴走使用罗盘
2. 当gps测得的速度小于最小有效速度时3m/s，使用罗盘
4.如果GPS测得的航向，与罗盘测得航向相差45度时，并且持续了2秒钟，则只使用GPS数据。

如果没有初始化yaw，则先初始化yaw：
用姿态矩阵计算航向，

参数

_time_constant_xy //时间常数，多长时间使用GPS和加速度融合

//位置补偿增益
_k1_xy = 3.0f / _time_constant_xy;
//速度补偿增益
_k2_xy = 3.0f / (_time_constant_xy*_time_constant_xy);
//加速度补偿增益
_k3_xy = 1.0f / (_time_constant_xy*_time_constant_xy*_time_constant_xy);