ArduCopter源码解析学习(一)——ArduCopter主程序
- 前言
- 一、准备工作
- 二、Copter.cpp解析
-
- 2.1 scheduler table
- 2.2 scheduler
-
- get_scheduler_tasks()
- setup()和loop()
- 2.3 fast_loop()
-
- 2.3.1 flight_mode.cpp
- 2.3.2 control_stabilize.cpp
- 2.3.3 AC_AttitudeControl.cpp
- 2.4 其他
- 三、总结
前言
由于项目经常需要做功能优化所以开源飞控APM和PX4作为最热门的飞控资源,需要针对性的进行梳理。
一、准备工作
参考https://blog.csdn.net/moumde/article/details/108814986 在网上查了很多资料,在阅读本文内容之前,请仔细阅读Ardupilot的官方手册,建立起一个大概的印象即可,这里便不再讲述一些基础知识。
Github源码戳这里~
Ardusub官方手册
Ardupilot官方手册
Ardupilot开发者手册
由于源码内容很多,此次我仅挑选 “ardupilot/Copter/” 路径下的Copter.cpp主文件进行解析,后续如果有时间再慢慢补充其他内容。如果自己下载源码,推荐使用Souce Insight或者understand进行阅读。
二、Copter.cpp解析
2.1 scheduler table
SCHED_TASK_CLASS和SCHED_TASK用来声明产生调度任务表,表中是除了被fast_loop()函数调用任务的其他常规任务。其中_rate_hz表示调用频率,_max_time_micros表示最长等待时间。
#define SCHED_TASK_CLASS(classname, classptr, func, _rate_hz, _max_time_micros)
#define SCHED_TASK(func, rate_hz, max_time_micros) SCHED_TASK_CLASS(Sub, &sub, func, rate_hz, max_time_micros)
注意到SCHED_TASK_CLASS可以指向不同类,而SCHED_TASK固定为Copter类中的函数实现,Copter为无人机类型。
2.2 scheduler
get_scheduler_tasks()
如果是master版本的,程序前面应该是get_scheduler_tasks()
void Copter::get_scheduler_tasks(const AP_Scheduler::Task *&tasks,
uint8_t &task_count,
uint32_t &log_bit)
{
tasks = &scheduler_tasks[0];
task_count = ARRAY_SIZE(scheduler_tasks);
log_bit = MASK_LOG_PM;
}
该函数主要实现获取调度表其实地址,并且计算出任务数量。
2.3 fast_loop()
void Copter::fast_loop()
{
ins.update();
attitude_control->rate_controller_run();
motors_output();
read_AHRS();
#if FRAME_CONFIG == HELI_FRAME
update_heli_control_dynamics();
#if MODE_AUTOROTATE_ENABLED == ENABLED
heli_update_autorotation();
#endif
#endif
read_inertia();
check_ekf_reset();
update_flight_mode();
update_home_from_EKF();
update_land_and_crash_detectors();
#if HAL_MOUNT_ENABLED
camera_mount.update_fast();
#endif
if (should_log(MASK_LOG_ANY)) {
Log_Sensor_Health();
}
AP_Vehicle::fast_loop();
}
该函数为本文件中最重要的函数,以400Hz频率运行。
attitude_control.rate_controller_run()在libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp中。
void AC_AttitudeControl_Multi::rate_controller_run()
{
update_throttle_rpy_mix();
_rate_target_ang_vel += _rate_sysid_ang_vel;
Vector3f gyro_latest = _ahrs.get_gyro_latest();
_motors.set_roll(get_rate_roll_pid().update_all(_rate_target_ang_vel.x, gyro_latest.x, _motors.limit.roll) + _actuator_sysid.x);
_motors.set_roll_ff(get_rate_roll_pid().get_ff());
_motors.set_pitch(get_rate_pitch_pid().update_all(_rate_target_ang_vel.y, gyro_latest.y, _motors.limit.pitch) + _actuator_sysid.y);
_motors.set_pitch_ff(get_rate_pitch_pid().get_ff());
_motors.set_yaw(get_rate_yaw_pid().update_all(_rate_target_ang_vel.z, gyro_latest.z, _motors.limit.yaw) + _actuator_sysid.z);
_motors.set_yaw_ff(get_rate_yaw_pid().get_ff()*_feedforward_scalar);
_rate_sysid_ang_vel.zero();
_actuator_sysid.zero();
control_monitor_update();
}
主要是用set_xxx()方法接收roll/pitch/yaw的-1~1的值,这些值表示的是期望尽快往哪个方向运动的意思。具体查看这里。
这里主要讲一下最主要的姿态控制函数update_flight_mode()。
2.3.1 mode.cpp
update_flight_mode()定义于Copter文件路径下的mode.cpp,该函数通常以100Hz及以上频率被调用。内部的 flightmode 定义于mode.h中,用来表示不同的飞行状态,共计27种模式。
在mode.cpp中update_flight_mode()的功能函数如下:
void Copter::update_flight_mode()
{
surface_tracking.invalidate_for_logging();
flightmode->run();
}
在mode.h中定义了各个飞行模式的具体含义内容:
enum class Number : uint8_t {
STABILIZE = 0,
ACRO = 1,
ALT_HOLD = 2,
AUTO = 3,
GUIDED = 4,
LOITER = 5,
RTL = 6,
CIRCLE = 7,
LAND = 9,
DRIFT = 11,
SPORT = 13,
FLIP = 14,
AUTOTUNE = 15,
POSHOLD = 16,
BRAKE = 17,
THROW = 18,
AVOID_ADSB = 19,
GUIDED_NOGPS = 20,
SMART_RTL = 21,
FLOWHOLD = 22,
FOLLOW = 23,
ZIGZAG = 24,
SYSTEMID = 25,
AUTOROTATE = 26,
};
update_flight_mode()中对应的flightmode->run ()函数则是在同路径下的mode_xxx.cpp中定义。下面以ModeStabilize::run()为例进行讲解,因此我们需要进入Mode_stabilize.cpp中。
2.3.2 Mode_stabilize.cpp
注意到内部包含Mode_stabilize.cpp文件只有一个ModeStabilize::run()函数。
void ModeStabilize::run()
{
update_simple_mode();
float target_roll, target_pitch;
get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, copter.aparm.angle_max);
float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
if (!motors->armed()) {
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
} else if (copter.ap.throttle_zero) {
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
} else {
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
}
switch (motors->get_spool_state()) {
case AP_Motors::SpoolState::SHUT_DOWN:
attitude_control->set_yaw_target_to_current_heading();
attitude_control->reset_rate_controller_I_terms();
break;
case AP_Motors::SpoolState::GROUND_IDLE:
attitude_control->set_yaw_target_to_current_heading();
attitude_control->reset_rate_controller_I_terms();
break;
case AP_Motors::SpoolState::THROTTLE_UNLIMITED:
if (!motors->limit.throttle_lower) {
set_land_complete(false);
}
break;
case AP_Motors::SpoolState::SPOOLING_UP:
case AP_Motors::SpoolState::SPOOLING_DOWN:
break;
}
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
attitude_control->set_throttle_out(get_pilot_desired_throttle(),
true,
g.throttle_filt);
}
run()函数则是实现了具体的控制器功能,该函数应该以100Hz及以上频率被调用。
void Copter::update_simple_mode(void)
{
float rollx, pitchx;
if (simple_mode == SimpleMode::NONE || !ap.new_radio_frame) {
return;
}
ap.new_radio_frame = false;
if (simple_mode == SimpleMode::SIMPLE) {
rollx = channel_roll->get_control_in()*simple_cos_yaw - channel_pitch->get_control_in()*simple_sin_yaw;
pitchx = channel_roll->get_control_in()*simple_sin_yaw + channel_pitch->get_control_in()*simple_cos_yaw;
}else{
rollx = channel_roll->get_control_in()*super_simple_cos_yaw - channel_pitch->get_control_in()*super_simple_sin_yaw;
pitchx = channel_roll->get_control_in()*super_simple_sin_yaw + channel_pitch->get_control_in()*super_simple_cos_yaw;
}
channel_roll->set_control_in(rollx*ahrs.cos_yaw() + pitchx*ahrs.sin_yaw());
channel_pitch->set_control_in(-rollx*ahrs.sin_yaw() + pitchx*ahrs.cos_yaw());
}
接下来get_pilot_desired_lean_angles()用于将打杆量映射成角度。
void Mode::get_pilot_desired_lean_angles(float &roll_out, float &pitch_out, float angle_max, float angle_limit) const
{
if (copter.failsafe.radio || !copter.ap.rc_receiver_present) {
roll_out = 0;
pitch_out = 0;
return;
}
roll_out = channel_roll->get_control_in();
pitch_out = channel_pitch->get_control_in();
angle_limit = constrain_float(angle_limit, 1000.0f, angle_max);
float scaler = angle_max/(float)ROLL_PITCH_YAW_INPUT_MAX;
roll_out *= scaler;
pitch_out *= scaler;
float total_in = norm(pitch_out, roll_out);
if (total_in > angle_limit) {
float ratio = angle_limit / total_in;
roll_out *= ratio;
pitch_out *= ratio;
}
roll_out = (18000/M_PI) * atanf(cosf(pitch_out*(M_PI/18000))*tanf(roll_out*(M_PI/18000)));
}
-
在执行完上述两部分内容后,首先判断电机是否已经准备就绪,如果还没有,就进入if中的代码段
- 首先是通过set_desired_spool_state()函数设置为期望的轴状态,此处设置为GROUOND_IDLE,即将所有电机闲置(lobraries/AP_Motors/AP_Motors_Class.h中SpoolState声明了3种轴状态:SHUT_DOWN关闭、GROUND_IDLE空闲和THROTTLE_UNLIMITED取消限制)。
-
如果电机已经准备好了,那么就不进入if()的代码段中,首先设置轴状态为取消限制。get_pilot_desired_lean_angles(float roll_in, float pitch_in, float &roll_out, float &pitch_out, float angle_max)函数将获取roll和pitch的输入量然后将其转换为对应的以度单位表示的角度输出传输给在run()最开始设置的target_roll和target_pitch。get_pilot_desired_yaw_rate()函数将获取输入的yaw期望并且将其转换为°/s为单位yaw角速度输出并保存在target_yaw_rate中。
-
然后进入姿态控制器,更新姿态至期望值。
2.3.3 AC_AttitudeControl.cpp
最后来看一下在libraries库中AC_AttitudeControl下AC_AttitudeControl.cpp中的input_euler_angle_roll_pitch_euler_rate_yaw()方法,其实大家看源码也能看出大概的意思了。
这边也简单说一下,函数内部先把相应的角度转换为弧度制,然后计算出对于的期望欧拉角,并在欧拉角的roll向上增加侧倾调整以补偿推力,然后判断是否开启了前馈。是的话,还需要进行一系列计算,例如将摇杆期望欧拉角与前馈目标欧拉角进行差值计算以得到前馈角速度,还有将目标欧拉角速度转换为机体角速度等等;如果没有开启前馈,那么就直接将欧拉角输入到目标中,并根据目标欧拉角转换得出目标四元数,然后前馈率归零。最后调用四元数姿态控制器。
总的来说,这个函数的功能就是通过角速度前馈和平滑来控制欧拉角和俯仰角以及欧拉偏航率。对于前面mode_stabilize.cpp中所用的input_euler_angle_roll_pitch_euler_rate_yaw则是通过角速度前馈和平滑控制欧拉角,俯仰角和偏航角。如果有兴趣继续研究,详看官方的手册。
void AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds)
{
float euler_roll_angle = radians(euler_roll_angle_cd * 0.01f);
float euler_pitch_angle = radians(euler_pitch_angle_cd * 0.01f);
float euler_yaw_rate = radians(euler_yaw_rate_cds * 0.01f);
_attitude_target_quat.to_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);
euler_roll_angle += get_roll_trim_rad();
if (_rate_bf_ff_enabled) {
Vector3f euler_accel = euler_accel_limit(_attitude_target_euler_angle, Vector3f(get_accel_roll_max_radss(), get_accel_pitch_max_radss(), get_accel_yaw_max_radss()));
_attitude_target_euler_rate.x = input_shaping_angle(wrap_PI(euler_roll_angle - _attitude_target_euler_angle.x), _input_tc, euler_accel.x, _attitude_target_euler_rate.x, _dt);
_attitude_target_euler_rate.y = input_shaping_angle(wrap_PI(euler_pitch_angle - _attitude_target_euler_angle.y), _input_tc, euler_accel.y, _attitude_target_euler_rate.y, _dt);
_attitude_target_euler_rate.z = input_shaping_ang_vel(_attitude_target_euler_rate.z, euler_yaw_rate, euler_accel.z, _dt);
euler_rate_to_ang_vel(_attitude_target_euler_angle, _attitude_target_euler_rate, _attitude_target_ang_vel);
ang_vel_limit(_attitude_target_ang_vel, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), radians(_ang_vel_yaw_max));
ang_vel_to_euler_rate(_attitude_target_euler_angle, _attitude_target_ang_vel, _attitude_target_euler_rate);
} else {
_attitude_target_euler_angle.x = euler_roll_angle;
_attitude_target_euler_angle.y = euler_pitch_angle;
_attitude_target_euler_angle.z += euler_yaw_rate * _dt;
_attitude_target_quat.from_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);
_attitude_target_euler_rate = Vector3f(0.0f, 0.0f, 0.0f);
_attitude_target_ang_vel = Vector3f(0.0f, 0.0f, 0.0f);
}
attitude_controller_run_quat();
}
2.4 其他
无。
三、总结
以上便是本次的全部内容,下一篇打算讲讲Copter内部的底层电机控制原理。
第一次更新:2020/12/1
第二次更新:2020/12/2
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)