先从mode_auto.cpp的update()开始分析。
流程如图:
进入函数update()后会执行函数navigate_to_waypoint()。
mode_auto.cpp
void ModeAuto::update()
{
navigate_to_waypoint();
}
mode.cpp
void Mode::navigate_to_waypoint()
{
g2.wp_nav.update(rover.G_Dt);
calc_steering_from_turn_rate(g2.wp_nav.get_turn_rate_rads(), desired_speed, g2.wp_nav.get_reversed());
}
AR_WPNav.cpp
void AR_WPNav::update(float dt)
{
update_steering(current_loc, speed);
update_desired_speed(dt);
}
AR_WPNav.cpp
void AR_WPNav::update_steering(const Location& current_loc, float current_speed)
{
_nav_controller.set_reverse(_reversed);
_nav_controller.update_waypoint(_reached_destination ? current_loc : _oa_origin, _oa_destination, _radius);
_desired_turn_rate_rads = _atc.get_turn_rate_from_lat_accel(_desired_lat_accel, current_speed);
}
此处调用了L1航迹控制算法,得到向心力后,通过函数get_turn_rate_from_lat_accel()得到应有的角速度。
至此,继续执行mode.cpp的calc_steering_from_turn_rate()函数。
mode.cpp
void Mode::calc_steering_from_turn_rate(float turn_rate, float speed, bool reversed)
{
const float steering_out = attitude_control.get_steering_out_rate(turn_rate,
g2.motors.limit.steer_left,
g2.motors.limit.steer_right,
rover.G_Dt);
g2.motors.set_steering(steering_out * 4500.0f);
}
AP_MotorsUGV.cpp
void AP_MotorsUGV::set_steering(float steering, bool apply_scaling)
{
_steering = steering;
_scale_steering = apply_scaling;
}
此处会得到一个_steering值,待其他函数调用。
再来看Rover.cpp里面的一个循环任务
SCHED_TASK(set_servos, 400, 200),
void Rover::set_servos(void)
{
if (motor_test) {
motor_test_output();
} else {
float speed;
if (!g2.attitude_control.get_forward_speed(speed)) {
speed = 0.0f;
}
g2.motors.output(arming.is_armed(), speed, G_Dt);
}
}
此处调用AP_MotorsUGV.cpp中的output()函数,使用到了_steering(应该是转弯成度)。
void AP_MotorsUGV::output(bool armed, float ground_speed, float dt)
{
if (!hal.util->get_soft_armed()) {
armed = false;
_throttle = 0.0f;
}
sanity_check_parameters();
slew_limit_throttle(dt);
output_regular(armed, ground_speed, _steering, _throttle);
output_skid_steering(armed, _steering, _throttle, dt);
output_omni(armed, _steering, _throttle, _lateral);
output_mainsail();
SRV_Channels::calc_pwm();
SRV_Channels::cork();
SRV_Channels::output_ch_all();
SRV_Channels::push();
}
SRV_Channels、SRV_Channel为PWM输出类,此处还不了解,SRV_Channels为多个输出通道,SRV_Channel为单个输出通道。
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)