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
// 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
两秒钟没有接收到心跳,即认为失联。
// -*- 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
}
/***************************************** * 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)
<code class="language-c hljs has-numbering"> <span class="hljs-comment">// Generate estimate of ground speed vector using GPS</span> <span class="hljs-keyword">if</span> (gotGPS) { <span class="hljs-keyword">float</span> cog = radians(_gps.ground_course_cd()*<span class="hljs-number">0.01f</span>); gndVelGPS = Vector2f(cosf(cog), sinf(cog)) * _gps.ground_speed(); }</code><ul class="pre-numbering" style="display: block;"><li>1</li><li> </li><li> </li><li>题:这个Vector2f(cosf(cog), sinf(cog)) 理论上不是等于1吗</li></ul>
纠正航向偏差
<code class="language-c hljs has-numbering"><span class="hljs-keyword">void</span> AP_AHRS_DCM::drift_correction_yaw(<span class="hljs-keyword">void</span>)</code><ul class="pre-numbering" style="display: block;"><li>1</li></ul><ul class="pre-numbering" style="display: block;"><li>1</li></ul>
使用罗盘或者GPS计算航向偏差
是否使用罗盘
1. 当没有gps时,或者不能假定车沿着x轴走使用罗盘
2. 当gps测得的速度小于最小有效速度时3m/s,使用罗盘
4.如果GPS测得的航向,与罗盘测得航向相差45度时,并且持续了2秒钟,则只使用GPS数据。如果没有初始化yaw,则先初始化yaw:
用姿态矩阵计算航向,参数
<code class="language-c hljs has-numbering">_time_constant_xy <span class="hljs-comment">//时间常数,多长时间使用GPS和加速度融合</span> <span class="hljs-comment">//位置补偿增益</span> _k1_xy = <span class="hljs-number">3.0f</span> / _time_constant_xy; <span class="hljs-comment">//速度补偿增益</span> _k2_xy = <span class="hljs-number">3.0f</span> / (_time_constant_xy*_time_constant_xy); <span class="hljs-comment">//加速度补偿增益</span> _k3_xy = <span class="hljs-number">1.0f</span> / (_time_constant_xy*_time_constant_xy*_time_constant_xy);</code>
-
-