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;            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);