ArduCopter代码分析

ArduCopter-v2代码结构及算法分析
代码模块划分及功能描述
ArduCopter.cpp是整个程序的启动点
模块路径：ardupilot/libraries/
一：AP_Scheduler 简单的多任务分时分片调度系统
模块对外接口
Void init(const Task *tasks, uint8_t num_tasks) 多任务初始化
void tick(void) 控制计数值加1
void run(uint16_t time_available) 遍寻任务数组，执行可以执行的任务
uint16_t time_available_usec(void) 获取当前任务的剩余可用时间
AP_Scheduler::Task 任务(ardupilot/ArduCopter. Pde Line 777)
二：AP_HAL 硬件抽象层
模块是整个系统的核心,Nuttx系统通过该模块启动用户代码部分
包含了各个模块的驱动
关键类:
AP_HAL::HAL抽象基类
HAL_PX4::HAL_PX4 是我们需要关心的子类(AP_HAL_PX4)
该模块提供给用户的扩展接口：
setup()进行系统中其余模块的初始化
loop()主循环
在本例中该方法在ardupilot/ArduCopter/ArduCopter.pde line 1612实现
AP_HAL_MAIN 宏定义（）用户必须通过该宏定义来定义程序的入口，完成hal的初始化init
init 会启动main_loop主循环，main_loop调用使用者定义的setup 和loop 扩展接口
包含的类
UARTDriver 端口读写的类
接口：
begin()
begin(波特率，读缓存，写缓存)
end()
flush()
set_blocking_writes()
set_flow_control()
get_flow_control()
(1)PX4AnalogSource
(2)PX4AnalogIn
(3)PX4GPIO
接口：
Init()
pinMode()
analogPinToDigitalPin()
read()
write()
toggle()
channel()
attach_interrupt()
usb_connected()
(5) PX4RCInput 遥控输入
接口：
void init(void* machtnichts);
bool new_input(); 判断是否有新的输入到达
uint8_t num_channels(); 获取RC输入通道个数
uint16_t read(uint8_t ch); 读取指定通道的输入
uint8_t read(uint16_t* periods, uint8_t len); 读取指定多个通道的输入
bool set_overrides(int16_t *overrides, uint8_t len);
bool set_override(uint8_t channel, int16_t override); 给指定通道设置假数据，如果该通道设置了假数据，则读取该通道的输入，始终读到的是设置的数据
void clear_overrides();
void _timer_tick(void); 循环调用每个tick更新一次input数据
(4)PX4RCOutput 输出
void init(void* machtnichts);
void set_freq(uint32_t chmask, uint16_t freq_hz); 设置多个通道的输出频率，必须大于50
uint16_t get_freq(uint8_t ch); 获取指定通道的输出频率
void enable_ch(uint8_t ch); 最多8个通道
void disable_ch(uint8_t ch);
void write(uint8_t ch, uint16_t period_us); 单个通道写输出
void write(uint8_t ch, uint16_t* period_us, uint8_t len); 多个通道写输出
uint16_t read(uint8_t ch);
void read(uint16_t* period_us, uint8_t len);
void set_safety_pwm(uint32_t chmask, uint16_t period_us);
void set_failsafe_pwm(uint32_t chmask, uint16_t period_us);
void force_safety_off(void);
void _timer_tick(void); 循环调用每个tick将output数据输出，电机、相机控制
(5)PX4Storage
(6)I2CDriver
AP_HAL::UARTDriver* uartA; GCS
AP_HAL::UARTDriver* uartB; GPS
AP_HAL::UARTDriver* uartC; GCS
AP_HAL::UARTDriver* uartD; GCS
AP_HAL::UARTDriver* uartE; GPS
AP_HAL::I2CDriver* i2c;
AP_HAL::SPIDeviceManager* spi; 板载加速度计和磁罗盘、陀螺仪、空压表
AP_HAL::AnalogIn* analogin; 电源输入
AP_HAL::Storage* storage;
AP_HAL::UARTDriver* console;
AP_HAL::GPIO* gpio; 控制飞控板上的LED指示灯、蜂鸣器等显示
AP_HAL::RCInput* rcin; 遥控器输入
AP_HAL::RCOutput* rcout; 电机输出
AP_HAL::Scheduler* scheduler;
AP_HAL::Util* util;
三：AP_Notify 根据飞机状态更新各种指示灯提示、告警
模块对外接口
init(bool enable_external_leds)
对初始化，并对AP_BoardLED、ToshibaLED_PX4、ToneAlarm_PX4、ExternalLED、ToshibaLED_I2C、ExternalLED、ToshibaLED_I2C进行初始化
void update(void)
用户层可以通过该模块通知
void AP_Notify::init(bool enable_external_leds) 初始化各种指示灯、蜂鸣器等等
void AP_Notify::update(void) 各种指示灯根据flags标示进行更新状态
四：AP_Vehicle
仅仅是固定翼和螺旋飞机的通用参数结构体
五：AP_GPS GPS相关库
模块接口
Init()
Update()
can_calculate_base_pos()
calculate_base_pos()
status() 查询gps状态
Location &location(uint8_t instance)
const Vector3f &velocity(uint8_t instance)
velocity()
float ground_speed(uint8_t instance)
AP_GPS_Glitch.h中GPS_Glitch GPS干扰保护库
添加了AP_GPS数据的判断
对外接口：check_position() 判断GPS是否正常
判断算法AP_GPS_Glitch.cpp
支持的几种GPS协议
AP_GPS_UBLOX 我们现在飞机应该装的这个
AP_GPS_MTK19
AP_GPS_MTK
AP_GPS_SBP
AP_GPS_SIRF
AP_GPS_NMEA
GPS模块结构：
上层接口AP_GPS
底层接口AP_GPS_Backend
read() 接口，读取GPS数据，解析后统一填充到AP_GPS：：GPS_State结构中，各种不同GPS会有不同的实现
底层实现AP_GPS_UBLOX
六：AP_Baro 气压计相关库PX4
AP_Baro_PX4
对外接口参考
Healthy() 判断气压计能否正常工作
bool init(); 初始化
uint8_t read();
float get_pressure(); 获取气压值
float get_temperature(); 获取气温值
get_altitude() 获取高度值
calibrate() 校准气压计
get_climb_rate() 获取爬升率DerivativeFilter
struct baro_report {
float pressure;
float altitude;
float temperature;
uint64_t timestamp;
uint64_t error_count;
};
AP_Baro_Glitch.h 气压计干扰保护库
check_alt() 判断气压计是否正常中Baro_Glitch
支持几种类型的气压计
AP_Baro_BMP085
AP_Baro_HIL
AP_Baro_MS5611
AP_Baro_PX4
AP_Baro_VRBRAIN
本例中使用的是AP_Baro_PX4
七：AP_Compass_PX4 三轴罗盘相关库
接口参考AP_Compass_PX4.h和Compass.h
Init() 三轴罗盘初始化
Read() 从罗盘驱动读取数据
get_field() 获取罗盘数据
八：AP_InertialSensor 惯性传感器
主要接口：
init( Start_style style, Sample_rate sample_rate) 加速度计和陀螺仪初始化
update() 根据罗盘、加速度的._gyro_health 情况更新主陀螺仪、主加速度计
get_gyro_drift_rate(void) 获取最大的陀螺漂移率
get_accel() 获取加速度信息
get_gyro() 获取陀螺仪信息
AP_InertialSensor_Backend/AP_InertialSensor_PX4
主要接口：
Update()
循环被调用，获取陀螺仪、加速度信息，并通过_rotate_and_offset_gyro() _rotate_and_offset_acce 对获取的信息做矩阵变换
最终输出保留在
_imu._gyro[instance]
_imu._gyro_healthy[instance]
_imu._accel[instance]
_imu._accel_healthy[instance]中
accel_sample_available() 判断加速度传感器可用
gyro_sample_available() 判断陀螺仪传感器可用
_rotate_and_offset_gyro(uint8_t instance, const Vector3f &gyro) gyro 为从陀螺仪获取的姿态数据
{
_imu._gyro[instance] = gyro;
_imu._gyro[instance].rotate(_imu._board_orientation);
_imu._gyro[instance] -= _imu._gyro_offset[instance];
_imu._gyro_healthy[instance] = true;
}
_rotate_and_offset_accel(uint8_t instance, const Vector3f &accel)
{// accel 为从加速度计获取的加速度信息
_imu._accel[instance] = accel;
_imu._accel[instance].rotate(_imu._board_orientation);
const Vector3f &accel_scale = _imu._accel_scale[instance].get();
_imu._accel[instance].x *= accel_scale.x;
_imu._accel[instance].y *= accel_scale.y;
_imu._accel[instance].z *= accel_scale.z;
_imu._accel[instance] -= _imu._accel_offset[instance];
_imu._accel_healthy[instance] = true;
}
_set_accel_error_count() 从陀螺仪、加速度计获取error信息
_set_gyro_error_count()
_default_filter()
属性：
AP_InertialSensor &_imu;
九：AP_AHRS Attitude Heading Reference System 航姿系统
AP_AHRS_DCM
AP_AHRS_NavEKF
采用DCM（方向余弦矩阵方法）或EKF（扩展卡尔曼滤波方法）预估飞行器姿态姿态和方位参考系统
综合惯性传感器、气压采样、gps进行姿态和方位管理
接口：
AP_AHRS_NavEKF(AP_InertialSensor &ins, AP_Baro &baro, AP_GPS &gps)
Init() 设置ins、compass的反向AP_AHRS
set_compass(Compass *compass) 设置磁罗盘
set_airspeed(AP_Airspeed *airspeed)
get_accel_ef(void) 获取加速度计的值in earth frame
airspeed_estimate() 预估空速AP_AHRS_NavEKF
add_trim(….) 调整-trim roll 、pitch
groundspeed_vector() 预估飞机相对地面的水平速度
update_trig(void) 根据dcm旋转矩阵更新cos_yaw、sina_yaw、cos_roll、sina_roll、sina_yaw、cos_yaw
update_cd_values(void) 根据roll、yaw、pitch更新roll_sensor、yaw_sensor、pitch_sensor get_gyro(void) 获取漂移校准的陀螺矢量
get_gyro_drift(void)
get_position(struct Location &loc) 获取当前位置，高度通过气压计计算，gps
euler_angles(void) 计算欧拉角、方向矩阵
estimate_wind() 预估风速
get_dcm_matrix(void) 获取方向余弦矩阵
get_error_rp(void)
get_error_yaw(void)
update() 更新数据
AP_Airspeed
十：AP_Mission 结合AP_AHRS进行飞行任务管理
十一：AP_RangeFinder 声呐或红外测距传感器的交互库
十二：RCMapper
通道映射，保存了roll、pitch、yaw、throttle的输入通道ID
十三：AP_BoardConfig
十四：AP_BattMonitor 电源模块管理
十五：AP_Frsky_Telem
十六：AP_InertialNav\AP_InertialNav_NavEKF 扩展带有gps和气压计数据的惯性导航库
接口：
get_altitude() 获取高度信息
get_velocity_z() 获取z轴上的速度
get_longitude() 获取基于经度的加速度
get_latitude() 获取基于纬度的加速度
get_latitude_diff()
get_longitude_diff()
get_velocity_xy() 设置在经度和纬度方向的速度
十七：AC_AttitudeControl_Heli/AC_AttitudeControl 航姿控制相关库
属性：
_angle_ef_target
模块接口：
rate_controller_run()
angle_ef_roll_pitch_rate_ef_yaw_smooth()
frame_conversion_ef_to_bf(const Vector3f& ef_vector, Vector3f& bf_vector) 世界坐标转换到体坐标
frame_conversion_bf_to_ef (const Vector3f& bf_vector, Vector3f& ef_vector) 体坐标系转换到世界坐标系
relax_bf_rate_controller()
set_yaw_target_to_current_heading() 设置目标航线为当前方向
set_throttle_out()
angle_ef_roll_pitch_rate_ef_yaw_smooth()
set_throttle_out()
十八：AC_PosControl 位置控制模块
模块接口：
AC_PosControl(const AP_AHRS& ahrs, const AP_InertialNav& inav, const AP_Motors& motors, AC_AttitudeControl& attitude_control,AC_P& p_alt_pos, AC_P& p_alt_rate, AC_PID& pid_alt_accel, AC_P& p_pos_xy, AC_PID& pid_rate_lat, AC_PID& pid_rate_lon)
set_alt_target() 设置目标高度
set_dt(float delta_sec) 设置时间间隔
set_alt_max(float alt) 设置最大的高度
set_speed_z(float speed_down, float speed_up) 设置最大的爬升和下降速度
set_accel_z(float accel_cmss) 设置垂直加速度
set_alt_target_with_slew() 调整目标高度为最终目标
set_alt_target_from_climb_rate() 通过爬升速率、时间调整目标高度
get_alt_error() 返回高度误差
get_stopping_point_z() 计算停止点高度
init_takeoff() 起飞前初始化高度
calc_leash_length()
lean_angles_to_accel() 将角度转换为加速度
accel_to_lean_angles() 将水平方向的期望转换为roll、pitch的角度
init_vel_controller_xyz()
update_vel_controller_xyz()
init_xy_controller()
update_xy_controller()
update_z_controller()
属性：
_dt 时间间隔
_alt_max 距离home位置的最大高度
_speed_up_cms 最大的爬升速率cm/s
_speed_down_cms 最大的下降速率cm/s
_accel_z_cms 最大垂直加速度
_accel_cms 最大水平加速度
_speed_cms 最大水平速度
_leash; 水平误差
_leash_down_z 向下误差
_leash_up_z 向上误差
_throttle_hover 估算的油门输出值
_pos_target 目标位置
_last_update_xy_ms 上次执行update_xy_controller的时间
_last_update_z_ms 上次执行update_z_controller的时间
_last_update_vel_xyz_ms 上次执行update_vel_controller_xyz的时间
_roll_target 输出的roll值
_pitch_target 输出的pitch值
十九：AP_Motors / AP_MotorsMatrix 多旋翼和传统直升机混合的电机库
接口：
Init() 初始化
set_update_rate() 设置输出频率
set_frame_orientation()
enable() 安全开关
output() 输出数据的接口，设置各个电机的value
set_roll(int16_t roll_in) Motors的对外接口，AC通过该接口设置计算出来的值
set_pitch(int16_t pitch_in)
set_yaw(int16_t yaw_in)
set_throttle(int16_t throttle_in)
二十：AC_WPNav 在loiter等模式中使用
接口函数：
AC_WPNav(const AP_InertialNav& inav, const AP_AHRS& ahrs, AC_PosControl& pos_control)
init_loiter_target() 初始化当前位置和速度
update_loiter()
get_yaw()
get_roll()
get_pitch()
二十一：AC_Circle 在Circle模式中使用
设置飞机飞行的范围参数原点、半径，然后每次循环根据当前位置计算下一个点的roll、pitch、yaw
接口函数：
init(const Vector3f& center)
init()
update()
get_roll()
get_pitch()
get_yaw()
二十二：AP_Relay
二十三：AP_ServoRelayEvents
二十四：AP_Param 运行时参数管理,配置参数的读写
二十五：StorageManager/StorageAccess/PX4Storage
提供了对hal->Storage进行读写封装
二十六：AC_PID
PID算法库
主要接口函数：
get_p(float error) 计算比例放大分量
get_i(float error, float dt) 计算积分放大分量
get_d(float input, float dt) 计算微分放大分量
get_pi(float error, float dt) 计算比例放大和积分放大分量
get_pid(float error, float dt) 计算PID值
AC_P 计算比例放大分量的库
主要接口函数：
get_p(float error) 计算比例放大分量
AP_Mission：从eeprom（电可擦只读存储器）存储/读取飞行指令相关库
AP_Buffer：惯性导航时所用到的一个简单的堆栈（FIFO，先进先出）缓冲区
AP_Mount，AP_Camera, AP_Relay：相机安装控制库，相机快门控制库
保持飞机速度始终始终不超过设置的最小速度
API接口：
void enable(bool true_false)
bool enabled() //设置和判断控制速度功能是否可用
void test_pump(bool true_false) //主要用于控制速度功能测试
void set_pump_rate(float pct_at_1ms) //设置速度调整的参数
void update(); // 被循环调用，不停的判断飞机速度是否超过设置的最小速度。

如果超出，进行调整
二十七：AP_ServoRelayEvents
二十八：AC_Fence 围栏控制
实现了对飞机飞行范围的控制功能。

支持两种形式的范围控制、或者两者组合
1、AC_FENCE_TYPE_ALT_MAX 限制飞行高度
2、AC_FENCE_TYPE_CIRCLE 限制飞机在一定半径内
参考代码Fence.pde的fence_check()函数
主要接口函数及作用：
uint8_t check_fence(); // 根据配置内容对飞机的飞行高度或半径进行检查，判断有没有飞出围栏
float get_breach_distance(uint8_t fence_type) const; 获取指定限制模式下的最大限制距离uint8_t get_action()
二十九：RCChannel 对RCInput、RCOutPut的顶层封装
接口：
set_range() 设置输入的最大、最小及输入类型
set_range_out() 设置输出的最大、最小
允许用户对参数进行配置，设置每个通道的min/max/trim值，同时支持AUX通道函数，还可以对input、output进行比例缩放处理
calc_pwm() 计算出输出
output() 将输出应用到输出通道
set_pwm() 将从RCInout获取的输入值，根据设置的min、max进行转换
部分属性说明：
AP_Int16 radio_min; 遥控最小PWM脉冲宽度
AP_Int16 radio_trim; 遥控微调PWM脉冲宽度
AP_Int16 radio_max; 遥控最大PWM脉冲宽度
AP_Int8 _reverse; -1 反向 1 正向
AP_Int16 _dead_zone; 中点周围的死区
uint8_t _type;
int16_t _high;
int16_t _low;
三十：NavEKF
扩展卡尔曼滤波算法
Scheduler内部的循环
一：rc_loop()
从接收机读取用户输入执行间隔4个tick
Read_radio() 从hal中读取8个通道的值，分别设置到全局变量Parameters g的八个通道
read_control_switch() 调整飞行模式，执行对应模式的初始化
从hal的rcin读取，读取结果存储在g的rc 1~8通道
设置飞行模式、简单和超简单模式
输出项：
修改全局参数g
二：throttle_loop
执行间隔8个tick
获取当前高度、z轴速度，判断飞机是否着陆
输出项：
更新全局变量current_loc.alt climb_rate
三：update_GPS
gps获取位置信息执行间隔8个tick
更新gps数据，如果有新的gps数据到来
如果没有设置ap.home_is_set,设置ahrs.se_thome(),intertial_nav.setup_home_position(),compass.set_initial_location(gps.location(). lat, gps.location().lng); 设置全局变量scaleLongDown scaleLongUp
设置起始位置，每次gps.calculate_base_pos();
如果已经设置了ap.home_is_set，执行gps.calculate_base_pos();
输出项：
设置导航起始位置，compass、ahrs、nav
四：update_batt_compass
读取电池和指南针信息执行间隔40个tick
读取电池电流、电压，设置compass.set_current(battery.current_amps())
输出项：
throttle_integrator
五：read_aux_switches
检查模式开关通道，实现飞行模式的切换，通道7、通道8支持的功能执行间隔40个tick
do_aux_switch_function() (Switches.pde line65) 修改control_mode这个全局变量
六：arm_motors_check
电机解锁、加锁循环执行间隔40个tick
输出项：
auto_trim_counter = 250;
auto_disarming_counter = 0;
七：auto_trim
自动休正执行间隔40个tick
根据rc1 rc2通道的输入通过ahrs对飞机进行休正
ahrs.set_fast_gains(true);
ahrs.add_trim(roll_trim_adjustment, pitch_trim_adjustment, (auto_trim_counter == 0));
八：update_altitude
更新高度执行间隔40个tick
计算当前高度和速度
输出项：
baro_alt通过气压计计算高度和速度
baro_climbrate
sonar_alt声呐、红外传感器计算高度
九：run_nav_updates
与自动巡航有关执行间隔8个tick
获取经纬度信息
计算当前飞行模式下下一个点的距离、方向
计算当前位置与起始位置的距离和方向
输出项：
current_loc.lng
current_t
wp_distance
wp_bearing
home_distance
home_bearing
super_simple_cos_yaw
super_simple_sin_yaw
十：update_thr_cruise
执行间隔40个tick
输出项：
g.throttle_cruise
pos_control.set_throttle_hover(throttle_avg);
十一：three_hz_loop
执行间隔133个tick
检查飞机和地面站心跳是否正常，异常则自动切换飞行模式让飞机返回登录检查确保飞机不能飞出指定的高度和范围
十二：compass_accumulate
罗盘指南执行间隔8个tick
罗盘计算，更新信息
十三：barometer_accumulate
气压采样执行间隔8个tick
气压计计算，更新气压信息
十四：update_notify
执行间隔8个tick
各种指示灯、蜂鸣器、LED灯更新
十五：one_hz_loop
检查罗盘、惯导等设备状态执行周期400个tick
十六：ekf_dcm_check
检查ekf或dcm故障执行周期40个tick
检查ekf、dcm姿态控制模块是否正常，异常则进入失控保护
十七：crash_check
执行周期40个tick
Parachute的什么安全检查
十八：gcs_check_input
接收地面站消息执行周期8个tick
十九：gcs_send_heartbeat
向地面站发送心跳执行周期400个tick
二十：gcs_send_deferred
向地面站发送MSG_RETRY_DEFERRED消息执行周期8个tick
二十一：gcs_data_stream_send
向地面站发送数据流执行周期8个tick
二十二：update_mount
更新安装设备角度信息如相机等并触发相机计数器减一执行周期8个tick
二十三：ten_hz_logging_loop
写记录日志循环执行周期40 个tick
二十四：fifty_hz_logging_loop
写日志执行周期 5 个tick
二十五：perf_update
执行周期4000个tick
讲perf_info_loop_count、perf_info_max_time、perf_info_long_running复位0 pmTest1复位0
二十六：read_receiver_rssi
获取RSSI引脚电压值执行周期40个tick
结果保存在receiver_rssi
二十七：telemetry_send
执行周期80个tick
二十八：Fastloop循环
ahrs.update();
attitude_control.rate_controller_run()
控制飞行模式变更，油门输出
set_servos_4() arduCopter.pde 995行电机输出
update_flight_mode() arduCopter.pde 1002行根据选择的模式，执行相应的模式方法
其余：
Parameters.cpp 传递的参数结构包含了飞行模式
参数说明：
AP_Int16 sysid_this_mav mavlink 的版本信息
AP_Int16 sysid_my_gcs; gcs 唯一的ID，用来实现只接收该gcs的消息
AP_Int16 serial0_baud; USB控制台上使用的波特率
AP_Int16 serial1_baud; 第一个遥控器上使用的波特率
AP_Int16 serial2_baud; 第二个要控制上使用的波特率
AP_Int8 serial2_protocol; 遥控器2上使用的协议GCS Mavlink Frsky D-port AP_Int8 telem_delay; mavlink协议发送消息的一个时延值
AP_Int16 rtl_altitude; 返航高度，设置为0则保持目前高度返航
AP_Float sonar_gain;
AP_Int8 failsafe_battery_enabled; 电池电压、电流过低时是否引发失控保护0：禁用1：降落2：RTL返航
AP_Float fs_batt_voltage; 启动失控保护的电池电压值
AP_Float fs_batt_mah; 启动失控保护的电池电流值，0：禁用
AP_Int8 failsafe_gps_enabled; 控制gps异常时是否启用失控保护
0：禁用1：降落2：定高3：Land even from Stabilize
AP_Int8 failsafe_gcs;
0：禁用1：始终启用RTL 2：在自动模式下继续执行任务
AP_Int16 gps_hdop_good; GPS水平精度阀值，等于或低于该值认为ok AP_Int8 compass_enabled; 是否使用罗盘
AP_Int8 optflow_enabled; 0：Disable 1：Enable optical flow
AP_Int8 super_simple; 0，禁用超级简单模式
AP_Int16 rtl_alt_final; 飞机完成任务返回起飞点的最后阶段的高度，设置为0则降落
AP_Int8 rssi_pin; -1禁用，选择一个模拟器引脚接收RSSI电压
AP_Float rssi_range; 接收机RSSI电压范围
AP_Int8 wp_yaw_behavior; 设置飞控在执行任务和RTL时如何控制yaw的动作
0：机头不改变方向1：机头朝着下一个航点2：机头朝着下一个航点除了RTL 3：机头沿
着gps方向
AP_Int8 rc_feel_rp; roll、pitch、yaw的遥控手感0~100
AP_Int16 poshold_brake_rate; posHold flight mode's rotation rate during braking in deg/sec
AP_Int16 poshold_brake_angle_max; PosHold flight mode's max lean angle during braking in centi-degrees
AP_Int32 rtl_loiter_time; 最后下降悬停在home上方的时间（毫秒）
AP_Int16 land_speed;着陆最后阶段的下降速度cm/s
AP_Int16 pilot_velocity_z_max; 最大垂直速度cm/s
AP_Int16 pilot_accel_z; 垂直加速度控制高度
// Throttle
AP_Int16 throttle_min; 最小油门值将发送到电机以保持电机旋转
AP_Int16 throttle_max; 最大油门值将发送到电机以保持电机旋转
AP_Int8 failsafe_throttle; 油门失控保护开关
0：禁用1：启用总是RTL 2：启用自动模式继续任务3：启用总是降落
AP_Int16 failsafe_throttle_value; 油门失控的阀值
AP_Int16 throttle_cruise;
AP_Int16 throttle_mid;
AP_Int16 throttle_deadzone; 油门中位的死区
// Flight modes
AP_Int8 flight_mode1;
AP_Int8 flight_mode2;
AP_Int8 flight_mode3;
AP_Int8 flight_mode4;
AP_Int8 flight_mode5;
AP_Int8 flight_mode6;
AP_Int8 simple_modes; 简单模式
// Misc
AP_Int32 log_bitmask; 四字节映射的日志类型启用
AP_Int8 esc_calibrate; 控制飞控重启时是否进入电调校准，不要手动调整
AP_Int8 radio_tuning; 控制那些参数正在调整与发射器的频道6旋钮AP_Int16 radio_tuning_high; 通道6旋钮的参数的最大值将被应用到当前被调整的发射器上
AP_Int16 radio_tuning_low; 通道6旋钮的参数的最小值将被应用到当前被调整的发射器上
AP_Int8 frame_orientation; 控制混合电机的多轴飞行器，不能用在三轴或传统直升机上
AP_Int8 ch7_option; 超过1800PWM进行选择那些功能
AP_Int8 ch8_option;
AP_Int8 arming_check; 可以启用或禁用的接收机、加速度计、气压计、罗盘和GPS预先解锁检查
AP_Int8 land_repositioning;
AP_Float ekfcheck_thresh; 允许设置的最大可接受罗盘和速度变化
0：禁用0.8：默认1.0 舒适的
AP_Float dcmcheck_thresh; 允许设置的最大可接受的偏航误差
0：禁用0.8：默认1.0 舒适的
AP_Int16 rc_speed; RC通道的接收频率
// Acro parameters
AP_Float acro_rp_p; 设置ACRO或者SPORT飞行模式时，俯仰与横滚动作的P值，值越高，动作相应越快
AP_Float acro_yaw_p; 设置ACRO或者SPORT飞行模式时，偏航动作的P值，值越高，动作相应越快
AP_Float acro_balance_roll; 设置ACRO比率飞行模式时横滚角返回水平的比率
AP_Float acro_balance_pitch; 设置ACRO比率飞行模式时俯仰角返回水平的比率
AP_Int8 acro_trainer;
AP_Float acro_expo;
Ahrs_comp_beta
Ahrs_gps_gain 设置AHRS航姿系统对GPS的依存值，不能为0，一般飞机用默认或者1.0
AC_PID pid_rate_roll;
AC_PID pid_rate_pitch;
AC_PID pid_rate_yaw;
AC_PID pid_loiter_rate_lat;
AC_PID pid_loiter_rate_lon;
AC_P p_throttle_rate;
AC_PID pid_throttle_accel;
AC_PID pid_optflow_roll;
AC_PID pid_optflow_pitch;
AC_P p_loiter_pos;
AC_P p_stabilize_roll;
AC_P p_stabilize_pitch;
AC_P p_stabilize_yaw;
AC_P p_alt_hold;
飞行模式
一：stabilize_run 自稳模式********
g.rc_1 g.rc_2 g.rc_4 g.rc_3
roll pitch yaw throttle
首先通过get_pilot_desired_lean_angles()函数对roll、pitch进行比例缩放
1：Attitude.pde文件中的get_pilot_desired_lean_angles()函数功能roll、pitch输入范围（-4500~4500）
根据配置的aparm.angle_max（所有飞行模式中的最大倾斜角）参数值，对输入的roll、pitch 进行比例缩放
如果aparm.angle_max == 4500 / 0,则1:1缩放
否则，则aparm.angle_max / 4500 缩放
通过Attitude.pde文件中的get_pilot_desired_yaw_rate()函数对输入的yaw值乘以配置的acro_yaw_p（1~10）
Attitude.pde文件中的get_pilot_desired_throttle()函数对输入的油门值，throttle输入范围0~1000之间
对输入的油门值按照校准的min、max、mid进行计算，输出油门值
Attitude.pde文件中的get_smoothing_gain() 函数通过获取用户配置的遥控手感参数rc_feel_rp(0~100)
计算过程2.0 + rc_feel_rp / 10.0 ,输出值范围 2.0 ~12.0
2、接着通过
AC_AttitudeControl::angle_ef_roll_pitch_rate_ef_yaw_smooth(roll_angle,pitch_angle,yaw_rate_ ef, smoothing_gain)
方法输入：roll_angle,pitch_angle,yaw_rate_ef,smoothing_gain
方法处理逻辑：参考AC angle_ef_roll_pitch_rate_ef_yaw_smooth.vsdx
方法输出：_angle_ef_target、_angle_bf_error、_rate_bf_target、_rate_bf_desired
attitude_control.set_throttle_out(throttle_scaled, true);
计算包含补偿俯仰角的油门值，设置motor的_rc_throttle.servo_out
3、AC_AttitudeControl:: rate_controller_run()
rate_bf_to_motor_roll() 输入_rate_bf_target.x 输出新的roll值
根据pid计算roll值
rate_bf_to_motor_pitch() 输入_rate_bf_target.y 输出pich
rate_bf_to_motor_yaw() 输入_rate_bf_target.z 输出yaw
设置到motor的_rc_roll.servo_out、_rc_pitch.servo_out、_rc_yaw.servo_out
4、Motors的set_servos_4()方法
关于外部设备，GPS、气压计、陀螺仪等设备的如何影响飞控输出的分析
首先，简单的分析fast_loop循环，查找代码中使用到的其他模块数据：
Read_ANRS() ahrs数据更新
attitude_control.rate_controller_run();中通过_ahrs.get_gyro() 获取飞机在Roll、Pitch、Yaw的角度值，用于计算PID的差值
attitude_control. update_ef_roll_angle_and_error()中使用_ahrs.roll_sensor
attitude_control. update_ef_pitch_angle_and_error() 中使用_ahrs.pitch_sensor
attitude_control. update_ef_yaw_angle_and_error()中使用_ahrs.yaw_sensor
attitude_control.frame_conversion_ef_to_bf()中使用_ahrs.sin_pitch()、_ahrs.cos_pitch()、_ahrs.cos_roll()、_ahrs.sin_roll()
从以上分析来看，外设应该是通过影响_ahrs.roll_sensor、_ahrs.pitch_sensor、_ahrs.yaw_sensor 来影响飞控的输出。

那现在的关键是外设如何控制这三个值
而想改变这三个值只有一种渠道AP_AHRS::update_cd_values(void) 将这三个值和roll、pitch、yaw关联起来
_dcm_matrix 的计算通过_ins.get_gyro(i) //AP_AHRS_DCM::matrix_update(float _G_Dt)
计算出来
通过罗盘、_dcm_matrix计算roll、pitch、yaw
没有罗盘，则通过gps、_dcm_matrix计算roll、pitch、yaw
参考AP_AHRS_DCM::drift_correction_yaw(void)代码
AP_AHRS_NavEKF:: update()
首先保存之前的方向余弦矩阵的属性值，用在DCM计算漂移误差值校正
调用AP_AHRS_DCM::update()
传感器数据更新，获取传感器两次数据之间的间隔
matrix_update(delta_t)
根据时间间隔、陀螺仪速率_ins.get_gyro(i) 生成dcm方向余弦矩阵
输出：_dcm_matrix
normalize();
drift_correction(delta_t); 校准
check_matrix();
euler_angles(); 转换欧拉角
输入：_dcm_matrix、_trim
输出：roll、pitch、yaw、roll_senor、pitch_senor、yaw_senor
update_trig()
输入：_dcm_matrix
输出：_cos_pitch、_sin_pitch、_cos_yaw、_sin_yaw、_cos_pitch、_sin_pitch
根据roll、pitch、yaw更新_dcm_attitude
进入卡尔曼滤波
输出：roll、pitch、yaw、roll_sensor、pitch_sensor、yaw_sensor、_cos_pitch、_sin_pitch、_cos_yaw、_sin_yaw、_cos_pitch、_sin_pitch
二：LOITER 留待模式*********
使用AC_WPNav实现定点悬停
模式初始化Control_loiter.pde:: loiter_init() line 8行
检查GPS状态，初始化wp_nav 根据当前位置初始化定点位置、速度
参考AP_InertialNav::update(float dt) line 35行定点位置的高度信息来之baro气压计，定点位置的x、y坐标信息来之GPS
1、首先根据roll、pitch输入更新WP_NAV 的_pilot_accel_fwd_cms、_pilot_accel_rgt_cms成
员
根据配置的acro_yaw_p对yaw通道输入进行比例缩放处理
Value = input * acro_yaw_p
根据油门校准配置值对throttle输入进行比例缩放处理
2、wp_nav.set_pilot_desired_acceleration
wp_nav.update_loiter()
wp_nav根据输入的roll、pitch、yaw计算出新的roll、pitch、yaw值
AC_WPNav::calc_loiter_desired_velocity()
3、attitude_control.angle_ef_roll_pitch_rate_ef_yaw()
通过AC模块将wp_nav计算出来的roll、pitch、yaw值输出到motors模块
方法输入：wp_nav.get_roll(),wp_nav.get_pitch()、target_yaw_rate
方法输出：_rate_bf_target
4、pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt);
pos_control.update_z_controller();
通过将pos_control将油门输入输出到motor模块
三：acro_run 模式基于速率的控制
1、首先通过get_pilot_desired_angle_rates()方法根据acro_expo、acro_rp_p、acro_yaw_p、acro_trainer、aparm.angle_max配置对roll_in、pitch_in、yaw_in进行处理计算出roll_out、pitch_out、yaw_out
输入：roll_in、pitch_in范围-4500~4500、yaw_in
输出：roll_out、pitch_out、yaw_out
计算方法：
如果acro_expo <= 0, rate_bf_request.x = roll_in * g.acro_rp_p; rate_bf_request.y = pitch_in * g.acro_rp_p;
否则，保证acro_expo范围在小于等于1.0
rp_in = float(roll_in)/4500;
rp_in3 = rp_in*rp_in*rp_in;
rp_out = (g.acro_expo * rp_in3) + ((1 - g.acro_expo) * rp_in);
rate_bf_request.x = 4500 * rp_out * g.acro_rp_p;
rp_in = float(pitch_in)/4500;
rp_in3 = rp_in*rp_in*rp_in;
rp_out = (g.acro_expo * rp_in3) + ((1 - g.acro_expo) * rp_in);
rate_bf_request.y = 4500* rp_out * g.acro_rp_p;
rate_bf_request.z = yaw_in * g.acro_yaw_p
如果acro_trainer != ACRO_TRAINER_DISABLED：
计算rate_bf_level
int32_t roll_angle = wrap_180_cd(ahrs.roll_sensor);
rate_ef_level.x = -constrain_int32(roll_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_roll;
int32_t pitch_angle = wrap_180_cd(ahrs.pitch_sensor);
rate_ef_level.y = -constrain_int32(pitch_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_pitch;
rate_ef_level.z = 0;
如果acro_trainer == ACRO_TRAINER_LIMITED
if (roll_angle > aparm.angle_max){
rate_ef_level.x -= g.acro_balance_roll*(roll_angle-aparm.angle_max);
}else if (roll_angle < -aparm.angle_max) {
rate_ef_level.x -= g.acro_balance_roll*(roll_angle+aparm.angle_max);
}
if (pitch_angle > aparm.angle_max){
rate_ef_level.y -= g.acro_balance_pitch*(pitch_angle-aparm.angle_max);
}else if (pitch_angle < -aparm.angle_max) {
rate_ef_level.y -= g.acro_balance_pitch*(pitch_angle+aparm.angle_max);
}
rate_ef_level坐标从ef转到bf
attitude_control.frame_conversion_ef_to_bf(rate_ef_level, rate_bf_level);
if (g.acro_trainer == ACRO_TRAINER_LIMITED) {
rate_bf_request.x += rate_bf_level.x;
rate_bf_request.y += rate_bf_level.y;
rate_bf_request.z += rate_bf_level.z;
}else{
acro_level_mix = constrain_float(1-max(max(abs(roll_in), abs(pitch_in)), abs(yaw_in))/4500.0, 0, 1)*ahrs.cos_pitch();
rate_bf_level = rate_bf_level*acro_level_mix;
rate_limit = fabs(fabs(rate_bf_request.x)-fabs(rate_bf_level.x));
rate_bf_request.x += rate_bf_level.x;
rate_bf_request.x = constrain_float(rate_bf_request.x, -rate_limit, rate_limit);
rate_limit = fabs(fabs(rate_bf_request.y)-fabs(rate_bf_level.y));
rate_bf_request.y += rate_bf_level.y;
rate_bf_request.y = constrain_float(rate_bf_request.y, -rate_limit, rate_limit);
rate_limit = fabs(fabs(rate_bf_request.z)-fabs(rate_bf_level.z));
rate_bf_request.z += rate_bf_level.z;
rate_bf_request.z = constrain_float(rate_bf_request.z, -rate_limit, rate_limit);
}
最后rate_bf_request + rate_bf_level
roll_out = rate_bf_request.x;
pitch_out = rate_bf_request.y;
yaw_out = rate_bf_request.z;
2、对通道3，油门输入进行比例缩放
Attitude.pde文件中的get_pilot_desired_throttle()
3、AC_AttitudeControl::rate_bf_roll_pitch_yaw() 计算_rate_bf_target
attitude_control.set_throttle_out(pilot_throttle_scaled, false);
四：althold_run 定高模式
1、首先对根据配置的angle_max 对roll_in、pitch_in进行比例缩放
Out = in * （angle_max / 4500）
根据acro_yaw_p配置对yaw进行比例缩放
Out = in * acro_yaw_p
根据油门校准参数和死区配置对throttle进行比例缩放
get_pilot_desired_climb_rate(int16_t throttle_control)
然后：
attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
if (sonar_alt_health >= SONAR_ALT_HEALTH_MAX) {
target_climb_rate = get_throttle_surface_tracking(target_climb_rate, pos_control.get_alt_target(), G_Dt);
}
pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt);
pos_control.update_z_controller();
五：auto_run 模式
下包含八种状态：
Auto_TakeOff、Auto_WP、Auto_Land、Auto_RTL、Auto_CircleMoveToEdge、Auto_Circle、Auto_Spline、Auto_NavGuided
六：circle_run 绕圈模式
1、首先对yaw、throttle进行比例缩放
Yawout = yawIn * acro_yaw_p
2、circle_nav.update()
3、attitude_control.angle_ef_roll_pitch_rate_ef_yaw(circle_nav.get_roll(), circle_nav.get_pitch(),
target_yaw_rate)
4、pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt);
pos_control.update_z_controller();
七：guided_run 引导模式
包含Guided_TakeOff、Guided_WP、Guided_Velocity三种模式
八：LAND 降落模式
根据gps是否一样，判断land_gps_run或land_nogps_run
九：rtl_run 返航模式
十：ofloiter_run 定点模式
十一：drift_run漂移模式
十二：sport_run 运动模式
代码编译
1、打开PX4Console，打开ardupilot/ArduCopter目录如图所示：
2、执行make px4-v2 如图所示：
3、编译成功提示
编译生成文件路径
代码调试
Debugging with GDB
需要的硬件设备
1、BlackMagic probe
2、有JIAG（Joint Test Action Group）连接口的Pixhaek
BlackMagic probe 应该被连接到Pixhawk的JIAG 连接口软件
1、GDB 7.7 以上，里面包含python执行器
Installing GDB
Which gdb
Version
开始调试，调试命令
GDB需要Copter-v2.px4 和firmware.Elf 两个文件
调试命令
r 重置
b className::functionname 设置断点
ctrl –c
continue
show interrupted-thread 显示执行器停止地址info
disassemable
exit
JIAT based debugging
OpenOCD Based debugging
OpenOCD min Version 0.7.0
Build OpenOCD 下载地址/ cd openocd
./configure
make
Sudo make install
二：AP_HAL
抽象基类
AP_HAL::AnalogSource
AP_HAL::AnalogIn
AP_HAL::DigitalSource 数字信号值
AP_HAL::GPIO
AP_HAL::I2CDriver
AP_HAL::RCInput
AP_HAL::RCOutput
AP_HAL::Scheduler
AP_HAL::SPIDeviceDriver
AP_HAL::SPIDeviceManager
AP_HAL::HAL
AP_HAL::Storage
AP_HAL::BetterStream
AP_HAL::UARTDriver
AP_HAL::Semaphore 同步用的互斥锁
AP_HAL::Util
三：AP_HAL_PX4
PX4::PX4AnalogSource
PX4::PX4AnalogIn
PX4::PX4DigitalSource 数字信号，提供了读写求反等方法
PX4::PX4GPIO 通用的IO处理方法
PX4::PX4RCInput 读取输入
PX4::PX4RCOutput 输出
PX4::PX4Scheduler
提供了获取时间、时间处理线程同步RCInput、RCOutPut、Storage、PX4UARTDriver读写的频率
以及注册RCInput、RCOutPut、Storage、PX4UARTDriver读写注册功能
micros64()获取当前绝对时间
micros()获取启动时间
PX4::PX4Storage 存储设备的读写
PX4::PX4UARTDriver 设备驱动
HAL_PX4
主要包含的成员对象
AP_HAL::UARTDriver* uartA;
AP_HAL::UARTDriver* uartB; GPS
AP_HAL::UARTDriver* uartC; AP_BoardConfig
AP_HAL::UARTDriver* uartD; AP_BoardConfig
AP_HAL::UARTDriver* uartE; GPS
AP_HAL::I2CDriver* i2c;
AP_HAL::SPIDeviceManager* spi;
AP_HAL::AnalogIn* analogin; AP_BattMonitor
AP_HAL::Storage* storage;
AP_HAL::UARTDriver* console;
AP_HAL::GPIO* gpio;
AP_HAL::RCInput* rcin;
AP_HAL::RCOutput* rcout;
AP_HAL::Scheduler* scheduler;
AP_HAL::Util* util;。