共计 93751 个字符，预计需要花费 235 分钟才能阅读完成。
VESC+Ardupilot
C/STM32/RTOS/C++/QT/MATLAB/
ArduPilot的安装与编译
系统安装
- 安装VM,安装Linux系统，我使用Debian10
- 安装后需要vmtools，以方便虚拟机与主机之间文件共享
vmtools安装
会安装在<code>/media/cdrom0 vmtools</code>
提取.tar.gztar.gz包
<code>tar ztvf yourtargzfile |grep fileyouwant，</code>
<code>tar zxvf</code>
用超级用户运行 <code>vmware-install.pl</code>
su与sudo
sudo的安装
1:先使用su进入root,
2: apt-get insatll sudo
3:vi /etc/sudoerd 在root下面添加name ALL=(ALL:ALL) ALL
4:apt源的设置<code>/etc/apt/source.list</code>
环境搭建
安装gcc,git,make，gcc-arm-none-eabi，rsync
sudo apt-get update
sudo apt-get install gcc git gcc-arm-none-eabi make rsync
编译命令是arm-none-eabi-gccclone固件
git clone https://github.com/ArduPilot/ardupilot.git
cd ardupilot
git submodule update --init --recursive错误1:git出错
git提交 new mode old mode
这是因为文件的访问模式出现问题，因此使用下面命令
<code>git config --add core.filemode false</code>
编译错误2:
from future import standard_library
ImportError: No module named future
pip 是 Python 包管理工具，该工具提供了对Python 包的查找、下载、安装、卸载的功能。
//安装future包
<code>sudo apt-get install python-pip</code>
<code>sudo pip install future</code>
出错3
'/usr/bin/git', 'submodule', 'status', '--recursive', '--', ' returned 128
显示HEAD提交的SHA1值，git rev-parse HEAD ，执行命令git rev-parse HEAD 出错找不到head
这个错误是因为git clone时，submodule出错，主目录下的.git下也有modules，各个module下有branch,hooks,info,logs,objects,refs文件夹
在主目录的各个module的文件夹下有.git文件
解决方法:
有好的网络，用git clone下载编译
./waf list_boards
./waf configure --board Pixhawk4
./waf copter在主目录下会有build目录，在该目录下找到对应的板子
./waf clean 保持配置信息，清楚当前板子上的对象
./waf distclean 清除板子的所有，包含配置的信息
./waf --targets bin/arducopter --upload
./waf list测试例子
./waf configure --board Pixhawk4
./waf build --target examples/AP_Common在SITL下使用sketch下的example
./waf list | grep 'examples'
./waf configure --board sitl
./waf build --target examples/AP_Motors_test
./build/sitl/examples/AP_Motors_test -M quad -C如何成功运行“GenerateEquations24states.m“来生成矩阵
1将InertialNav-master\derivations\OptimiseAlgebra.m用下列文件中内容替换
function [SymExpOut,SubExpArray] = OptimiseAlgebra(SymExpIn,SubExpName)
% Loop through symbolic expression, identifying repeated expressions and
% bringing them out as shared expression or sub expressions
% do this until no further repeated expressions found
% This can significantly reduce computations
syms SubExpIn SubExpArray;
SubExpArray(1,1) = 'invalid';
index = 0;
f_complete = 0;
while f_complete == 0
index = index + 1;
% change variable naming scheme to avoid recent Matlab warnings
%更改变量命名方案以避免最近的 Matlab 警告
SubExpIn = [SubExpName,'_l_',num2str(index),'_r_'];
SubExpInStore{index} = SubExpIn;
[SymExpOut,SubExpOut] = subexpr(SymExpIn,SubExpIn);
% Fix crash when no sub-expression can be found
%当没有子表达式发现时，修正崩溃
if isempty(SubExpOut)
break;
end
for k = 1:index
if SubExpOut == SubExpInStore{k}
f_complete = 1;
end
end
if f_complete || index > 100
SymExpOut = SymExpIn;
else
SubExpArray(index,1) = SubExpOut;
SymExpIn = SymExpOut;
end
end2打开Matlab视窗,选择InertialNav-master\modes\为工作路径
3运行addpath('../scripts/','../TestData','../derivations')
4复制GenerateEquations24states.m中的内容到Matlab视窗 或直接打GenerateEquations24states.m来运行
5 等待运行结束，在InertialNav-master\derivations下输出对应的m和c文件
APM的代码结构
APM下的目录有AntennaTracker，ArduCopter，ArduPlane，ArduSub，benchmark，docs,libraries,mk,modules,rover,tests,Tools
modules下有chibios，gbenchmark，gtest，libcanard，mavlink，uavcan，waf
==libraries\AP_HAL 下基本都是抽象基类
在\libraries\AP_HAL_ChibiOS下实现虚函数==
ArduPilot的基本结构可以分为5部分
- vehicle代码
- 共享的库
- 硬件抽象层(AP_HAL)
- 工具目录
- 额外支持的代码(如：mavlink，dronekit)
Vehicle 代码
vehicle目录是顶层目录，它为每种vehicle定义了固件，目前共有5种vehicle类型，分别是
Plane，Copter，Rover，Sub，AntennaTracker。虽然在不同vehicle类型间有许多共同的元素
但是他们是不同的，现在我们仅详细的描述Copter的代码结构。每个vehicle目录下都有一个wscript
文件，该文件描述了编译与依赖
共享库
共享库由四种vehicle类型共享,共享库包含传感器驱动，位置估计(既EKF)，控制代码(如pid，)
AP_HAL
硬件抽象层AP_HAL是让我们如何使ArduPilot便携于各种不同的平台，
有一个在libraries/AP_HAL目录下的顶层AP_HAL,它定义了接口，让其余的代码有特殊的板子特点
然后有一个AP_HAL_XXX 目录对应每种板子类型 ，例如，AP_HAL_Linux 对应基于linux的板子
工具目录
工具目录是各种杂项支持的目录，如tools/autotest提供了 autotest.ardupilot.org 网站后的自动测试架构
tools/Replay 提供日志重播功能
额外支持的代码
在一些平台，我们需要额外的代码来支持其他的功能或支持板子。
目前额外的代码有：使用在ArduPilot上的uavcan CANBUS的实现，mavlink协议和代码产生器，chibios，gtest,gbenchmark,uavcan,waf
请注意 这些额外的代码是作为git submodule出现的
ArduPilot 目录
目录由Copter，Plane和Rover共享
以下是高层的库和他们的函数列表
核心库：
AP_AHRS-- 使用DCM或者EKF的高度估计
AP_Common --包含所有sketch和库的核心
AP_Math--各种数学函数，尤其是用来向量操作的
AC_PID--pid控制库(比例-积分-微分)
AP_InertialNav --惯性导航库，用于融合加速度输入，gps，和气压数据
AC_AttitudeControl--高度控制， ArduCopter’s的控制库包含各种高度函数，基于pid的位置控制，
AC_WPNav--航点导航库
AP_Motors--多旋翼和传统直升机的电机混合
RC_Channel--来把从APM_RC的pwm输入与输出转换为内部单位，如角度
AP_HAL, AP_HAL_ChibiOS, AP_HAL_Linux--来实现硬件抽象层,来对上层代码呈现一个相似的接口，
因此更容易移植到其他板子上
传感器库:
AP_InertialSensor-读陀螺仪和加速度的数据，进行矫正，然后以标准单位提供数据(deg/s, m/s)给主代码和其他函数库
AP_RangeFinder--sonar和红外测距传感器库
AP_Baro--气压接口函数库
AP_GPS--gps 接口函数库
AP_Compass--3轴罗盘接口函数库
AP_OpticalFlow--光流传感器接口函数库
其他的库
AP_Mount, AP_Camera, AP_Relay--照相机挂载控制库，照相机快门控制库
AP_Mission--存储/索引任务命令从eeprom
AP_Buffer--简单的FIFO缓存，用于惯性导航
APM系统的入口点
\ArduCopter\copter.cpp
Copter copter;
AP_Vehicle& vehicle = copter;
AP_HAL_MAIN_CALLBACKS(&copter);
把该宏展开就是
int main(int argc, char* const argv[])
{
hal.run(argc, argv, &copter);
return 0;
} \ArduCopter\copter.cpp 下定义hal
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
\libraries\AP_HAL\HAL.h
class AP_HAL::HAL
{
struct Callbacks
{
virtual void setup() = 0;
virtual void loop() = 0;
};
struct FunCallbacks : public Callbacks
{
FunCallbacks(void (*setup_fun)(void), void (*loop_fun)(void));
void setup() override { _setup(); }
void loop() override { _loop(); }
private:
void (*_setup)(void);
void (*_loop)(void);
};
virtual void run(int argc, char * const argv[], Callbacks* callbacks) const = 0;
}HAL是纯虚函数，他的run函数需要子类来实现。
const AP_HAL::HAL& hal是引用，此处会动态绑定hal.run()就会调用子类实现的run()函数
static AP_HAL::HAL::Callbacks* g_callbacks;
//此处是按照Ardunino的写法，即setup() 和loop()
\libraries\AP_HAL_ChibiOSHAL_ChibiOS_Class.cpp
HAL_ChibiOS定义了run
void HAL_ChibiOS::run(int argc, char * const argv[], Callbacks* callbacks) const
{
g_callbacks = callbacks;
//Takeover main
main_loop(); //接管main
}==因此hal.run(argc, argv, &copter)就是运行这里的run()==
g_callbacks=copter;
接下来分析main_loop()
{
/*
switch to high priority for main loop
*/
//为main转到最高优先级
chThdSetPriority(APM_MAIN_PRIORITY);
peripheral_power_enable();
hal.uartB->begin(38400);
hal.uartC->begin(57600);
hal.analogin->init();
hal.scheduler->init();
//此处调用copter的setup();
g_callbacks->setup();
while (true)
{
//此处调用copter的loop()
g_callbacks->loop();
}
}class vehicle
{
/***在vehicle初始化时，setup()被调用一次，用来初始化vehicle对象和vehicle包含的
对象。AP_HAL_MAIN_CALLBACKS创造出main()函数，引用一个包含setup()和loop()的对象。
vehicle不应该来override setup()函数，但是子类特殊的初始化可以在init_ardupilot()
中实现，它在setup()中被调用
***/
// setup() is called once during vehicle startup to initialise the
// vehicle object and the objects it contains. The
// AP_HAL_MAIN_CALLBACKS pragma creates a main(...) function
// referencing an object containing setup() and loop() functions.
// A vehicle is not expected to override setup(), but
// subclass-specific initialisation can be done in init_ardupilot
// which is called from setup().
void setup(void) override final;
//HAL::Callbacks的实现
// HAL::Callbacks implementation.
void loop() override final;
}setup()和loop()是不能被子类override的，如果子类需要特殊的初始化，需要在init_ardupilot()
中实现，该函数会在vehicle::setup()中调用
这样整个系统就跑起来了
APM的启动
APM支持多种vehicle，我们主要APM 4.1版本 下的copter为例进行解读
APM的版本在\ArduCopter\version.h中可以查看
define THISFIRMWARE "ArduCopter V4.1.0-dev"
必备的宏
在\libraries\AP_HAL\AP_HAL_Main.h
ifndef AP_MAIN
define AP_MAIN main
endif
该宏主要是各个libraries进行测试的主函数
#define AP_HAL_MAIN() \
AP_HAL::HAL::FunCallbacks callbacks(setup, loop); \
extern "C" { \
int AP_MAIN(int argc, char* const argv[]); \
int AP_MAIN(int argc, char* const argv[]) { \
hal.run(argc, argv, &callbacks); \
return 0; \
} \
}该宏主要是copter的主函数
#define AP_HAL_MAIN_CALLBACKS(CALLBACKS) extern "C" { \
int AP_MAIN(int argc, char* const argv[]); \
int AP_MAIN(int argc, char* const argv[]) { \
hal.run(argc, argv, CALLBACKS); \
return 0; \
} \
}
主要继承关系
AP_HAL是名字空间
\libraries\AP_HAL\AP_HAL_Namespace.h
namespace AP_HAL
{
/* Toplevel pure virtual class Hal.*/
/*HAL是顶层的纯虚类**/
class HAL;
}<code>\libraries\AP_Vehicle\AP_Vehicle.h</code>
<code>class AP_Vehicle : public AP_HAL::HAL::Callbacks</code>
<code>ArduCopter\Copter.h</code>
<code>class Copter : public AP_Vehicle</code>
<code>\libraries\AP_HAL_ChibiOS\AP_HAL_ChibiOS_Namespace.h</code>
namespace ChibiOS
{
}<code>\libraries\AP_HAL_ChibiOS\HAL_ChibiOS_Class.h</code>
class HAL_ChibiOS : public AP_HAL::HAL
{
}APM的radio
init_ardupilot()
{
init_rc_in();
rc().init();
init_rc_out();s
}
init_rc_in()
{
channel_roll;
channel_pitch;
channel_throttle;
channel_yaw;
default_dead_zones();
}
rc().init()
{
channel(i)->ch_in = i;
init_aux_all();
}
The PPM encoder allows to encode up to 8 PWM (pulse width modulated)
signals into one PPM (pulse position modulation) signal.
往Copter中添加新的参数
添加参数到主代码
步骤一
在Parameters.h中的Parameters类的enum中找个空位，然后按照如下来添加你自己的参数。尽量添加参数到分享一个相似函数的参数处，最差的情况就是添加到“Misc” 段的最后。请确保你正在添加的段没有满，你可以通过计数此段的参数数量来检查段是否满，确保本段的最后一个参数小于下一个段的第一个参数。不要添加参数到组的中部，这样会造成现存的参数改变。不要使用有“deprecated”和“remove”注释的slot，因为一些用户仍然让eeprom中的这些旧的参数有默认值
因此你的新参数默认值可能会奇怪的设置步骤二
在Parameters类中声明一个变量，变量的类型可能是下列枚举值(AP_Int8, AP_Int16, AP_Float, AP_Int32 and AP_Vector3)
(注意，目前无符号变量是不支持的)
变量的名字应该和在enum中定义的名字是一样的，除去"k_param_"前缀步骤三
添加变量声明到Parameters.cpp中的var_info表中。@Param ~ @User之间的注释用于GCS来对用户显示并限制用户可以设置的值。例如:MY_NEW_PARAMETER限制为16个字符。步骤四
添加参数的默认值到config.h
你已经完成了，新的参数可以在主代码中以g.my_new_parameter的方式访问s添加参数到库
参数也可以按照以下步骤添加到函数库中，本次以AP_Compass库函数作为例子
步骤一
添加新的类变量到顶级.h文件。(例如Compass.h)可能的类型包含AP_Int8, AP_Int16, AP_Float, AP_Int32 and AP_Vector3f
也可以为你想要的变量添加默认值步骤二
添加变量到cpp文件的var_info表，包括@Param ~ @Increment注释允许GCS来为使用者显示描述信息和控制在GCS端设置的最大与最小值
当添加新的参数时，注意：
slot比前一个变量大1
参数的名字长度小于16，包含被添加的对象的前缀，例如，compass对象的前缀是“COMPASS_”。参数在库函数可以作为_my_new_lib_parameter被访问。注意我们让参数为“protected”。
所以不能从类外访问
如果我们想让它public，然后就可以从主代码用“compass._my_new_lib_parameter”访问步骤三
除过添加定义到.cpp文件，添加var_info声明到新库函数类的.h的public部分步骤四
如果类是一个完全新加到代码中(而不是现存的类如AP_Compass)
它应该被添加到Parameters.cpp文件中的主vehicle的var_info表中
(在var_info表中的顺序是不重要的)
步骤五
如果一个类是完全新加到代码中，也添加 k_param_my_new_lib到Parameters.h的枚举。my_new_lib是GOBJECT声明的第一个实参。改变参数的类型
时不时地参数需要改变或重新命名，ArduPilot有能力允许这个从版本到版本间管理，所以更新总是保存用户配置的参数，参数从eeprom的插槽中删除
所以如果占据的slot没有改变，无论是什么名字，那么参数也不会改变。然而如果类型需要改变，然后参数需要移到新的slot，现存的slot
被保存来阻止意外的配置，为了配置保存，它需要被复制并从旧的slot转到新的slot
步骤一
改变现在的参数index到一个不使用的index。并添加注释到旧的slot需要保存步骤二
设置AP_PARAM_KEY_DUMP定义为1 AP_Param.h
改变这里的延迟从1ms到2ms AP_Param.cpp
移除 这段#if / #endif system.cpp
开始sitl运行旧代码，所有参数和它的魔术数字会显示
从Copter’s Parameters.cpp文件 复制-粘贴ArduPilot中的数据类型
enum ap_var_type
{
AP_PARAM_NONE = 0,
AP_PARAM_INT8,
AP_PARAM_INT16,
AP_PARAM_INT32,
AP_PARAM_FLOAT,
AP_PARAM_VECTOR3F,
AP_PARAM_GROUP
};
#define AP_PARAMDEF(_t, _suffix, _pt) typedef AP_ParamT<_t, _pt> AP_ ## _suffix;
AP_PARAMDEF(float, Float, AP_PARAM_FLOAT); // defines AP_Float
AP_PARAMDEF(int8_t, Int8, AP_PARAM_INT8); // defines AP_Int8
AP_PARAMDEF(int16_t, Int16, AP_PARAM_INT16); // defines AP_Int16
AP_PARAMDEF(int32_t, Int32, AP_PARAM_INT32); // defines AP_Int32
AP_PARAMDEF(int32_t, Int32, AP_PARAM_INT32); // defines AP_Int32typedef AP_ParamT<int32_t,3> AP_Int32
所以AP_Int32是AP_ParamT模板类的实例化
// Base class for variables. 变量的基本类
/// Provides naming and lookup services for variables. 为变量提供命名和查找服务
class AP_Param
{
}
/// Template class for scalar variables.标量的模板类
/// Objects of this type have a value, and can be treated in many ways as though they
/// were the value.
///这种类型的对象有一个值，并且可以通过多种方式将其视为值
/// @tparam T The scalar type of the variable
//// 变量的标量类型
/// @tparam PT The AP_PARAM_* type
///
template<typename T, ap_var_type PT>
class AP_ParamT : public AP_Param
{
protected:
T _value;
}
/// Template class for non-scalar variables.
///非标量变量的类模板
/// Objects of this type have a value, and can be treated in many ways as though they
/// were the value.
///这种类型的对象有一个值，并且可以通过多种方式将其视为值
/// @tparam T The non-scalar type of the variable
//非标量型的变量
/// @tparam PT AP_PARAM_* type
///
template<typename T, ap_var_type PT>
class AP_ParamV : public AP_Param
{
protected:
T _value;
}
// declare a non-scalar type
//声明一种非标量类型
// this is used in AP_Math.h
//用于AP_Math.h
// _t is the base type
//_t是基本类型
// _suffix is the suffix on the AP_* type name
//_suffix是在AP_*类型名字上的后缀
// _pt is the enum ap_var_type type
//_pt是ap_var_type枚举值
#define AP_PARAMDEFV(_t, _suffix, _pt) typedef AP_ParamV<_t, _pt> AP_ ## _suffix;
AP_PARAMDEFV(Vector3f, Vector3f, AP_PARAM_VECTOR3F);
typedef AP_ParamV<Vector3f,5> AP_Vector3f
typedef Vector3<float> Vector3f;
template <typename T>
class Vector3
{
public:
T x, y, z;
}
APM函数库例子Sketches
探索代码的第一步是使用库下的例子Sketches，按照arduino的习惯，我们为大多数的库有例子sketches
sketch是一个由cpp写的主程序
了解ArduPilot中的库api和约定是理解代码的关键，所以使用库函数例子sketch是一种好的开始方式
作为开始，你应该先阅读，build，运行以下库函数的例子sketches
libraries/AP_GPS/examples/GPS_AUTO_test
libraries/AP_InertialSensor/examples/INS_generic
libraries/AP_Compass/examples/AP_Compass_test
libraries/AP_Baro/examples/BARO_generic
libraries/AP_AHRS/examples/AHRS_Test例如，以下将build和安装AP_GPS例子sketch到Pixhawk板子上。
cd $ARDUPILOT_HOME # the top-level of an AruPilot repository
./waf configure --board=Pixhawk1
./waf build --target examples/INS_generic --uploadwaf可以列出它可以build的例子
cd $ARDUPILOT_HOME
./waf list | grep 'examples'一旦你上传了例子，通过连接控制台，就可以看到输出。控制台是与你的板子相关的。在Pixhawk
板子上，它是usb连接器。所以只需用你的串口程序联接usb设备
例如，你已经安装了mavproxy，可以在linu上使用下面命令联接Pixhawk
<code>mavproxy.py --setup --master /dev/serial/by-id/usb-3D_Robotics_PX4_FMU_v2.x_0-if00</code>
使用–setup 选项让mavproxy进入原始串口模式，而不是处理MAVLink的模式。
对于例子sketches，这些就是你需要
运行例子在SITL模式运行协议解码sketch：
某些sketches也可以运行在SITL。例如
cd $ARDUPILOT_HOME # the top-level of an AruPilot repository
./waf configure --board sitl
./waf build --target examples/RCProtocolDecoder为了开始sketch，直接运行它
<code>./build/sitl/examples/RCProtocolDecoder -M quad -C</code>
理解例子sketch代码
当你阅读例子sketch代码时(如:GPS_AUTO_test代码)，首先你会注意到一些陌生的代码
它声明一个hal的引用变量；代码是粗糙的同时也没有好的注释
hal引用
每个需要使用AP_HAL的文件都需要声明一个hal引用，它可以访问AP_HAL::HAL对象，他提供了
所以硬件相关的函数，包含打印信息到控制台，休息和I2C与SPI总线交流
真正的hal变量是隐藏在与板子相关的AP_HAL_XXX函数库。
每个文件中的引用提供了便捷的方式来得到hal，
常用的hal函数有：
hal.console->printf() 打印字符串
AP_HAL::millis() and AP_HAL::micros() 从启动到现在的时间
hal.scheduler->delay() and hal.scheduler->delay_microseconds() 睡眠一段时间
hal.gpio->pinMode(), hal.gpio->read() and hal.gpio->write() 访问gpio的引脚
hal.i2c 访问I2C
hal.spi 访问SPI可以去看下libraries/AP_HAL 目录下的所有在HAL上的函数
setup()与loop()函数
你会注意到每个sketch都有一个setup()函数与loop()函数，当板子启动时，setup()被调用。真正的
调用来自每个板子的HAL，所以main()函数是隐藏在HAL，板子相关的启动完成后调用setup() 。
setup()函数仅调用一次，它用来初始化库函数，可能会打印"hello"旗号来说明它启动了
setup()完成后，loop()函数继续被调用(通过AP_HAL中的main代码)
sketch的主要工作是在loop()函数中
注意setup()/loop()的这种安排只是复杂板子的冰山一角
可能看到ArduPilot是单线程，实际上下面有许多在运行。同时在板子上有线程，实际上由续贷哦实时线程开始
AP_HAL_MAIN() 宏
你会注意到在每个sketch的底部有一行下面代码
<code>AP_HAL_MAIN();</code>
这是一个HAL宏，来产生必须的代码来声明C++主函数。还有板子的初始化代码，你基本不需要担心
它是如何工作的。如果你很好奇，那就在AP_HAL_XXX目录下看下#define，通常在AP_HAL_XXX_Main.h文件中。
你会注意到例子sketch是粗糙的，也没什么注释，这就是你贡献代码的机会。
当你阅读例子sketches时，同时探索他是怎么工作的，你可以添加注释到代码来解释API，然后提交pull request，这样其他人就可以从你的学习中受益
理解ArduPilot的线程
一旦你学完了ArduPilot库函数的基础，是时候了解ArduPilot是如何处理线程的
setup()/loop()结构继承与Arduino，这可能使你看起来ArduPilot是单线程系统
但事实上并不是。
ArduPilot中的线程方法依赖于它构建的板子，一些板子(如APM1，APM2)就不支持线程，只是使用了定时器与
回调函数。一些板子(如:PX4,Linux)就支持丰富的带有实时优先级的posix线程模型，他们广泛的用于ArduPilot
有许多和线程相关的关键概念，你需要在ArduPilot中理解
- 定时器回调函数
- HAL特定的线程
- 驱动特定的线程
- ardupilot驱动与平台驱动
- 平台特定的线程用于任务
- AP_Scheduler系统
- 信号量
- 无锁数据结构
定时器回调函数
每种平台都在AP_HAL中提供了1KHZ的定时器,在ArduPilot中的任何代码都可以注册一个定时器函数，然后以1kHZ的频率调用
所有注册的定时函数依次被调用。这种非常原始的机制使它方便移植，且非常有用。
注册一个定时回调函数通过调用hal.scheduler->register_timer_process()
hal.scheduler->register_timer_process(AP_HAL_MEMBERPROC(&AP_Baro_MS5611::_update));
这个例子是来自MS5611气压驱动
The AP_HAL_MEMBERPROC() 宏提供了一种封装C++成员函数作为回调函数实参的方式(即绑定对象与函数指针)
当一部分代码想要小于1khz时，它应该管理自己的last_called变量，然后当没有足够的时间时立即返回
你可以使用hal.scheduler->millis() 和 hal.scheduler->micros()函数来得到自从系统启动后的时间
你现在就可以修改现存的例子sketch来添加一个定时回调，让定时器增加一个技术，然后在loop()函数每秒打印出这个定时值修改你的代码所以每隔25ms增加一次计数值
HAL 特定线程
在支持真正线程的平台上，AP_HAL会创建许多线程来支持基本操作。例如，在Pixhawk上下面HAL特例线程被创建
UART线程，来读与写UARTs
定时器线程 ，支持1KHZ的定时器函数
IO线程，支持写SD卡，EEPROM，FRAM
在每个AP_HAL实现的目录里看下Scheduler.cpp，什么线程被创建，每个线程的实时优先级时什么
如果你有Pixhawk那么你也可以设置一个调试控制台线缆联接到nsh控制台，57600连接，当已经联接上时候
试着输入ps命令，你会看到一些类似下面的信息
ID PRI SCHD TYPE NP STATE NAME
0 0 FIFO TASK READY Idle Task()
1 192 FIFO KTHREAD WAITSIG hpwork()
2 50 FIFO KTHREAD WAITSIG lpwork()
3 100 FIFO TASK RUNNING init()
37 180 FIFO TASK WAITSEM AHRS_Test()
38 181 FIFO PTHREAD WAITSEM <pthread>(20005400)
39 60 FIFO PTHREAD READY <pthread>(20005400)
40 59 FIFO PTHREAD WAITSEM <pthread>(20005400)
10 240 FIFO TASK WAITSEM px4io()
13 100 FIFO TASK WAITSEM fmuservo()
30 240 FIFO TASK WAITSEM uavcan()在这个例子中，你可以看到AHRS_Test线程，在libraries/AP_AHRS/examples/AHRS_Test目录下，你也可以
看到定时器线程(优先级181)，uart线程(优先级60)，io线程(优先级59)
也可以看到px4io，fmuservo，uavcan，lpwork，hpwork和idle任务
其他的AP_HAl按照自己的需要，也或多或少有些线程
通常使用线程是提供驱动一种方式来调度慢的任务，不打断主自动驾驶飞行代码。
AP_Terrain库需要能够文件化io到microSD卡(来存取和索引地形数据),它使用的方式是
调用hal.scheduler->register_io_process()函数
hal.scheduler->register_io_process(AP_HAL_MEMBERPROC(&AP_Terrain::io_timer));
设置AP_Terrain::io_timer的函数将被调用，这就是和board相关的io线程。意味着它是一个低实时优先级和适用于存储io任务
类似这种慢的io任务不应该在定时器线程来调用，因为他会对处理高速传感器数据 造成延迟
驱动特例的线程
也可能对某个驱动以特定的方式来创建驱动特例的线程来支持异步处理
当前你仅可以以与平台依赖的方式创建驱动特例的线程，因此如果你的驱动是用于一种autopilot板子，这种方式就是恰当的
如果你想让他运行在多种AP_HAL目标下，你有两种选择
你可以使用register_io_process()和register_timer_process()调度器调用使用现存的定时器与io线程
你也可以添加提供一种通用方式来创造线程在多种AP_HAL目标上的新的hal接口
驱动特例的线程的例子是在linux平台下的ToneAlarm 线程 参考 AP_HAL_Linux/ToneAlarmDriver.cpp
ardupilot驱动与平台驱动
在ArduPilot中，你可能会注意到一些复制的驱动，例如我们有MPU6000 驱动在libraries/AP_InertalSensor/AP_InertialSensor_MPU6000.cpp 另一个MPU6000 驱动在PX4Firmware/src/drivers/mpu6000
这种复制的理由是 PX4 项目已经提供了测试好的硬件驱动为板子，我们享受这种与px4的合作关系
AP_Scheduler系统
理解ArduPilot 线程和任务的另一方面是AP_Scheduler系统
AP_Scheduler函数库用来分开与主vehicle线程的时间,
当提供一些简单的机制来控制时间是如何用于每个操作(在AP_Scheduler中叫任务)
它工作的方式是对每种vehicle实现的loop()函数包含如下的代码
等待新的IMU采样来到达
调用一系列的任务在每个IMU采样之间
它是一种表驱动的调度器，每一种vehicle 类型有一个AP_Scheduler::Task表，为了学习它是如何工作的
请看AP_Scheduler/examples/Scheduler_test.cpp sketch
如果你看文件的里面，你会看到一个小的由3个调度任务组成的表，每个任务有两个数，表看起来如下
static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
{ ins_update, 1, 1000 },
{ one_hz_print, 50, 1000 },
{ five_second_call, 250, 1800 },
};
在函数名字后的第一个数是调用频率，单位由ins.init() 调用来控制的，在这个例子里
ins.init()使用50hz，所以每个调度步伐是20ms，也就是说ins_update()20ms调用一次
one_hz_print()函数是每50次，也就是一秒一次，
five_second_call()是250次，也就是5秒一次
第二个数字是函数期望的最大时间，这个主要是用来避免造成调用，除非有足够的时间来运行这个函数。
当scheduler.run()被调用时，它传递对这个任务总的可用时间
如果对这个任务最差时间意味着在时间用光前它不合适，以至不能被调用
另一个注意点是ins.wait_for_sample()，它是在ArduPilot中驱动调度的节拍器
它阻塞执行主vehicle线程直到新的IMU采样可用。
IMU 采样之间的时间是由传递给ins.init()的实参决定的
注意在AP_Scheduler表中的任务必须有以下属性
它们不能被阻塞(除了ins.update())
当飞行时，它们不能调用休眠函数 (自动驾驶就像真的pilot，不应该在飞行时睡觉)
they should have predictable worst case timing你可以修改Scheduler_test例子来添加你自己的任务来运行，尝试添加可以做这些功能的任务
读气压计，读罗盘，读gps，更新AHRS且打印roll/pitch
read the barometer
read the compass
read the GPS
update the AHRS and print the roll/pitch信号量
当你有许多线程时，你需要确保由两个线程共享的数据结构以阻止腐败的方式更新
在ArduPilot中有三种原理方法可以实现：信号量，无锁的数据结构，px4 ORB
AP_HAL 信号量包裹了在特殊平台可用的信号量系统，也为互斥访问提供了简单的机制
例如，I2C驱动可以要求I2C总线信号量来确保每次只有一个I2C设备可以。
看下在libraries/AP_Compass/AP_Compass_HMC5843.cpp下的 hmc5843驱动
寻找调用get_semaphore(),看下所有使用它的地方，试试你是否能知道为什么需要它
无锁数据结构
ArduPilot 代码也包含使用无锁数据结构来避免需要信号量的例子，这会比信号量更有效
ArduPilot中的两个使用无锁数据结构的例子是
在libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp下的_shared_data结构体
在许多地方使用了环形队列，一个好的例子是libraries/DataFlash/DataFlash_File.cpp
去看下这两个例子，来证明它是可以安全访问的。
对于DataFlash_File，看下使用_writebuf_head和_writebuf_tail变量。
创建一个可以代替在多处实现分离环形队列的 通用的环形队列类
如果你想贡献，就pull request
ArduPilot的串口和控制台
ArduPilot中的许多部分依赖于UARTS，他们用来debug输出，遥测，GPS模块
了解通过HAL来和UARTs对话将有助于理解许多ArduPilot代码
8个串口
ArduPilot HAl当前定义了8个UARTs，HAL自己并没有为UARTs定义任何特殊角色，
但是ArduPilot的许多部分假设他们用于特殊的功能:
uartA --是终端(通常是USB，运行MAVLink遥测)
uartB--第一个GPS
uartC--主要的遥测
uartD--副遥测
uartE--第二个GPS
uartF--用户配置
uartG--用户配置
uartH--用户配置
如果你在用ArduPilot HAL写你自己的sketch，你可以使用这些UARTs做为任何你喜欢的目的.
如果你试图使用上述的分配，它将允许你更容易适合现存的代码
你可以通过SERIALn_PROTOCOL参数来改变UART的角色.
允许的参数值是1:None, 1:MAVLink1, 2:MAVLink2, 3:Frsky D, 4:Frsky SPort, 5:GPS, 7:Alexmos Gimbal Serial,
8:SToRM32 Gimbal Serial, 9:Rangefinder, 10:FrSky SPort Passthrough (OpenTX), 11:Lidar360, 13:Beacon, 14:Volz servo out,
15:SBus servo out, 16:ESC Telemetry, 17:Devo Telemetry, 18:OpticalFlow, 19:RobotisServo,
20:NMEA Output, 21:WindVane, 22:SLCAN Search code for 1_PROTOCOL to get updated list of uart roles.
去<code>libraries/AP_HAL/examples/UART_test</code>看下这个例子，它打印hello信息到第一个UARTs
在你的板子上尝试并用usb适配器看是否会得到输出
并试图改变波特率
Debug控制台
历史上，除过基础的5个UARTs。在其他平台还有额外的debug控制台可以使用.
最近Debug控制台被定向到USB。
当运行SITL 时，debug被定向到终端，当USB被定向到5760端口
如果你的板子有HAL_OS_POSIX_IO集(核查AP_HAL/AP_HAL_Boards.h)，
然后添加::printf()和其他stdio函数到UART_test
如果::printf不工作，可能是你的文件顶端没有“#include <stdio.h>”，那就加上它
也可以使用hal.console->printf()来指定USB端口
UART 函数
每一个UART都有一些基本的io函数，主要的函数是
printf--格式化输出
printf_P--用progmem 字符串的格式化输出(在AVR板子上节省内存)
println--打印和回车
write--写一些字节
read--读一些字节
available--检查是否有字节在等待
txspace--坚持多少出去的缓存空间可用
get_flow_control--检查是否uart有流控制
Mavlink
EKF
本文介绍了扩展卡尔曼滤波器 (EKF) 算法，该算法用于基于速率陀螺仪、加速度计、罗盘（磁力计）、GPS、空速和气压测量来估计车辆位置、速度和角方向。
它包括算法概述和有关可用调整参数的信息。
概述
更快的处理器（如 Pixhawk 上的处理器）的可用性使得能够实施更先进的数学算法来估计飞行器的方向、速度和位置。 已开发出扩展卡尔曼滤波器 (EKF) 算法，该算法使用速率陀螺仪、加速度计、罗盘、GPS、空速和气压测量来估计飞行器的位置、速度和角方向。 该算法在 AP_NavEKF2 和 AP_NavEKF3 库中实现，并基于此处记录的初始工作：链接
与DCM使用的简单的互补滤波器算法的和Copter的惯性导航相比，EKF 的优势在于，通过融合所有可用的测量结果，可以更好地拒绝具有重大错误的测量结果，从而使vehicle不易受到单个传感器的故障的影响。
EKF 算法的另一个特点是它能够估计车辆罗盘读数的偏移量，还可以估计飞机和直升机以及漫游车应用的地球磁场。 这使得它对罗盘校准误差的敏感度低于当前的 DCM 和 INAV 算法。
它还支持来自可选传感器（例如光流和激光测距仪）的测量，用于辅助导航。
调整参数
ACC_P_NSE
加速度噪音
AP_GROUPINFO("ACC_P_NSE", 25, NavEKF2, _accNoise, ACC_P_NSE_DEFAULT),ENABLE
使能EKF2
AP_GROUPINFO_FLAGS("ENABLE", 0, NavEKF2, _enable, 1, AP_PARAM_FLAG_ENABLE),GPS_TYPE
GPS类型
AP_GROUPINFO("GPS_TYPE", 1, NavEKF2, _fusionModeGPS, 0),VELNE_M_NSE
GPS 水平速度测量噪声
AP_GROUPINFO("VELNE_M_NSE", 2, NavEKF2, _gpsHorizVelNoise, VELNE_M_NSE_DEFAULT),VELD_M_NSE
ArduPilot的Info与GroupInfo
Info和GroupInfo// the Info and GroupInfo structures are passed by the main
// program in setup() to give information on how variables are
// named and their location in memory
//Info 和 GroupInfo在main中的setup()被传递，来给出变量是如何命名和在内存中的位置信息
class AP_Param::struct Info
{
uint8_t type; // AP_PARAM_*
const char *name;
uint16_t key; // k_param_*
const void *ptr; // pointer to the variable in memory
union
{
const struct GroupInfo *group_info;
const struct GroupInfo **group_info_ptr; // when AP_PARAM_FLAG_INFO_POINTER is set in flags
const float def_value;
};
uint16_t flags;
};Copter类中的数据成员定义
copter.h
{
// Global parameters are all contained within the 'g' class.
Parameters g;//237行
ParametersG2 g2;
static const AP_Param::Info var_info[];//584行
}Paramter类和ParametersG2类的定义
//parameter.h
// Global parameter class.
//全局参数类
class Parameters
{
// The enumeration defined here is used to ensure that every parameter
定义在这里的枚举类型用来确保每个参数或者参数组有唯一的id
// or parameter group has a unique ID number. This number is used by
// AP_Param to store and locate parameters in EEPROM.
这个数是由AP_Param使用来在EEPROM中存储和定位
enum
{
// Layout version number, always key zero.
//
k_param_format_version = 0,
k_param_software_type, // deprecated
k_param_ins_old, // *** Deprecated, remove with next eeprom number change
k_param_ins, // libraries/AP_InertialSensor variables
k_param_NavEKF2_old, // deprecated - remove
k_param_NavEKF2,
k_param_g2, // 2nd block of parameters
k_param_NavEKF3,
k_param_BoardConfig_CAN,
k_param_osd,
}
}
/*
2nd block of parameters, to avoid going past 256 top level keys
第二块参数，来避免超过顶级256个keys
*/
class ParametersG2
{
}Info的使用
enum ap_var_type {
AP_PARAM_NONE = 0,
AP_PARAM_INT8,
AP_PARAM_INT16,
AP_PARAM_INT32,
AP_PARAM_FLOAT,
AP_PARAM_VECTOR3F,
AP_PARAM_VECTOR6F,
AP_PARAM_MATRIX3F,
AP_PARAM_GROUP
};
//parameter.cpp 38行
const AP_Param::Info Copter::var_info[]
{
GSCALAR(format_version, "FORMAT_VERSION", 0),
ASCALAR(angle_max, "ANGLE_MAX", DEFAULT_ANGLE_MAX),
GOBJECT(relay, "RELAY_", AP_Relay),
GOBJECTPTR(wp_nav, "WPNAV_", AC_WPNav),
GOBJECTN(mode_auto.mission, mission, "MIS_", AP_Mission),
}
GSCALAR的使用
#define GSCALAR(v, name, def) { copter.g.v.vtype, name, Parameters::k_param_ ## v, &copter.g.v, {def_value : def} }
{copter.g.format_version.vtype,"FORMAT_VERSION",Parameters::k_param_format_version,&copter.g.format_version,{def_value:0}}
g是Parameters类的对象，它有format_version成员， format_version是 AP_Int16 类对象，AP_Int16 类有vtype成员变量
{}是无名union的初始化方式，用0来初始化无名union中的def_value成员ASCALAR的使用
copter:
{
// key aircraft parameters passed to multiple libraries
//关键飞行器参数，用来传给多个库
AP_Vehicle::MultiCopter aparm;
}
class AP_Vehicle
{
struct MultiCopter
{
AP_Int16 angle_max;
};
}
#define ASCALAR(v, name, def) { copter.aparm.v.vtype, name, Parameters::k_param_ ## v, (const void *)&copter.aparm.v, {def_value : def} }
ASCALAR(angle_max, "ANGLE_MAX", DEFAULT_ANGLE_MAX),
{copter.aparm.angle_max.vtype,"ANGLE_MAX",Parameters::k_param_angle_max,(const void *)&copter.aparm.angle_max,{def_value:DEFAULT_ANGLE_MAX}
//unio中的def_value值用DEFAULT_ANGLE_MAX来初始化
GOBJECT的使用
#define GOBJECT(v, name, class)
{ AP_PARAM_GROUP, name, Parameters::k_param_ ## v, (const void *)&copter.v, {group_info : class::var_info} }
GOBJECT(relay, "RELAY_", AP_Relay),
{AP_PARAM_GROUP,"RELAY_",Parameters::k_param_relay,(const void *)&copter.relay,{group_info : AP_Relay::var_info}}
//无名union中的成员const struct GroupInfo *group_info 用AP_Relay::var_info来初始化GOBJECTPTR的使用
#define GOBJECTPTR(v, name, class)
{ AP_PARAM_GROUP, name, Parameters::k_param_ ## v, (const void *)&copter.v, {group_info : class::var_info}, AP_PARAM_FLAG_POINTER }
GOBJECTPTR(wp_nav, "WPNAV_", AC_WPNav),
{AP_PARAM_GROUP,"WPNAV_",Parameters::k_param_wp_nav,(const void *)&copter.wp_nav,{group_info : AC_WPNav::var_info},AP_PARAM_FLAG_POINTER}GOBJECTN的使用
class copter
{
ModeAuto mode_auto
}
class ModeAuto
{
AP_Mission mission
{
FUNCTOR_BIND_MEMBER(&ModeAuto::start_command, bool, const AP_Mission::Mission_Command &),
FUNCTOR_BIND_MEMBER(&ModeAuto::verify_command, bool, const AP_Mission::Mission_Command &),
FUNCTOR_BIND_MEMBER(&ModeAuto::exit_mission, void)};
}
}
//Parameters.cpp
#define GOBJECTN(v, pname, name, class)
{ AP_PARAM_GROUP, name, Parameters::k_param_ ## pname, (const void *)&copter.v, {group_info : class::var_info} }
GOBJECTN(mode_auto.mission, mission, "MIS_", AP_Mission),
{AP_PARAM_GROUP,"MIS_",Parameters::k_param_mission,(const void *)&copter.mode_auto.mission,{group_info : AP_Mission::var_info}}
GroupInfo
GroupInfo定义
class AP_Param::struct GroupInfo
{
uint8_t type; // AP_PARAM_*
uint8_t idx; // identifier within the group
const char *name;
ptrdiff_t offset; // offset within the object
union
{
const struct GroupInfo *group_info;
const struct GroupInfo **group_info_ptr; // when AP_PARAM_FLAG_INFO_POINTER is set in flags
const float def_value;
};
uint16_t flags;
};GroupInfo的使用
/*
2nd group of parameters
*/
const AP_Param::GroupInfo ParametersG2::var_info[] =
{
AP_GROUPINFO("WP_NAVALT_MIN", 1, ParametersG2, wp_navalt_min, 0),
AP_SUBGROUPPTR(button_ptr, "BTN_", 2, ParametersG2, AP_Button),
}
需要的宏定义
#define AP_VAROFFSET(type, element) (((ptrdiff_t)(&((const type *)1)->element))-1)
// find the type of a variable given the class and element
#define AP_CLASSTYPE(clazz, element) ((uint8_t)(((const clazz *) 1)->element.vtype))
#define AP_GROUPINFO_FLAGS(name, idx, clazz, element, def, flags)
{ AP_CLASSTYPE(clazz, element), idx, name, AP_VAROFFSET(clazz, element), {def_value : def}, flags }AP_GROUPINFO
//AP_Param.h
// declare a group var_info line
//声明一组var_info
#define AP_GROUPINFO(name, idx, clazz, element, def) AP_GROUPINFO_FLAGS(name, idx, clazz, element, def, 0)
AP_GROUPINFO_FLAGS("WP_NAVALT_MIN",1,ParametersG2,wp_navalt_min,0,0)
||||||
{AP_CLASSTYPE(ParametersG2,wp_navalt_min),1,"WP_NAVALT_MIN",AP_VAROFFSET(ParametersG2, wp_navalt_min),{def_value:0},0}
|||||||||
{((uint8_t)(((const ParametersG2*)1)->wp_navalt_min.vtype)),1,"WP_NAVALT_MIN",(((ptrdiff_t)(&((const ParametersG2*)1)->wp_navalt_min))-1),{def_value:0},0}
//wp_navalt_min是ParametersG2类的数据成员，AP_SUBGROUPPTR
//声明一个指向 组var_info中的子组实体的指针
// declare a pointer subgroup entry in a group var_info
#define AP_SUBGROUPPTR(element, name, idx, thisclazz, elclazz)
{ AP_PARAM_GROUP, idx, name, AP_VAROFFSET(thisclazz, element), { group_info : elclazz::var_info }, AP_PARAM_FLAG_POINTER }
AP_SUBGROUPPTR(button_ptr, "BTN_", 2, ParametersG2, AP_Button),
{AP_PARAM_GROUP, 2, "BTN_",(((ptrdiff_t)(&((const ParametersG2*)1)->button_ptr))-1),{group_info: AP_Button::var_info},AP_PARAM_FLAG_POINTER}
//button_ptr是ParametersG2类的数据成员
ArduPilot的传感器驱动
ArduPilot支持来自各制造厂商的各种传感器，一个清晰的例子可以在测距仪的列表中发现(sonars,lidars),
本页试图解释传感器驱动是如何写的以及是如何集成到vehicle代码中的
支持的协议
I2C, SPI, UART (aka Serial) and CANBUS (in particular UAVCAN) 协议是支持的.
如果你想写一个新的驱动，你需要参考传感器的datasheet来决定应该使用哪种协议，
I2C
一个主机，许多从机
一种适合于短距离通信的相当简单的协议
总线运行在100KHZ或400KHZ，但是数据速率相对于其他协议是低的
需要4个pin(VCC, GND, SDA, SCL)
SPI
一个主机，一个从机
20mHZ速度意味着，相对I2C,他的速度是很快的
只适用于短距离
至少需要5个pin(VCC, GND, SCLK, Master-Out-Slave-In, Master-In-Slave-Out)和一个从选择pin
串口
一个主机，一个从机
基于字符的协议,相比于i2c和spi，是长距离的通信方式
速度在57Kbps ~ 1.5Mbps
至少4个pin(vcc,gnd,tx,rx),还有可选的2个pin(CTS:清除发送,CTR:清除接受)
can总线与UAVCAN
多主模式，当需要时任何节点都可以发起传输
基于包的传输协议，适于长距离
高速，典型的速度是1 Mb(然而只有50%的总线比率可以没有主碰撞的真正的使用)
至少3个Pin，(GND, CAN HI, CAN LO).可选的是VCC可以作为电源结点
点对点拓扑，星形或stubs拓扑是不建议的。
终端是需要在每个总线的尾部
前端与后端分离
关于传感器驱动架构最重要的概念是前端和后端分离
刚启动时候，前端基于自动探测传感器(探测一个已知I2C地址的回应)或者使用定义的_TYPE参数
前端管理一个指向每一个后端的指针，后端通常在一个叫_drivers[]的数组里
用户设置的参数通常在前端
驱动代码是如何和什么时候运行的
上图显示了一个放大了的ardupilot架构图，左上角的蓝色框显示传感器驱动的后端是如何运行在后端线程的。
从传感器来的原始数据被收集，然后转为标准单位，之后存于驱动的缓存
vehicle代码的主线程是正常运行，通过驱动前端的方法来访问最新可用的数据。
例如为了计算最新的高度估计，AHRS/EKF可能从传感器的前端来拉取最新的加速度，陀螺仪，罗盘信息
图片是通用的，对于使用I2C或SPI的驱动，他们必须运行在背景线程，以至于与传感器的高速通信不会影响主循环的性能。
但是对于使用串口的驱动，在主线程中可以安全的使用，因为串口驱动自身在后台收集数据并包含一个缓冲器，
Vehicle代码与前端例子
下面的例子展示了Copter vehicle代码是如何从测距仪驱动拉取数据，Copter代码以20hz的调度器调用vehicle的read_rangefinder()函数
下面是这种方法的图，最后一版可以在sensors.cpp中看到
rangefinder.update()是一个在驱动前端调用的方法
下面是测距仪驱动的前端更新函数，这使驱动程序有机会在主线程中进行任何可能需要的常规处理
每个后端的更新方法都依次调用
串口后端例子
UART/Serial Back-End Example
接下来是使用串口协议的后端更新方法，正如使用wiki中写的那样，串口测距仪可以连接到任何飞行器的串口，但是用户必须指定是哪个串口
和串口波特率，要用SERIALX_BAUD和SERIALX_PROTOCOL参数
在串口驱动的代码里，它首先通过来寻找上述的参数设置的serial_manager类来寻找哪个串口是用户想要的
每次驱动后端的update()被调用时，它调用get_reading方法，这个方法会检测是否有新的字符到达，并解码数据
正如上面说的那样，因为串口实现了自己的缓冲区，处理任何从sensor来的数据是运行在main线程里
没有将在I2C 和 SPI驱动中调用"register_periodic_callback"的函数
I2C后端例子
这个例子展示了轻量级I2C驱动的后端例子，在这种情况下，前端得到I2C的地址并在初始化时传递给后端
后端的初始化方法然后注册它的定时器方法，它将以20hz的频率调用
在定时器方法中，调用get_reading()来从传感器读取数据，然后把距离转换为厘米
SPI后端例子
这个例子展示了包括陀螺仪，加速度，和罗盘的MPU9250的后端
在初始化期间，前端得到spi总线并传给后端，
在初始化时，start()被调用来配置传感器，它使用信号量确保在相同的spi总线上没有其他spi设备干扰
_read_sample注册后，调度频率是1000hz,
注意，没有必要在_read_sample中来释放或获得信号量
因为它是周期回调代码的一部分
_block_read方法显示数据是如何从传感器的寄存器得到的
其他的建议
当写一个传感器驱动时，不要包含任何的等待或睡眠代码，
因为这样会延迟主线程或正在使用总线的后台线程
如果一个新的库要添加，一定要在vehicle目录下的wscript文件中添加它
(例如:/ardupilot/ArduCopter/wscript),这是为了把它链接到最终的二进制
EKF2
EKF2优点
1它可以为每个IMU运行单独的EKF2，从而更有可能从IMU故障中恢复
2:如果有错误，在磁力计间切换
3：估计gyro缩放因子，在高速率操纵时，可以提高准确度
4它可以在启动时同时估计陀螺仪偏移和方向，并且不依赖DCM算法来确定其初始方向。这使其非常适合在陀螺仪尚未校准时从移动平台飞行。
5:在飞行时处理大的gyro 偏差改变
6;从坏的传感器数据快速恢复
7提供了相当平滑的输出
8相当准确
9使用更小的运行电源
10当检查通过时使用GPS开始，尔不是等待vehicle电机arm
如何实现这个:
1:不是试图直接估计四元数方位，它估计错误旋转向量并应用校准到来自惯性导航等式的四元数.这在有大角度误差时更好，因为它避免了与线性化相关的误差
2:新的EKF运行在延时的时区，因此当测量融合时，他们做 使用来自相同时刻的测量，过滤状态和协方差矩阵
老的EKF使用简单的方法--延迟的测量使用延迟状态来融合，但是协方差矩阵是来自当前时刻，因此减少精度
--然后使用互补滤波器将延迟滤波器状态向前预测到当前时间范围，该滤波器消除时间延迟并滤除测量融合时发生的状态突然变化。
---这种方法的灵感来自 Ali Khosravian 所做的输出预测器工作
--这在计算上比在每次更新时使用缓冲的 IMU 数据向前缠绕状态要便宜得多，但准确性稍低。 传统 EKF 通过在下一次测量中逐步应用状态校正来平滑状态校正，这会降低滤波器的稳定性
--数学和代码的进一步估计来提高已经引入的速度
--带固定倾斜的简单的罗盘yaw角度融合方法已经添加，用于地面。
或者当磁场干扰阻止使用高准确度的3轴6状态磁力计融合技术
使用EKF2
1:通过设定EK2_ENABLE = 1，使能新的EKF，EKF2将并行运行和log。
不会由控制循环来使用
2:为了在控制循环中使用,设置AHRS_EKF_TYPE=2
3：多个IMU的使用是由 EK2_IMU_MASK 参数设置的
只使用IMU1,设置EK2_IMU_MASK =1
只使用IMU2，设置EK2_IMU_MASK=2
使用IMU1和IMU2，设置EK2_IMU_MASK=3，并通过设置EKF_ENABLE=0来关闭老的EKF
4设置完参数后，需要重新启动
EKF2 日志数据
EKF2的数据可以在NKF1和NKF5 log包中发现
NKF1和NKF4包包含带实例数的每个EKF实例的信息。实例数是通过EKF2_IMU——MASK参数设置的.
NKF5包 有EKF实例的光流信息，是飞行控制的主要
可用的EKF2日志数据列于下表.一些显示飞行数据的图已经包含，
这些数据通过Pixhawk并有下列参数改变后重启 来记录
EK2_ENABLE = 1 (turns on EKF2)开启EKF2
EK2_IMU_MASK = 3 (EKF2来运行两个实例, one for IMU1 (MPU6000) and one for IMU2 (LDG20H + LSM303D)
AHRS_EKF_TYPE = 2 (告诉飞控系统使用EKF2)
LOG_BITMASK = 131071 从启动时候log 50hz数据
过滤器状态估计
NKF1[0] NKF1[1]包含由飞行控制系统使用的输出
roll.pitch
yaw
velocity North,East
Velocity 和Position Derivative Down位置衍生下降
Position North,East,Down
XYZ gyro bias
NKF2[0] NKF2[1]包含额外的状态信息
z 加速度bias
X.Y.Z gyro 缩放因子
VWN,VWE:wind velocity North,East
MN.ME.MD:地磁场N,E,D
MX,MY,MZ-体磁场X,Y,Z
Filter innovation
NKF3[0]包含关于过滤器innovation的信息
innovation是由EKF2预测的测量值和传感器返回值的差,
越小的innovation表明越小的sensor错误。
因为IMU数据用于预测，坏的IMU数据对所有测量会造成大的innovation
1IVN,IVE:GPS速度innovation North，East
IVD GPS速度innovation Down
IPN,IPE:GPS位置innovation North，East
IPD：气压位置innovation Down
IMX,IMY,IMZ:磁力计innovation X,Y,Z
IYAW:罗盘yaw innovation
IVT:真的空速innovation
光流和测距仪融合
NKF5 包含有关用于飞行控制的 EK2 实例的光流融合的信息
normInnov-光流innovation方差测试率
FIX,FIY:光流X和Y轴innovation
AFI:光流地形高度估计innovation
HAGL:估计的高于地面的高度
meaRng:由测距仪测的距离
调整参数:
EKF2 参数已经过调整，以在传感器误差的准确性和鲁棒性之间提供折衷。 通过进一步调整，可能会进一步提高性能
如果你有关于调整filter的问题，发表在论坛上，带上log文件和
ekf2项
新的EKF参数以EKF2_前缀开始
new usage
clear all;
reset(symengine)
syms dax day daz 'real' % IMU delta angle measurements in body axes - rad IMU delta 角度测量
syms dvx dvy dvz 'real' % IMU delta velocity measurements in body axes - m/sec IMU delta 速度测量
syms q0 q1 q2 q3 'real' % quaternions defining attitude of body axes relative to local NED 定义体轴相对于局部 NED 的姿态的四元数
syms vn ve vd 'real' % NED velocity - m/sec NED 速度
syms pn pe pd 'real' % NED position - m NED位置
syms dax_b day_b daz_b 'real' % delta angle bias - rad delta角度偏差
syms dax_s day_s daz_s 'real' % delta angle scale factor delta角度缩放因子
syms dvz_b dvy_b dvz_b 'real' % delta velocity bias - m/sec delta速度偏差
syms dt 'real' % IMU time step - sec
syms gravity 'real' % gravity - m/sec^2
syms daxNoise dayNoise dazNoise dvxNoise dvyNoise dvzNoise 'real' % IMU delta angle and delta velocity measurement noise
%IMU delta角度和delta速度测量噪音
syms vwn vwe 'real' % NE wind velocity - m/sec NED风速
syms magX magY magZ 'real' % XYZ body fixed magnetic field measurements - milligauss XYZ体固定磁场
syms magN magE magD 'real' % NED earth fixed magnetic field components - milligauss NED地面固定磁场
syms R_VN R_VE R_VD 'real' % variances for NED velocity measurements - (m/sec)^2
syms R_PN R_PE R_PD 'real' % variances for NED position measurements - m^2
syms R_TAS 'real' % variance for true airspeed measurement - (m/sec)^2 真实空速测量的方差
syms R_MAG 'real' % variance for magnetic flux measurements - milligauss^2 磁通量测量的方差
syms R_BETA 'real' % variance of sidelsip measurements rad^2 侧滑测量的方差
syms R_LOS 'real' % variance of LOS angular rate mesurements (rad/sec)^2
syms ptd 'real' % location of terrain in D axis
syms rotErrX rotErrY rotErrZ 'real' % error rotation vector in body frame 在体frame中的错误旋转向量
syms decl 'real' % earth magnetic field declination from true north
syms R_DECL R_YAW 'real' % variance of declination or yaw angle observation
syms BCXinv BCYinv 'real' % inverse of ballistic coefficient for wind relative movement along the x and y body axes
syms rho 'real' % air density (kg/m^3)
syms R_ACC 'real' % variance of accelerometer measurements (m/s^2)^2
syms Kaccx Kaccy 'real' % derivative of X and Y body specific forces wrt componenent of true airspeed along each axis (1/s)
%% define the state prediction equations
%定义状态预测等式
% define the measured Delta angle and delta velocity vectors
%定义测量的Delta角度和Delta速度向量
dAngMeas = [dax day daz];
dVelMeas = [dvx dvy dvz];
% define the IMU bias errors and scale factor
%定义IMU偏差和缩放错误
dAngBias = [dax_b day_b daz_b];
dAngScale = [dax_s day_s daz_s];
dVelBias = [0 0 dvz_b];
% define the quaternion rotation vector for the state estimate
%为状态估计定义四元数旋转向量
estQuat = [q0 q1 q2 q3];
% define the attitude error rotation vector, where error = truth - estimate
%定义海拔错误旋转向量 错误=真实-估计
errRotVec = [rotErrX rotErrY rotErrZ];
% define the attitude error quaternion using a first order linearisation
%使用一阶线性来定义姿态错误四元数
errQuat = [1 0.5*errRotVec];
% Define the truth quaternion as the estimate + error
%定义真正的四元数作为估计+错误
truthQuat = quatmultiply(estQuat, errQuat);
% derive the truth body to nav direction cosine matrix
%真体导出到nav DCM
Tbn = Quat2Tbn(truthQuat);
% define the truth delta angle
% ignore coning compensation as these effects are negligible in terms of
% covariance growth for our application and grade of sensor
%定义真正delta角度
dAngTruth = dAngMeas.*dAngScale - dAngBias - [daxNoise dayNoise dazNoise];
% define the attitude update equations
%定义高度更新等式
% use a first order expansion of rotation to calculate the quaternion increment
% acceptable for propagation of covariances
deltaQuat = [1
0.5*dAngTruth(1)
0.5*dAngTruth(2)
0.5*dAngTruth(3)
] ;
truthQuatNew = quatmultiply(truthQuat,deltaQuat);
% calculate the updated attitude error quaternion with respect to the previous estimate
%计算相对于先前估计的更新姿态误差四元数
errQuatNew = QuatDivide(truthQuatNew,estQuat);
% change to a rotaton vector - this is the error rotation vector updated state
%改变到旋转向量
errRotNew = 2 * [errQuatNew(2) errQuatNew(3) errQuatNew(4)];
% Define the truth delta velocity -ignore sculling and transport rate
% corrections as these negligible are in terms of covariance growth for our
% application and grade of sensor
%定义真正的delta速度
dVelTruth = dVelMeas - dVelBias - [dvxNoise dvyNoise dvzNoise];
% define the velocity update equations
% ignore coriolis terms for linearisation purposes
%定义速度更新等式
vNew = [vn ve vd] + [0 0 gravity]*dt + Tbn*dVelTruth;
% define the position update equations
%定义位置更新等式
pNew = [pn pe pd] + [vn ve vd]*dt;
% define the IMU error update equations
%定义IMU错误更新等式
dabNew = [dax_b day_b daz_b];
dasNew = [dax_s day_s daz_s];
dvbNew = dvz_b;
% define the wind velocity update equation
%定义风速更新等式
vwnNew = vwn;
vweNew = vwe;
% define the earth magnetic field update equations
%定义地球磁场更新等式
magNnew = magN;
magEnew = magE;
magDnew = magD;
% define the body magnetic field update equations
%定义体磁场更新等式
magXnew = magX;
magYnew = magY;
magZnew = magZ;
% Define the state vector & number of states
%定义状态向量&状态数目
stateVector = [errRotVec vn ve vd pn pe pd dax_b day_b daz_b dax_s day_s daz_s dvz_b magN magE magD magX magY magZ vwn vwe];
nStates=numel(stateVector);
% Define vector of process equations
%过程等式的向量
newStateVector = [errRotNew vNew pNew dabNew dasNew dvbNew magNnew magEnew magDnew magXnew magYnew magZnew vwnNew vweNew];
% derive the state transition matrix
%派生出状态转换矩阵
F = jacobian(newStateVector, stateVector);
% set the rotation error states to zero
%设置旋转错误状态到0
F = subs(F, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
[F,SF]=OptimiseAlgebra(F,'SF'); U1
clear all
reset(symengine)
syms dax day daz 'real'
syms dvx dvy dvz 'real'
syms vn ve vd 'real'
syms pn pe pd 'real'
syms rotErrX rotErrY rotErrZ 'real'
syms q0 q1 q2 q3 'real'
syms dax_s day_s daz_s 'real'
syms dvz_b dvy_b dvz_b 'real'
syms dax_b day_b daz_b 'real'
syms daxNoise dayNoise dazNoise dvxNoise dvyNoise dvzNoise 'real'
syms dt 'real'
syms gravity 'real'
dAngMeas = [dax day daz];
dVelMeas = [dvx dvy dvz];
dAngScale = [dax_s day_s daz_s];
dAngBias = [dax_b day_b daz_b];
dVelBias = [0 0 dvz_b];
estQuat = [q0 q1 q2 q3];
errRotVec = [rotErrX rotErrY rotErrZ];
errQuat = [1 0.5*errRotVec];
truthQuat = quatmultiply(estQuat,errQuat);
Tbn = Quat2Tbn(truthQuat);
dAngTruth = dAngMeas.*dAngScale - dAngBias - [daxNoise dayNoise dazNoise];
deltaQuat = [1 0.5*dAngTruth(1) 0.5*dAngTruth(2) 0.5*dAngTruth(3)] ;
truthQuatNew = quatmultiply(truthQuat,deltaQuat);
errQuatNew = QuatDivide(truthQuatNew,estQuat);
errRotNew = 2 * [errQuatNew(2) errQuatNew(3) errQuatNew(4)];
dVelTruth = dVelMeas - dVelBias - [dvxNoise dvyNoise dvzNoise];
vNew = [vn ve vd] + [0 0 gravity]*dt + Tbn*dVelTruth;
pNew = [pn pe pd] + [vn ve vd]*dt;
dabNew = [dax_b day_b daz_b];
dasNew = [dax_s day_s daz_s];
dvbNew = dvz_b;
vwnNew = vwn;
vweNew = vwe;
magNnew = magN;
magEnew = magE;
magDnew = magD;
magXnew = magX;
magYnew = magY;
magZnew = magZ;
stateVector = [errRotVec vn ve vd pn pe pd dax_b day_b daz_b dax_s day_s daz_s dvz_b magN magE magD magX magY magZ vwn vwe];
nStates=numel(stateVector);
newStateVector = [errRotNew vNew pNew dabNew dasNew dvbNew magNnew magEnew magDnew magXnew magYnew magZnew vwnNew vweNew];
F = jacobian(newStateVector, stateVector);
F = subs(F, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
[F,SF]=OptimiseAlgebra(F,'SF'); usage
%addpath('../scripts/','../TestData','../derivations')
clear all
reset(symengine)
syms dax day daz 'real'
syms dvx dvy dvz 'real'
syms vn ve vd 'real'
syms pn pe pd 'real'
syms rotErrX rotErrY rotErrZ 'real'
syms q0 q1 q2 q3 'real'
syms dax_s day_s daz_s 'real'
syms dvz_b dvy_b dvz_b 'real'
syms dax_b day_b daz_b 'real'
syms daxNoise dayNoise dazNoise dvxNoise dvyNoise dvzNoise 'real'
syms dt 'real'
syms gravity 'real'
syms vwn vwe 'real'
syms magX magY magZ 'real' % XYZ body fixed magnetic field measurements - milligauss XYZ体固定磁场
syms magN magE magD 'real'
dAngMeas = [dax day daz];
dVelMeas = [dvx dvy dvz];
dAngScale = [dax_s day_s daz_s];
dAngBias = [dax_b day_b daz_b];
dVelBias = [0 0 dvz_b];
estQuat = [q0 q1 q2 q3];
errRotVec = [rotErrX rotErrY rotErrZ];
errQuat = [1 0.5*errRotVec];
truthQuat = quatmultiply(estQuat,errQuat);
Tbn = Quat2Tbn(truthQuat);
dAngTruth = dAngMeas.*dAngScale - dAngBias - [daxNoise dayNoise dazNoise];
deltaQuat = [1 0.5*dAngTruth(1) 0.5*dAngTruth(2) 0.5*dAngTruth(3)] ;
truthQuatNew = quatmultiply(truthQuat,deltaQuat);
errQuatNew = QuatDivide(truthQuatNew,estQuat);
errRotNew = 2 * [errQuatNew(2) errQuatNew(3) errQuatNew(4)];
dVelTruth = dVelMeas - dVelBias - [dvxNoise dvyNoise dvzNoise];
vNew = [vn ve vd]+[0 0 gravity]*dt+ dVelTruth*Tbn;
pNew = [pn pe pd] + [vn ve vd]*dt;
dabNew = [dax_b day_b daz_b];
dasNew = [dax_s day_s daz_s];
dvbNew = dvz_b;
vwnNew = vwn;
vweNew = vwe;
magNnew = magN;
magEnew = magE;
magDnew = magD;
magXnew = magX;
magYnew = magY;
magZnew = magZ;
stateVector = [errRotVec vn ve vd pn pe pd dax_b day_b daz_b dax_s day_s daz_s dvz_b magN magE magD magX magY magZ vwn vwe];
nStates=numel(stateVector);
newStateVector = [errRotNew vNew pNew dabNew dasNew dvbNew magNnew magEnew magDnew magXnew magYnew magZnew vwnNew vweNew];
F = jacobian(newStateVector, stateVector);
F = subs(F, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
%F(1,1)
%[F,SF]=OptimiseAlgebra(F,'SF');
for rowIndex = 1:nStates
for colIndex = 1:nStates
eval(['syms OP_l_',num2str(rowIndex),'_c_',num2str(colIndex), '_r_ real']);
eval(['P(',num2str(rowIndex),',',num2str(colIndex), ') = OP_l_',num2str(rowIndex),'_c_',num2str(colIndex),'_r_;']);
end
end
save 'StatePrediction.mat';
distVector = [daxNoise;dayNoise;dazNoise;dvxNoise;dvyNoise;dvzNoise];
G = jacobian(newStateVector, distVector);
G = subs(G, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
%[G,SG]=OptimiseAlgebra(G,'SG');
distMatrix = diag(distVector.^2);
Q = G*distMatrix*transpose(G);
%[Q,SQ]=OptimiseAlgebra(Q,'SQ');
vNew = subs(vNew,{'daxNoise','dayNoise','dazNoise','dvxNoise','dvyNoise','dvzNoise'}, {0,0,0,0,0,0});
errRotNew = subs(errRotNew,{'daxNoise','dayNoise','dazNoise','dvxNoise','dvyNoise','dvzNoise'}, {0,0,0,0,0,0});
PP = F*P*transpose(F) + Q;
[PP,SPP]=OptimiseAlgebra(PP,'SPP');
save('StateAndCovariancePrediction.mat');
clear all;
reset(symengine);
AP_Logger的使用
ArduCopter/config.h
1 #ifndef LOGGING_ENABLED
2 # define LOGGING_ENABLED ENABLED
3 #endif
ArduCopter/system.cpp
1 void Copter::init_ardupilot()
2 {
3 #if LOGGING_ENABLED == ENABLED
4 log_init();//此处调用Copter的log_init
5 #endif
6 }
ArduCopter/log.cpp
1 const struct LogStructure Copter::log_structure[] =
2 {
3 LOG_COMMON_STRUCTURES,
4 }
5
6 void Copter::log_init(void)
7 {
8 logger.Init(log_structure, ARRAY_SIZE(log_structure));
9 }
10 11 //写各种包
12 void Copter::Log_Write_xxxxx()
13 {
14 }copter.h
1 AP_Logger logger;
AP_Logger.h
1 //在这里除了添加东西外不做任何事情，管理旧的实体意味着log分析很容易
2 enum class LogEvent : uint8_t
3 {
4 }
5
6 enum class LogDataID : uint8_t
7 {
8 }
9
10 enum class LogErrorSubsystem
11 {
12 }
13
14 //奇怪的是，该枚举有很多重复的值，几乎没有提供类型安全性
15 enum class LogErrorCode : uint8_t
16 {
17 }
18
19 class AP_Logger
20 {
21 friend class AP_Logger_Backend;
22 }
23
24 struct
25 {
26 AP_Int8 backend_types;
27 AP_Int8 file_bufsize; // in kilobytes
28 AP_Int8 file_disarm_rot;
29 AP_Int8 log_disarmed;
30 AP_Int8 log_replay;
31 AP_Int8 mav_bufsize; // in kilobytes
32 AP_Int16 file_timeout; // in seconds
33 AP_Int16 min_MB_free;
34 } _params;
35
36 enum class Backend_Type : uint8_t
37 {
38 NONE = 0,
39 FILESYSTEM = (1<<0),
40 MAVLINK = (1<<1),
41 BLOCK = (1<<2),
42 };
43
44 AP_Logger_Backend *backends[LOGGER_MAX_BACKENDS];
AP_Logger.cpp
1 #define FOR_EACH_BACKEND(methodcall) \
2 do { \
3 for (uint8_t i=0; i<_next_backend; i++) { \
4 backends[i]->methodcall; \
5 } \
6 } while (0)
LogStructure.h
1 struct UnitStructure
2 {
3 const char ID;
4 const char *unit;
5 };
6
7 const struct UnitStructure log_Units[]
8 {
9 { 'A', "A" }, // 安培
10 }
11
12 #define LOG_PACKET_HEADER uint8_t head1, head2, msgid;
13
14 // 定义log格式的结构体
15 struct LogStructure {
16 uint8_t msg_type;
17 uint8_t msg_len;
18 const char *name;
19 const char *format;
20 const char *labels;
21 const char *units;
22 const char *multipliers;
23 };
24
25 struct PACKED log_Format
26 {
27 }
28
29 struct PACKED log_POS
30 {
31
32 };
33
34 struct PACKED log_POWR
35 {
36 }
37
38 #define LOG_BASE_STRUCTURES
39 { LOG_FORMAT_MSG, sizeof(log_Format), "FMT", "BBnNZ","Type,Length,Name,Format,Columns", "-b---", "-----" },
40
41
42 #define LOG_SBP_STRUCTURES \
43 { LOG_MSG_SBPHEALTH, sizeof(log_SbpHealth),"SBPH", "QIII", "TimeUS,CrcError,LastInject,IARhyp", "s---", "F---" },
44
45 #define LOG_COMMON_STRUCTURES LOG_BASE_STRUCTURES, LOG_SBP_STRUCTURES
46
47
48 //消息0-63为vehicle特殊使用 保留
49 // 通用消息的消息类型
50 enum LogMessages : uint8_t
51 {
52 }ArduPilot中copter的rc_loop()函数分析
copter的rc_loop()任务定义在任务列表中
//频率 100 最大运行时间130us
SCHED_TASK(rc_loop, 100, 130),
//从遥控器或接收器 读取用户的输入
copter的rc_loop()
{
//读radio和radio上的3位置开关
read_radio();
rc().read_mode_switch();
}
RC_Channels
//得到RC_Channels的单列模式
RC_Channels RC_Channels::_singleton; //对象指针
RC_Channels &rc()
{
return RC_Channels::get_singleton();
}
View Code
1 void RC_Channel::read_mode_switch()
2 {
3 //计算flight mode开关的位置
4 const uint16_t pulsewidth = get_radio_in();
5 if (pulsewidth <= 900 || pulsewidth >= 2200)
6 {
7 return; // 这是一个错误条件
8 }
9
10 modeswitch_pos_t position;
11 if (pulsewidth < 1231) position = 0;
12 else if (pulsewidth < 1361) position = 1;
13 else if (pulsewidth < 1491) position = 2;
14 else if (pulsewidth < 1621) position = 3;
15 else if (pulsewidth < 1750) position = 4;
16 else position = 5;
17
18 if (!debounce_completed((int8_t)position)) {
19 return;
20 }
21
22 //设置flight mode和simple mode设置
23 mode_switch_changed(position);
24 }
RC_Channels_Copter
1 //一些函数要在子类实现
2 class RC_Channels_Copter : public RC_Channels
3
4 void RC_Channel_Copter::mode_switch_changed(modeswitch_pos_t new_pos)
5 {
6 if (new_pos < 0 || (uint8_t)new_pos > copter.num_flight_modes)
7 {
8 //不应该被调用
9 return;
10 }
11 if (!copter.set_mode((Mode::Number)copter.flight_modes[new_pos].get(), ModeReason::RC_COMMAND))
12 {
13 // 提醒用户模式改变失败
14 if (copter.ap.initialised)
15 {
16 AP_Notify::events.user_mode_change_failed = 1;
17 }
18 return;
19 }
20 //// 播放曲调，提醒用户模式改变
21 if (copter.ap.initialised)
22 {
23 AP_Notify::events.user_mode_change = 1;
24 }
25 //为option找通道
26 if (!rc().find_channel_for_option(AUX_FUNC::SIMPLE_MODE) &&
27 !rc().find_channel_for_option(AUX_FUNC::SUPERSIMPLE_MODE))
28 {
29 //如果没有Aux开关在simple和super模式设置，就使用存在eeprom中的数据
30 if (BIT_IS_SET(copter.g.super_simple, new_pos))
31 {
32 copter.set_simple_mode(Copter::SimpleMode::SUPERSIMPLE);
33 } else
34 {
35 copter.set_simple_mode(BIT_IS_SET(copter.g.simple_modes, new_pos) ? Copter::SimpleMode::SIMPLE : Copter::SimpleMode::NONE);
36 }
37 }
38 }
Copter::set_simple_mode
1 /*
2 检查是否bitnumber在值中设置，
3 */
4 #define BIT_IS_SET(value, bitnumber) (((value) & (1U<<(bitnumber))) != 0)
5
6 //设置简单模式
7 void Copter::set_simple_mode(SimpleMode b)
8 {
9 if (simple_mode != b)
10 {
11 switch (b)
12 {
13 case SimpleMode::NONE:
14 AP::logger().Write_Event(LogEvent::SET_SIMPLE_OFF);
15 gcs().send_text(MAV_SEVERITY_INFO, "SIMPLE mode off");
16 break;
17 case SimpleMode::SIMPLE:
18 AP::logger().Write_Event(LogEvent::SET_SIMPLE_ON);
19 gcs().send_text(MAV_SEVERITY_INFO, "SIMPLE mode on");
20 break;
21 case SimpleMode::SUPERSIMPLE:
22 // initialise super simple heading
23 update_super_simple_bearing(true);
24 AP::logger().Write_Event(LogEvent::SET_SUPERSIMPLE_ON);
25 gcs().send_text(MAV_SEVERITY_INFO, "SUPERSIMPLE mode on");
26 break;
27 }
28 simple_mode = b;
29 }
30 }
void Copter::read_radio()
1 //更新所有的输入通道
2 bool RC_Channels::read_input(void);
3
4 void Copter::read_radio()
5 {
6 //得到时间
7 const uint32_t tnow_ms = millis();
8 if (rc().read_input())
9 {
10 //如果我们有新的PWM数据要通过无线电进行操作，则设置为true
11 ap.new_radio_frame = true;
12
13 //RC_Channel channel_throttle是油门通道
14 //radio_in 是pwm值
15 //control_in 是将pwm归一化
16 set_throttle_and_failsafe(channel_throttle->get_radio_in());
17 set_throttle_zero_flag(channel_throttle->get_control_in());
18
19 //如果已经有了输入，那么RC接收器要加上
20 //rc_receiver_present 如果有rc接收器出现，就为真true if we have an rc receiver present
21 ap.rc_receiver_present = true;
22
23 //传递pilot输入到motors
24 // pass pilot input through to motors (used to allow wiggling servos while disarmed on heli, single, coax copters)
25 radio_passthrough_to_motors();
26
27 //计算时间差 s
28 const float dt = (tnow_ms - last_radio_update_ms)*1.0e-3f;
29
30 // 过滤pilot得油门输入 用来取消登陆 当油门保持高时，
31 // filtered pilot's throttle input used to cancel landing if throttle held high
32 // LowPassFilterFloat rc_throttle_control_in_filter;
33 //低通滤波器
34 //typedef LowPassFilter<float> LowPassFilterFloat;
35 rc_throttle_control_in_filter.apply(channel_throttle->get_control_in(), dt);
36 //更新时间
37 last_radio_update_ms = tnow_ms;
38 return;
39 }
40 //此时没有radio输入
41 if (failsafe.radio)
42 {
43 // 已经在failsafe
44 return;
45 }
46 const uint32_t elapsed = tnow_ms - last_radio_update_ms;
47
48 //如果没有从RC radio更新， 打开油门failsafe
49 // turn on throttle failsafe if no update from the RC Radio for 500ms or 2000ms if we are using RC_OVERRIDE
50 const uint32_t timeout = RC_Channels::has_active_overrides() ? FS_RADIO_RC_OVERRIDE_TIMEOUT_MS : FS_RADIO_TIMEOUT_MS;
51
52 if (elapsed < timeout)
53 {
54 //还没有超时
55 return;
56 }
57 if (!g.failsafe_throttle)
58 {
59 //油门failsafe失能
60 return;
61 }
62 if (!ap.rc_receiver_present && !motors->armed())
63 {
64 //当armed或者已经看到RC接收器，才failsafe
65 return;
66 }
67 //没有任何人曾告诉我，log一个错误和进入failsafe。
68 // 写错误包
69 //void AP_Logger::Write_Error(LogErrorSubsystem sub_system,
70 // LogErrorCode error_code)
71 AP::logger().Write_Error(LogErrorSubsystem::RADIO, LogErrorCode::RADIO_LATE_FRAME);
72 set_failsafe_radio(true);
73 //void Copter::set_failsafe_radio(bool b)
74 }ArduPilot中Copter的函数分布
copter的所有函数是在copter.h中声明的，
但是定义不仅在Copter.cpp中，还分布在其他文件中
AP_State.cpp
1 void set_auto_armed(bool b);
2 void set_simple_mode(SimpleMode b);
3 void set_failsafe_radio(bool b);
4 void set_failsafe_gcs(bool b);
5 void update_using_interlock();
Copter.cpp
1 void get_scheduler_tasks(const AP_Scheduler::Task *&tasks,
2 uint8_t &task_count,
3 uint32_t &log_bit) override;
4 void fast_loop() override;
5 bool start_takeoff(float alt) override;
6 bool set_target_location(const Location& target_loc) override;
7 bool set_target_velocity_NED(const Vector3f& vel_ned) override;
8 bool set_target_angle_and_climbrate(float roll_deg, float pitch_deg, float yaw_deg, float climb_rate_ms, bool use_yaw_rate, float yaw_rate_degs) override;
9 void rc_loop();
10 void throttle_loop();
11 void update_batt_compass(void);
12 void fourhundred_hz_logging();
13 void ten_hz_logging_loop();
14 void twentyfive_hz_logging();
15 void three_hz_loop();
16 void one_hz_loop();
17 void init_simple_bearing();
18 void update_simple_mode(void);
19 void update_super_simple_bearing(bool force_update);
20 void read_AHRS(void);
21 void update_altitude();
Attitude.cpp
1 float get_pilot_desired_yaw_rate(int16_t stick_angle);
2 void update_throttle_hover();
3 void set_throttle_takeoff();
4 float get_pilot_desired_climb_rate(float throttle_control);
5 float get_non_takeoff_throttle();
6 float get_avoidance_adjusted_climbrate(float target_rate);
7 void set_accel_throttle_I_from_pilot_throttle();
8 void rotate_body_frame_to_NE(float &x, float &y);
9 uint16_t get_pilot_speed_dn();
baro_ground_effect.cpp
1 void update_ground_effect_detector(void);
2 void update_ekf_terrain_height_stable();
commands.cpp
1 void update_home_from_EKF();
2 void set_home_to_current_location_inflight();
3 bool set_home_to_current_location(bool lock) WARN_IF_UNUSED;
4 bool set_home(const Location& loc, bool lock) WARN_IF_UNUSED;
5 bool far_from_EKF_origin(const Location& loc);
crash_check.cpp
1 void crash_check();
2 void thrust_loss_check();
3 void parachute_check();
4 void parachute_release();
5 void parachute_manual_release();
ekf_check.cpp
1 void ekf_check();
2 bool ekf_over_threshold();
3 void failsafe_ekf_event();
4 void failsafe_ekf_off_event(void);
5 void check_ekf_reset();
6 void check_vibration();
esc_calibration.cpp
1 void esc_calibration_startup_check();
2 void esc_calibration_passthrough();
3 void esc_calibration_auto();
4 void esc_calibration_notify();
5 void esc_calibration_setup();
events.cpp
1 bool failsafe_option(FailsafeOption opt) const;
2 void failsafe_radio_on_event();
3 void failsafe_radio_off_event();
4 void handle_battery_failsafe(const char* type_str, const int8_t action);
5 void failsafe_gcs_check();
6 void failsafe_gcs_on_event(void);
7 void failsafe_gcs_off_event(void);
8 void failsafe_terrain_check();
9 void failsafe_terrain_set_status(bool data_ok);
10 void failsafe_terrain_on_event();
11 void gpsglitch_check();
12 void set_mode_RTL_or_land_with_pause(ModeReason reason);
13 void set_mode_SmartRTL_or_RTL(ModeReason reason);
14 void set_mode_SmartRTL_or_land_with_pause(ModeReason reason);
15 bool should_disarm_on_failsafe();
16 void do_failsafe_action(Failsafe_Action action, ModeReason reason);
failsafe.cpp
1 void failsafe_enable();
2 void failsafe_disable();
fence.cpp
1 void fence_check();
inertia.cpp
1 void read_inertia();
land_detector.cpp
1 void update_land_and_crash_detectors();
2 void update_land_detector();
3 void set_land_complete(bool b);
4 void set_land_complete_maybe(bool b);
5 void update_throttle_mix();
landing_gear.cpp
1 void landinggear_update();
standby.cpp
1 void standby_update();
Log.cpp
1 void Log_Write_Control_Tuning();
2 void Log_Write_Performance();
3 void Log_Write_Attitude();
4 void Log_Write_EKF_POS();
5 void Log_Write_MotBatt();
6 void Log_Write_Data(LogDataID id, int32_t value);
7 void Log_Write_Data(LogDataID id, uint32_t value);
8 void Log_Write_Data(LogDataID id, int16_t value);
9 void Log_Write_Data(LogDataID id, uint16_t value);
10 void Log_Write_Data(LogDataID id, float value);
11 void Log_Write_Parameter_Tuning(uint8_t param, float tuning_val, float tune_min, float tune_max);
12 void Log_Sensor_Health();
mode.cpp
1 bool set_mode(Mode::Number mode, ModeReason reason);
2 bool set_mode(const uint8_t new_mode, const ModeReason reason) override;
3 uint8_t get_mode() const override { return (uint8_t)control_mode; }
4 void update_flight_mode();
5 void notify_flight_mode();
mode_land.cpp
1 void set_mode_land_with_pause(ModeReason reason);
2 bool landing_with_GPS();
motor_test.cpp
1 void motor_test_output();
2 bool mavlink_motor_test_check(const GCS_MAVLINK &gcs_chan, bool check_rc);
3 MAV_RESULT mavlink_motor_test_start(const GCS_MAVLINK &gcs_chan, uint8_t motor_seq, uint8_t throttle_type, uint16_t throttle_value, float timeout_sec, uint8_t motor_count);
4 void motor_test_stop();
motors.cpp
1 void arm_motors_check();
2 void auto_disarm_check();
3 void motors_output();
4 void lost_vehicle_check();
navigation.cpp
1 void run_nav_updates(void);
2 int32_t home_bearing();
3 uint32_t home_distance();
Parameters.cpp
1 void load_parameters(void) override;
2 void convert_pid_parameters(void);
3 void convert_lgr_parameters(void);
4 void convert_tradheli_parameters(void);
5 void convert_fs_options_params(void);
precision_landing.cpp
1 void init_precland();
2 void update_precland();
radio.cpp
1 void default_dead_zones();
2 void init_rc_in();
3 void init_rc_out();
4 void enable_motor_output();
5 void read_radio();
6 void set_throttle_and_failsafe(uint16_t throttle_pwm);
7 void set_throttle_zero_flag(int16_t throttle_control);
8 void radio_passthrough_to_motors();
9 int16_t get_throttle_mid(void);
sensors.cpp
1 void read_barometer(void);
2 void init_rangefinder(void);
3 void read_rangefinder(void);
4 bool rangefinder_alt_ok();
5 bool rangefinder_up_ok();
6 void rpm_update();
7 void init_optflow();
8 void update_optical_flow(void);
9 void compass_cal_update(void);
10 void accel_cal_update(void);
11 void init_proximity();
12 void update_proximity();
13 void winch_init();
14 void winch_update();
RC_Channel.cpp
1 void save_trim();
2 void auto_trim();
3 void auto_trim_cancel();
system.cpp
1 void init_ardupilot() override;
2 void startup_INS_ground();
3 void update_dynamic_notch() override;
4 bool position_ok() const;
5 bool ekf_position_ok() const;
6 bool optflow_position_ok() const;
7 void update_auto_armed();
8 bool should_log(uint32_t mask);
9 MAV_TYPE get_frame_mav_type();
10 const char* get_frame_string();
11 void allocate_motors(void);
12 bool is_tradheli() const;
terrain.cpp
1 void terrain_update();
2 void terrain_logging();
tuning.cpp
1 void tuning();
UserCode.cpp
1 void userhook_init();
2 void userhook_FastLoop();
3 void userhook_50Hz();
4 void userhook_MediumLoop();
5 void userhook_SlowLoop();
6 void userhook_SuperSlowLoop();
7 void userhook_auxSwitch1(uint8_t ch_flag);
8 void userhook_auxSwitch2(uint8_t ch_flag);
9 void userhook_auxSwitch3(uint8_t ch_flag);
ArduPilot中Copter的init_ardupilot分析
1 void Copter::init_ardupilot()
2 {
3 BoardConfig.init();//板子配置初始化
4
5 #if HAL_WITH_UAVCAN
6 BoardConfig_CAN.init();
7 #endif
8
9 // 初始化绞盘和车轮编码器
10 winch_init();
11
12 // 初始化通知系统
13 notify.init();
14 notify_flight_mode();
15
16 // 初始化电池监测
17 battery.init();
18
19 //初始化RSSI
20 rssi.init();
21
22 barometer.init();
23
24 // 用串行口设置telem 槽
25 gcs().setup_uarts();
26
27 #if LOGGING_ENABLED == ENABLED
28 log_init();
29 #endif
30
31 //更新motor互锁状态
32 update_using_interlock();
33
34 //从radio设置rc通道
35 init_rc_in();
36
37 // 分配电机类
38 allocate_motors();
39
40 //初始化rc 通道，包括设置模式
41 rc().init();
42
43 //设置电机，输出到esc
44 init_rc_out();
45
46 //检查是否应该进入esc 校准模式
47 esc_calibration_startup_check();
48
49 //motors 初始化完，所以参数可以发送
50 ap.initialised_params = true;
51
52 relay.init();
53
54
55 //设置"主循环是死的"检查，注意，这个依赖于正在初始化的RC库
56 hal.scheduler->register_timer_failsafe(failsafe_check_static, 1000);
57
58
59 //GPS初始化
60 gps.set_log_gps_bit(MASK_LOG_GPS);
61 gps.init(serial_manager);
62
63 AP::compass().set_log_bit(MASK_LOG_COMPASS);
64 AP::compass().init();
65
66 attitude_control->parameter_sanity_check();//完整性检查
67 pos_control->set_dt(scheduler.get_loop_period_s());
68
69 // 初始化光流传感器
70 init_optflow();
71
72 //初始化起落架位置
73 // initialise landing gear position
74 landinggear.init();
75
76 //读地面气压
77 barometer.set_log_baro_bit(MASK_LOG_IMU);
78 barometer.calibrate();
79
80 //初始化测距仪
81 init_rangefinder();
82
83 // 初始化接近传感器
84 init_proximity();
85
86 //初始化AP_Logger库
87 logger.setVehicle_Startup_Writer(FUNCTOR_BIND(&copter, &Copter::Log_Write_Vehicle_Startup_Messages, void));
88
89 startup_INS_ground();
90
91 // 设置降落的标志
92 set_land_complete(true);
93 set_land_complete_maybe(true);
94
95 // we don't want writes to the serial port to cause us to pause
96 // mid-flight, so set the serial ports non-blocking once we are
97 // ready to fly
98 //我们不想写到串口来造成我们暂停mid-flight，一旦准备起飞时，设置串口非阻塞
99 serial_manager.set_blocking_writes_all(false);
100
101 //使能CPU failsafe
102 failsafe_enable();
103
104 ins.set_log_raw_bit(MASK_LOG_IMU_RAW);
105
106 // 使能输出到motors
107 if (arming.rc_calibration_checks(true))
108 {
109 enable_motor_output();
110 }
111
112
113 //如果需要，失能安全性
114 BoardConfig.init_safety();
115
116 hal.console->printf("\nReady to FLY ");
117
118 //表明初始化已经完成的标志
119 ap.initialised = true;
120 }ArduPilot中Copter的Parameter
参数支持的机架类型
1 AP_Param.h
2 #define AP_PARAM_FRAME_COPTER (1<<0)
3 #define AP_PARAM_FRAME_ROVER (1<<1)
4 #define AP_PARAM_FRAME_PLANE (1<<2)
5 #define AP_PARAM_FRAME_SUB (1<<3)
6 #define AP_PARAM_FRAME_TRICOPTER (1<<4)
7 #define AP_PARAM_FRAME_HELI (1<<5)
1 enum ap_var_type
2 {
3 AP_PARAM_NONE = 0,
4 AP_PARAM_INT8,
5 AP_PARAM_INT16,
6 AP_PARAM_INT32,
7 AP_PARAM_FLOAT,
8 AP_PARAM_VECTOR3F,
9 AP_PARAM_GROUP
10 };
设置机架类型标志
1 AP_Param::set_frame_type_flags(AP_PARAM_FRAME_COPTER);
EEPROM
///EEPROM头 放在EEPROM的头部来表明ROM被格式化为AP_Param
struct EEPROM_header
{
uint8_t magic[2];
uint8_t revision;
uint8_t spare;
};
1 /*
2 这个头部在 存储在EEPROM中变量的前面，含义如下
3 * -key:来自Parameter.h中的k_param 枚举值
4 * - group_element: 对顶层参数是0，对存储对象的参数，它被分为6bits的3部分，允许三级对象存储在EEPROM
5 -type: 变量的ap_var_type 值
6 */
7 struct Param_header
8 {
9 //为得到9bits的键值，我们需要把它分为两部分来保持二进制兼容
10 uint32_t key_low : 8;
11 uint32_t type : 5;
12 uint32_t key_high : 1;
13 uint32_t group_element : 18;
14 };
15
16
17 // 查找给定标题的信息结构
18 // 返回Info结构体，指向变量存储的指针
19
20 const struct AP_Param::Info *AP_Param::find_by_header(struct Param_header phdr, void **ptr)
21 {
22 }
23
24 //一个标记用于first()/next()状态
25 typedef struct
26 {
27 uint32_t key : 9;
28 uint32_t idx : 5; // 数组类型的偏移
29 uint32_t group_element : 18;
30 } ParamToken;
Info 和 GroupInfo
Info 和 GroupInfo在main中的setup()被传递，来给出变量是如何命名和在内存中的位置信息
Info
1 class AP_Param::struct Info
2 {
3 uint8_t type; // AP_PARAM_*
4 const char *name;
5 uint16_t key; // k_param_*
6 const void *ptr; // pointer to the variable in memory
7 union
8 {
9 const struct GroupInfo *group_info;
10 const struct GroupInfo **group_info_ptr; // when AP_PARAM_FLAG_INFO_POINTER is set in flags
11 const float def_value;
12 };
13 uint16_t flags;
14 };
copter.h
1 {
2 //全局参数都包含"g"
3 Parameters g;//237行
4 ParametersG2 g2;
5 static const AP_Param::Info var_info[];//584行
6 }parameter和parameterG2的定义
parameter.h
1 //全局参数类
2 class Parameters
3 {
4
5 //定义在这里的枚举类型用来确保每个参数或者参数组有唯一的id
6 //这个数是由AP_Param使用来在EEPROM中存储和定位
7 enum
8 {
9 // Layout version number, always key zero.
10 //
11 k_param_format_version = 0,
12 k_param_software_type, // deprecated
13 k_param_ins_old, // *** Deprecated, remove with next eeprom number change
14 k_param_ins, // libraries/AP_InertialSensor variables
15 k_param_NavEKF2_old, // deprecated - remove
16 k_param_NavEKF2,
17 k_param_g2, // 2nd block of parameters
18 k_param_NavEKF3,
19 k_param_BoardConfig_CAN,
20 k_param_osd,
21 }
22 }
23
24 /*
25 第二块参数，来避免超过顶级256个keys
26 */
27 class ParametersG2
28 {
29 }Ardupilot的Copter的throttle_loop
copter的throttle_loop()任务定义在任务列表中
1 //频率 50 最大运行时间75us
2 SCHED_TASK(throttle_loop, 50, 75),
throttle_loop - 应该以50HZ运行
1 void Copter::throttle_loop()
2 {
3 //更新throttle_low_comp值(控制油门的优先级 VS 高度控制)
4 update_throttle_mix();
5
6 //检查自动armed状态
7 update_auto_armed();
8
9
10 //补偿地面效应
11 update_ground_effect_detector();
12 update_ekf_terrain_height_stable();
13 }update_throttle_mix
1 //根据vehicle状态来设置电机throttle_low_comp值
2 //当油门高于悬停油门时，较低的值优先于飞行员/自动驾驶油门而不是姿态控制，而较高的值则优先于姿态控制而不是油门
3 void Copter::update_throttle_mix()
4 {
5 attitude_control->set_throttle_mix_man();
6 }
AC_AttitudeControl_Multi
1 class AC_AttitudeControl_Multi
2 {
3 //设置所需的油门与姿态混合 (实际混合会在1到2秒内转换为该值 )
4 void set_throttle_mix_min() override { _throttle_rpy_mix_desired = _thr_mix_min};
5 }
ap_t
1 typedef union
2 {
3 struct
4 {
5 uint8_t unused1 : 1; // 0
6 uint8_t unused_was_simple_mode : 2; // 1,2
7 uint8_t pre_arm_rc_check : 1; // 3
8 //当rc 输入 pre-arm检查已经完全成功时为真
9 uint8_t pre_arm_check : 1; // 4
10 //当所有的pre-arm检查(rc，accel,calibration,gps,lock)已经被执行时，为真
11 uint8_t auto_armed : 1; // 5
12 //从开始停止自动执行，直到油门上升
13 uint8_t logging_started : 1; // 6
14 //日志记录开始就为真
15 uint8_t land_complete : 1; // 7
16 //当探测到着陆就为真
17 uint8_t new_radio_frame : 1; // 8
18 //当来自radio的pwm数据来执行时，为真
19 uint8_t usb_connected_unused : 1; // 9 // UNUSED
20 uint8_t rc_receiver_present : 1; // 10
21 //有rc接收机时为真
22 uint8_t compass_mot : 1; // 11
23 //正在进行罗盘校准为真
24 uint8_t motor_test : 1; // 12
25 //执行电机测试时为真
26 uint8_t initialised : 1; // 13
27 //一旦init_ardupilot函数完成，就为真，到gps的扩展状态不发送，直到这个完成
28 uint8_t land_complete_maybe : 1; // 14
29 //如果已经着陆就为真
30 uint8_t throttle_zero : 1; // 15
31 // 如果油门杆为零且反跳，则为true，它确定飞行员是否在不使用电动机互锁时打算关闭
32 uint8_t system_time_set_unused : 1; // 16
33 //系统时间已经从cps设置，为真
34 uint8_t gps_glitching : 1; // 17
35 //GPS错误影响导航准确度
36 uint8_t using_interlock : 1; // 20
37 // aux switch motor interlock function is in use
38 //辅助开关电机互锁功能在使用
39 uint8_t land_repo_active : 1; // 21
40 // 如果飞行员超越着陆位置，则为true
41 uint8_t motor_interlock_switch : 1; // 22
42 // 如果飞行员请求启用电机互锁，则为true
43 uint8_t in_arming_delay : 1; // 23
44 // 当我们武装但正在等待旋转电机时为真
45 uint8_t initialised_params : 1; // 24
46 // true when the all parameters have been initialised. we cannot send parameters to the GCS until this is done
47 //当所有参数已经初始化，我们不能发送参数到GPS，直到这个完成
48 uint8_t unused3 : 1; // 25
49
50 //当指南针的初始化位置已经设置为真，
51 uint8_t unused2 : 1; // 26
52 //辅助开关rc_override是允许的
53 uint8_t armed_with_switch : 1; // 27
54 //使用arm开关来armed
55 };
56 uint32_t value;
57 } ap_t;
update_auto_armed
1 //更新auto_armed标志的状态
2 void Copter::update_auto_armed()
3 {
4 if(ap.auto_armed)
5 {
6 }
7 }
baro_ground_effect.cpp
1 void update_ground_effect_detector(void);
Ardupilot的AP_GPS的update
copter的AP_GPS::update()()任务定义在任务列表中
1 //运行频率50HZ,最长时间是200
2 SCHED_TASK_CLASS(AP_GPS, &copter.gps, update, 50, 200),
3 gps是copter的数据成员，调用AP_GPS::update()函数
AP_GPS::update
1 //基于接受自module的可能字节来更新GPS状态。这个例行必须被周期性的调用来处理接受的数据
2 void update(void);
3
4
5 //更新所有的GPS实例
6 void AP_GPS::update(void)
7 {
8 for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++)
9 {
10 update_instance(i);
11 }
12
13 //计算实例的数目
14 for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++)
15 {
16 if (drivers[i] != nullptr)
17 {
18 num_instances = i+1;
19 }
20 }
21 update_primary();
22 }
1 //GPS驱动类型
2 enum GPS_Type
3 {
4 GPS_TYPE_NONE = 0,
5 GPS_TYPE_AUTO = 1,
6 GPS_TYPE_UBLOX = 2,
7 GPS_TYPE_MTK = 3,
8 GPS_TYPE_MTK19 = 4,
9 GPS_TYPE_NMEA = 5,
10 GPS_TYPE_SIRF = 6,
11 GPS_TYPE_HIL = 7,
12 GPS_TYPE_SBP = 8,
13 GPS_TYPE_UAVCAN = 9,
14 GPS_TYPE_SBF = 10,
15 GPS_TYPE_GSOF = 11,
16 GPS_TYPE_ERB = 13,
17 GPS_TYPE_MAV = 14,
18 GPS_TYPE_NOVA = 15,
19 GPS_TYPE_HEMI = 16, // hemisphere NMEA
20 GPS_TYPE_UBLOX_RTK_BASE = 17,
21 GPS_TYPE_UBLOX_RTK_ROVER = 18,
22 };
23
AP_GPS::update_instance
1 // 更新一个GPS实例，应该以10HZ的频率调用
2 void AP_GPS::update_instance(uint8_t instance)
3 {
4 // 我们有这个实例的活动驱动程序
5 bool result = drivers[instance]->read();
6 }
AP_GPS::update_primary
1 //更新主要的GPS实例
2 void AP_GPS::update_primary(void)
3 {
4 }Ardupilot的Copter的update_batt_compass
copter的update_batt_compass()任务定义在任务列表中
1 //运行频率10HZ,最长时间是120
2 SCHED_TASK(update_batt_compass, 10, 120)
update_batt_compass
1 //读电池和指南针，以10hz的频率调用
2 void Copter::update_batt_compass(void)
3 {
4 先读电池，它用于电机干扰补偿
5 battery.read();
6
7 if(AP::compass().enabled())
8 {
9 //用油门值来更新指南针，用于指南针
10 // Compass compass;
11 compass.set_throttle(motors->get_throttle());
12 compass.set_voltage(battery.voltage());
13 compass.read();
14 }
15 }copter.h
1 // 电池传感器
2 AP_BattMonitor battery{MASK_LOG_CURRENT,
3 FUNCTOR_BIND_MEMBER(&Copter::handle_battery_failsafe, void, const char*, const int8_t),
4 _failsafe_priorities};
5
6 FUNCTOR_TYPEDEF(battery_failsafe_handler_fn_t, void, const char *, const int8_t);
7 void (const char *,const int8_t);
8
9 constexpr int8_t Copter::_failsafe_priorities[7];
10
11 static constexpr int8_t _failsafe_priorities[] =
12 {
13 Failsafe_Action_Terminate,
14 Failsafe_Action_Land,
15 Failsafe_Action_RTL,
16 Failsafe_Action_SmartRTL_Land,
17 Failsafe_Action_SmartRTL,
18 Failsafe_Action_None,
19 -1 // 优先级列表必须以-1的监视结束
20 };AP_BattMonitor.h
1 AP_BattMonitor的构造函数
2
3 AP_BattMonitor(uint32_t log_battery_bit, battery_failsafe_handler_fn_t battery_failsafe_handler_fn, const int8_t *failsafe_priorities);
4
5
6 //返回电池电压，单位是V
7 float voltage(uint8_t instance) const;
8 float voltage() const { return voltage(AP_BATT_PRIMARY_INSTANCE); }
9
10 /// 返回电池电压，单位是V
11 float AP_BattMonitor::voltage(uint8_t instance) const
12 {
13 if (instance < _num_instances)
14 {
15 return state[instance].voltage;
16 } else
17 {
18 return 0.0f;
19 }
20 }AP_BattMonitor_Backend.h
1 AP_BattMonitor_Backend *drivers[AP_BATT_MONITOR_MAX_INSTANCES]
2 具体的backend由BattMonitor_Type来分类
3
4
5 /// 读所有电池的电压和电流，以10HZ频率调用
6 void read()
7 {
8 drivers[i]->read();
9 }
10
11
12
13 BattMonitor_State state[AP_BATT_MONITOR_MAX_INSTANCES];
14
15
16 //每个cell电压在mv
17 struct cells
18 {
19 uint16_t cells[AP_BATT_MONITOR_CELLS_MAX];
20 };
21
22 //BattMonitor_State是由后端驱动填充的
23 struct BattMonitor_State
24 {
25 cells cell_voltages;
26 //电池每个cell电压在mv
27 float voltage;
28 // 电压在V
29 float current_amps;
30 // 电流A
31 float consumed_mah;
32 // 自启动以来的总电流（以毫安小时为单位）
33 float consumed_wh;
34 // 自启动以来消耗的总能量
35 uint32_t last_time_micros;
36 // 最后一次读取电压和电流的时间（以微秒为单位）
37 uint32_t low_voltage_start_ms;
38 // 电压下降到最小值以下的时间（以毫秒为单位）
39 uint32_t critical_voltage_start_ms;
40 // 临界电压故障开始时间
41 float temperature;
42 //电池温度单位是摄氏度
43 uint32_t temperature_time;
44 // 最后接受温度消息的时间戳
45 float voltage_resting_estimate;
46 // 根据伏特的电流和电阻估算值去除掉落的电压
47 float resistance;
48 // 阻抗，通过比较剩余电压与在分电压来计算
49 BatteryFailsafe failsafe;
50 //电池处于故障保护阶段
51 bool healthy;
52 //电池监控是通信正常的
53 bool is_powering_off;
54 // 当电源按键命令关机
55 bool powerOffNotified;
56 // 仅发关机通知
57 }Ap_compass.h
1 //设置油门作为百分比，从0.0到1.0
2 void set_throttle(float thr_pct)
3 {
4 if (_motor_comp_type == AP_COMPASS_MOT_COMP_THROTTLE)
5 {
6 _thr = thr_pct;
7 }
8 }
9
10
11 ///为每个电机补偿 设置电池电压
12 void set_voltage(float voltage)
13 {
14 _per_motor.set_voltage(voltage);
15 }
16
17
18 class AP_Compass_Backend
19 {
20 // 读传感器数据
21 virtual void read(void) = 0;
22 }
23
24
25
26 //读指南针并跟新mag_变量
27 bool read()
28 {
29 for (uint8_t i=0; i< _backend_count; i++)
30 {
31 //调用每个后端的read函数，这个调用更新field[];
32 _backends[i]->read();
33 }
34 }AP_BattMonitor_Params.h
1 AP_BattMonitor_Params
2 {
3 // 电池监控驱动类型
4 enum BattMonitor_Type
5 {
6 BattMonitor_TYPE_NONE = 0,
7 BattMonitor_TYPE_ANALOG_VOLTAGE_ONLY = 3,
8 BattMonitor_TYPE_ANALOG_VOLTAGE_AND_CURRENT = 4,
9 BattMonitor_TYPE_SOLO = 5,
10 BattMonitor_TYPE_BEBOP = 6,
11 BattMonitor_TYPE_SMBus_Generic = 7,
12 BattMonitor_TYPE_UAVCAN_BatteryInfo = 8,
13 BattMonitor_TYPE_BLHeliESC = 9,
14 BattMonitor_TYPE_Sum = 10,
15 BattMonitor_TYPE_FuelFlow = 11,
16 BattMonitor_TYPE_FuelLevel_PWM = 12,
17 BattMonitor_TYPE_SUI3 = 13,
18 BattMonitor_TYPE_SUI6 = 14,
19 BattMonitor_TYPE_NeoDesign = 15,
20 BattMonitor_TYPE_MAXELL = 16,
21 BattMonitor_TYPE_Generator = 17,
22 };
23 }winch_init()
1 // winch and wheel encoder initialisation
2 void Copter::winch_init()
3 {
4 #if WINCH_ENABLED == ENABLED
5 g2.wheel_encoder.init();
6 g2.winch.init(&g2.wheel_encoder);
7 #endif
8 }
void AP_Winch::init(const AP_WheelEncoder *wheel_encoder)
{
// return immediately if not enabled
//如果沒有是能，就馬上退出
if (!_enabled.get())
{
return;
}
switch(config.type.get())
{
case 0:
break;
case 1:
backend = new AP_Winch_Servo(config);
break;
default:
break;
}
if (backend != nullptr)
{
backend->init(wheel_encoder);
}
}
1 void AP_WheelEncoder::init(void)
2 {
3 if (num_instances != 0)
4 {
5 // init called a 2nd time?
6 return;
7 }
8 for (uint8_t i=0; i<WHEELENCODER_MAX_INSTANCES; i++)
9 {
10 switch ((WheelEncoder_Type)_type[i].get())
11 {
12
13 case WheelEncoder_TYPE_QUADRATURE:
14 #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
15 drivers[i] = new AP_WheelEncoder_Quadrature(*this, i, state[i]);
16 #endif
17 break;
18
19 case WheelEncoder_TYPE_SITL_QUADRATURE:
20 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
21 drivers[i] = new AP_WheelEncoder_SITL_Qaudrature(*this, i, state[i]);
22 #endif
23 break;
24
25 case WheelEncoder_TYPE_NONE:
26 break;
27 }
28
29
30 if (drivers[i] != nullptr)
31 {
32 // we loaded a driver for this instance, so it must be
33 // present (although it may not be healthy)
34 //为此实列加载一个驱动，所有必须被展现
35 num_instances = i+1; // num_instances is a high-water-mark 高水位标记
36 }
37 }
38 }GCS流程
两个任务
1 SCHED_TASK_CLASS(GCS, (GCS)&copter._gcs, update_receive, 400, 180),
2 SCHED_TASK_CLASS(GCS, (GCS)&copter._gcs, update_send, 400, 550),
Update_receive
void GCS::update_receive(void)
{
for (uint8_t i=0; i<num_gcs(); i++)
{
chan(i)->update_receive();
}
// also update UART pass-thru, if enabled
//如果使能,也更新UART透传
update_passthru();
}
GCS_MAVLINK::update_receive
void GCS_MAVLINK::update_receive(uint32_t max_time_us)
{
if (mavlink_parse_char(chan, c, &msg, &status))
{
packetReceived(status, msg);
}
}Message
typedef struct __mavlink_message
{
uint16_t checksum; /// sent at end of packet
uint8_t magic; ///< protocol magic marker
uint8_t len; ///< Length of payload
uint8_t seq; ///< Sequence of packet
uint8_t sysid; ///< ID of message sender system/aircraft
uint8_t compid; ///< ID of the message sender component
uint8_t msgid; ///< ID of message in payload
uint64_t payload64[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+7)/8];
} mavlink_message_t;
Status
typedef struct __mavlink_status
{
uint8_t msg_received; ///< Number of received messages 接受的消息数量
uint8_t buffer_overrun; ///< Number of buffer overruns缓冲区溢出次数
uint8_t parse_error; ///< Number of parse errors 解析错误次数
mavlink_parse_state_t parse_state; ///< Parsing state machine 解析状态机
uint8_t packet_idx; ///< Index in current packet 当前包的index
uint8_t current_rx_seq; ///< Sequence number of last packet received 最后接受包的序列号
uint8_t current_tx_seq; ///< Sequence number of last packet sent 最后发送包的序列号
uint16_t packet_rx_success_count; ///< Received packets 接受的包
uint16_t packet_rx_drop_count; ///< Number of packet drops 丢弃的包
} mavlink_status_t;
Parser_state
typedef enum
{
MAVLINK_PARSE_STATE_UNINIT=0,
MAVLINK_PARSE_STATE_IDLE,
MAVLINK_PARSE_STATE_GOT_STX,
MAVLINK_PARSE_STATE_GOT_SEQ,
MAVLINK_PARSE_STATE_GOT_LENGTH,
MAVLINK_PARSE_STATE_GOT_SYSID,
MAVLINK_PARSE_STATE_GOT_COMPID,
MAVLINK_PARSE_STATE_GOT_MSGID,
MAVLINK_PARSE_STATE_GOT_PAYLOAD,
MAVLINK_PARSE_STATE_GOT_CRC1,
MAVLINK_PARSE_STATE_GOT_BAD_CRC1
} mavlink_parse_state_t; ///< The state machine for the comm parserArdupilot(4.1)中的radio
初始化
void Copter::init_ardupilot()
{
init_rc_in();
rc().init();
init_rc_out();
}
## init_rc_in
void Copter::init_rc_in()
{
channel_roll = rc().channel(rcmap.roll()-1);
channel_pitch = rc().channel(rcmap.pitch()-1);
channel_throttle = rc().channel(rcmap.throttle()-1);
channel_yaw = rc().channel(rcmap.yaw()-1);
// set rc channel ranges
//设置rc通道范围
channel_roll->set_angle(ROLL_PITCH_YAW_INPUT_MAX);
channel_pitch->set_angle(ROLL_PITCH_YAW_INPUT_MAX);
channel_yaw->set_angle(ROLL_PITCH_YAW_INPUT_MAX);
channel_throttle->set_range(1000);
// set default dead zones
//设置默认死区
default_dead_zones();
// initialise throttle_zero flag
//初始化风门0标志
ap.throttle_zero = true;
}
## rc().init()
// singleton instance
RC_Channels *RC_Channels::_singleton;
RC_Channels &rc()
{
return *RC_Channels::get_singleton();
}
void RC_Channels::init(void)
{
// setup ch_in on channels
for (uint8_t i=0; i<NUM_RC_CHANNELS; i++)
{
channel(i)->ch_in = i;
}
init_aux_all();
}
void RC_Channels::init_aux_all()
{
for (uint8_t i=0; i<NUM_RC_CHANNELS; i++)
{
RC_Channel *c = channel(i);
if (c == nullptr)
{
continue;
}
c->init_aux();
}
reset_mode_switch();
}
## init_rc_out()
void Copter::init_rc_out()
{
motors->set_loop_rate(scheduler.get_loop_rate_hz());
motors->init((AP_Motors::motor_frame_class)g2.frame_class.get(), (AP_Motors::motor_frame_type)g.frame_type.get());
}
## 循环
## task rc_loop
SCHED_TASK(rc_loop, 100, 130),
## rc_loop
void Copter::rc_loop()
{
// Read radio and 3-position switch on radio
// -----------------------------------------
//读radio和radio上的3位置开关
read_radio();
rc().read_mode_switch();
}
## read_radio()
void Copter::read_radio()
{
const uint32_t tnow_ms = millis();
if (rc().read_input())
{
ap.new_radio_frame = true;
set_throttle_and_failsafe(channel_throttle->get_radio_in());
set_throttle_zero_flag(channel_throttle->get_control_in());
// RC receiver must be attached if we've just got input
//如果已经得到输入，RC接收器必须加上
ap.rc_receiver_present = true;
// pass pilot input through to motors (used to allow wiggling servos while disarmed on heli, single, coax copters)
//将飞行员输入传递给电机
radio_passthrough_to_motors();
const float dt = (tnow_ms - last_radio_update_ms)*1.0e-3f;
rc_throttle_control_in_filter.apply(channel_throttle->get_control_in(), dt);
last_radio_update_ms = tnow_ms;
return;
}
}
## read_input
bool RC_Channels::read_input(void)
{
if (hal.rcin->new_input())
{
_has_had_rc_receiver = true;
}
else if (!has_new_overrides)
{
return false;
}
has_new_overrides = false;
last_update_ms = AP_HAL::millis();
bool success = false;
for (uint8_t i=0; i<NUM_RC_CHANNELS; i++)
{
success |= channel(i)->update();
}
return success;
}
## new_input(Ap_HAL_Chibios/rcinput.)
bool RCInput::new_input()
{
if (!_init)
{
return false;
}
bool valid;
{
WITH_SEMAPHORE(rcin_mutex);
valid = _rcin_timestamp_last_signal != _last_read;
_last_read = _rcin_timestamp_last_signal;
}
#if HAL_RCINPUT_WITH_AP_RADIO
if (!_radio_init)
{
_radio_init = true;
radio = AP_Radio::get_singleton();
if (radio)
{
radio->init();
}
}
#endif
return valid;
}
*此处 AP_Radio *radio; 定义在rc_input.h中*
## channel(i)->update()
bool RC_Channel::update(void)
{
radio_in = hal.rcin->read(ch_in);
if (type_in == RC_CHANNEL_TYPE_RANGE)
{
control_in = pwm_to_range();
}
else
{
//RC_CHANNEL_TYPE_ANGLE
control_in = pwm_to_angle();
}
return true;
## RCInput::read()
uint16_t RCInput::read(uint8_t channel)
{
v = _rc_values[channel];
#if HAL_RCINPUT_WITH_AP_RADIO
if (radio && channel == 0)
{
// hook to allow for update of radio on main thread, for mavlink sends
radio->update();
}
#endif
return v;
}
## _rc_values在哪里被设置了？？
***uint16_t _rc_values[RC_INPUT_MAX_CHANNELS] = {0};
在AP_HAL_ChibiOS/RCInput.h中***
void RCInput::_timer_tick(void)
{
AP_RCProtocol &rcprot = AP::RC();
if (rcprot.new_input())
{
WITH_SEMAPHORE(rcin_mutex);
_rcin_timestamp_last_signal = AP_HAL::micros();
_num_channels = rcprot.num_channels();
_num_channels = MIN(_num_channels, RC_INPUT_MAX_CHANNELS);
//在此处设置了_rc_values
rcprot.read(_rc_values, _num_channels);
_rssi = rcprot.get_RSSI();
}
}
## AP_RCProtocol::read
void AP_RCProtocol::read(uint16_t *pwm, uint8_t n)
{
if (_detected_protocol != AP_RCProtocol::NONE)
{
backend[_detected_protocol]->read(pwm, n);
}
}
## AP_RCProtocol_Backend::read
void AP_RCProtocol_Backend::read(uint16_t *pwm, uint8_t n)
{
if (n >= MAX_RCIN_CHANNELS)
{
n = MAX_RCIN_CHANNELS;
}
memcpy(pwm, _pwm_values, n*sizeof(pwm[0]));
}
*****将_pwm_values值赋给pwm，即赋给_rc_values*****
## _pwm_values在哪里得到?
***uint16_t _pwm_values[MAX_RCIN_CHANNELS];定义在
AP_RCProtocol_Backend.h中***
void AP_RCProtocol_Backend::add_input(uint8_t num_values, uint16_t *values, bool in_failsafe, int16_t _rssi)
{
num_values = MIN(num_values, MAX_RCIN_CHANNELS);
memcpy(_pwm_values, values, num_values*sizeof(uint16_t));
_num_channels = num_values;
rc_frame_count++;
}
## add_input被process_pulse调用
void AP_RCProtocol_PPMSum::process_pulse(uint32_t width_s0, uint32_t width_s1)
{
if (width_s0 == 0 || width_s1 == 0)
{
//invalid data: reset frame
//无效数据:复位frame
ppm_state._channel_counter = -1;
return;
}
uint32_t width_usec = width_s0 + width_s1;
if (width_usec >= 2700)
{
// a long pulse indicates the end of a frame. Reset the
// channel counter so next pulse is channel 0
//一个长pluse表明frame的结束,复位channel计数器，因此下个pulse是channel0
if (ppm_state._channel_counter >= MIN_RCIN_CHANNELS)
{
add_input(ppm_state._channel_counter, ppm_state._pulse_capt, false);
}
ppm_state._channel_counter = 0;
return;
}
if (ppm_state._channel_counter == -1)
{
// we are not synchronised
//我们没有同步
return;
}
/*
we limit inputs to between 700usec and 2300usec. This allows us
to decode SBUS on the same pin, as SBUS will have a maximum
pulse width of 100usec
限制输入到700-2300usec,允许我们在相同pin解码SBUS
*/
if (width_usec > 700 && width_usec < 2300)
{
// take a reading for the current channel
//读取当前频道
// buffer these
ppm_state._pulse_capt[ppm_state._channel_counter] = width_usec;
// move to next channel
//移动到下一个通道
ppm_state._channel_counter++;
}
// if we have reached the maximum supported channels then
// mark as unsynchronised, so we wait for a wide pulse
//如果我们到最大支持的通道然后标记不同步,所以等待宽脉冲
if (ppm_state._channel_counter >= MAX_RCIN_CHANNELS)
{
add_input(ppm_state._channel_counter, ppm_state._pulse_capt, false);
ppm_state._channel_counter = -1;
}
}
## AP_RCProtocol::process_pulse
void AP_RCProtocol::process_pulse(uint32_t width_s0, uint32_t width_s1)
{
backend[_detected_protocol]->process_pulse(width_s0, width_s1);
}
## AP_RCProtocol::process_pulse_list
void AP_RCProtocol::process_pulse_list(const uint32_t *widths, uint16_t n, bool need_swap)
{
process_pulse(widths0, widths1);
}
## void RCInput::_timer_tick(void)
void RCInput::_timer_tick(void)
{
rcprot.process_pulse_list(p, n*2, sig_reader.need_swap);
}
## Scheduler::_rcin_thread
void Scheduler::_rcin_thread(void *arg)
{
Scheduler *sched = (Scheduler *)arg;
chRegSetThreadName("rcin");
while (!sched->_hal_initialized) {
sched->delay_microseconds(20000);
}
while (true) {
sched->delay_microseconds(1000);
((RCInput *)hal.rcin)->_timer_tick();
}
}
## void Scheduler::init()
_rcin_thread_ctx = chThdCreateStatic(_rcin_thread_wa,
sizeof(_rcin_thread_wa),
APM_RCIN_PRIORITY, /* Initial priority. */
_rcin_thread, /* Thread function. */
this); /* Thread parameter. */
Chibios
--{root} - Distribution directory.
+--os/ - ChibiOS products, this directory.
| +--rt/ - ChibiOS/RT product.
| | +--include/ - RT kernel headers.
| | +--src/ - RT kernel sources.
| | +--templates/ - RT kernel port template files.
| | +--ports/ - RT kernel port files.
| | +--osal/ - RT kernel OSAL module for HAL interface.
| +--nil/ - ChibiOS/NIL product.
| | +--include/ - Nil kernel headers.
| | +--src/ - Nil kernel sources.
| | +--templates/ - Nil kernel port template files.
| | +--ports/ - Nil kernel port files.
| | +--osal/ - Nil kernel OSAL module for HAL interface.
| +--hal/ - ChibiOS/HAL product.
| | +--include/ - HAL high level headers.
| | +--src/ - HAL high level sources.
| | +--templates/ - HAL port template files.
| | +--ports/ - HAL port files (low level drivers implementations).
| | +--boards/ - HAL board files.
| +--common/ - Files used by multiple ChibiOS products.
| | +--ports - Common port files for various architectures and
| | compilers.
| +--various/ - Various portable support files.
| +--ext/ - Vendor files used by ChibiOS products.APM的启动与入口
#define AP_HAL_MAIN() \
AP_HAL::HAL::FunCallbacks callbacks(setup, loop); \
extern "C" { \
int AP_MAIN(int argc, char* const argv[]); \
int AP_MAIN(int argc, char* const argv[]) { \
hal.run(argc, argv, &callbacks); \
return 0; \
} \
}
#define AP_HAL_MAIN_CALLBACKS(CALLBACKS) extern "C" { \
int AP_MAIN(int argc, char* const argv[]); \
int AP_MAIN(int argc, char* const argv[]) { \
hal.run(argc, argv, CALLBACKS); \
return 0; \
} \
}
virtual void run(int argc, char * const argv[], Callbacks* callbacks) const = 0; HAL.h
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
class HAL_ChibiOS : public AP_HAL::HAL HAL_ChibiOS_class.cpp
{
}
class HAL
{
struct Callbacks
{
virtual void setup() = 0;
virtual void loop() = 0;
};
}
static AP_HAL::HAL::Callbacks* g_callbacks;
HAL_ChibiOS::run(int argc, char * const argv[], Callbacks* callbacks)
{
g_callbacks = callbacks;
//Takeover main
main_loop();
}
main_loop()
{
hal.scheduler->init(); //AP_HAL::Scheduler* scheduler; class AP_HAL::Scheduler {}
g_callbacks->setup();
while (true)
{
g_callbacks->loop();
}
}
class AP_Vehicle : public AP_HAL::HAL::Callbacks
{
void setup(void) override final;
// HAL::Callbacks implementation.
void loop() override final;
}
void AP_Vehicle::setup()
{
hal.console->printf("\n\nInit %s"
"\n\nFree RAM: %u\n",
AP::fwversion().fw_string,
(unsigned)hal.util->available_memory());
const AP_Scheduler::Task *tasks;
uint8_t task_count;
uint32_t log_bit;
get_scheduler_tasks(tasks, task_count, log_bit);
AP::scheduler().init(tasks, task_count, log_bit);
init_ardupilot();
}
void AP_Vehicle::loop()
{
scheduler.loop();
G_Dt = scheduler.get_loop_period_s();
}
virtual void get_scheduler_tasks(const AP_Scheduler::Task *&tasks, uint8_t &task_count, uint32_t &log_bit) = 0; AP_Verticle.h
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;
}
// \libraries\AP_HAL\utility/function.h
#define FUNCTOR_TYPEDEF(name, rettype, ...) \
typedef Functor<rettype, ## __VA_ARGS__> name
#define FUNCTOR_DECLARE(name, rettype, ...) \
Functor<rettype, ## __VA_ARGS__> name
#define FUNCTOR_BIND(obj, func, rettype, ...) \
Functor<rettype, ## __VA_ARGS__>::bind<std::remove_reference<decltype(*obj)>::type, func>(obj)
#define FUNCTOR_BIND_MEMBER(func, rettype, ...) \
Functor<rettype, ## __VA_ARGS__>::bind<std::remove_reference<decltype(*this)>::type, func>(this)
//constructor
AP_Scheduler::AP_Scheduler(scheduler_fastloop_fn_t fastloop_fn) :
_fastloop_fn(fastloop_fn)
{
}
AP_Scheduler scheduler{FUNCTOR_BIND_MEMBER(&AP_Vehicle::fast_loop, void)}; AP_Vehicle.h
Functor<void>::bind<&AP_Scheduler,&AP_Vehicle::fast_loop>
{
return {obj,(void)AP_Vehicle::fast_loop}
}
template <class RetType, class... Args>
class Functor
{
template<class T, RetType (T::*method)(Args...)>
static constexpr Functor bind(T *obj)
{
return { obj, method_wrapper<T, method> };
}
template<class T, RetType (T::*method)(Args...)>
static RetType method_wrapper(void *obj, Args... args)
{
T *t = static_cast<T*>(obj);
return (t->*method)(args...);
}
}
}
virtual void fast_loop();
void Copter::fast_loop()
{
AP_Vehicle::fast_loop();
}
class Copter : public AP_Vehicle
Copter copter;
AP_HAL_MAIN_CALLBACKS(&copter);
展开是
int main(int argc, char* const argv[]);
int main(int argc, char* const argv[])
{
hal.run(argc, argv, &copter);
return 0;
}Ardupilot的RC_Channels的read_aux_all
copter.g2.rc_channels的read_aux_all()任务定义在任务列表中
1 ////运行频率10HZ,最长时间是50
2 SCHED_TASK_CLASS(RC_Channels, (RC_Channels*)&copter.g2.rc_channels, read_aux_all, 10, 50)
ParametersG2
1 class ParametersG2
2 {
3 RC_Channels_Copter rc_channels
4 }ArduCopter/RC_Channel.h
1 class RC_Channel_Copter : public RC_Channel
2 {
3 }
4
5 class RC_Channels_Copter : public RC_Channels
6 {
7 }
1 //支持辅助开关，读辅助开关，检查辅助开关位置和调用配置的动作
2 void RC_Channels::read_aux_all()
3 {
4 RC_Channel *c = channel(i);
5 need_log |= c->read_aux();
6 }
7
8 //读辅助通道，如果开关已经改变，就为真
9 bool RC_Channel::read_aux()
10 {
11
12 }ArduPilot中Copter的auto_disarm_check()
copter的auto_disarm_check()任务定义在任务列表中
1 ////运行频率10HZ,最长时间是50
2 SCHED_TASK(auto_disarm_check, 10, 50),
auto_disarm_check
1 //如果在手动模式的copter在地面，油门值低于至少15s，disarm copter
2 void Copter::auto_disarm_check()
3 {
4 }
libraries/Ap_Motor_Class.h
1 void AP_Motors::armed(bool arm)
2 {
3 if (_flags.armed != arm)
4 {
5 _flags.armed = arm;
6 AP_Notify::flags.armed = arm;
7 if (!arm)
8 {
9 save_params_on_disarm();
10 }
11 }
12 };Ardupilot的copter的pre_arming
RC检查
1 void Copter::init_ardupilot()
2 {
3 //#271 使能输出到电机
4 if (arming.rc_calibration_checks(true))
5 {
6 enable_motor_output();
7 }
8 }
1 bool AP_Arming_Copter::rc_calibration_checks(bool display_failure)
2 {
3 const RC_Channel *channels[] =
4 {
5 copter.channel_roll,
6 copter.channel_pitch,
7 copter.channel_throttle,
8 copter.channel_yaw
9 };
10
11 copter.ap.pre_arm_rc_check = rc_checks_copter_sub(display_failure, channels)
12 & AP_Arming::rc_calibration_checks(display_failure);
13
14 return copter.ap.pre_arm_rc_check;
15 } 1 //Copter和SUb共有相同的RC输入限制，Copter检查已经默认配置的最小和最大，sub不做
2 bool AP_Arming::rc_checks_copter_sub(const bool display_failure, const RC_Channel *channels[4]) const
3 {
4 //如果RC 检查是失能，设置RC-checks 为成功
5 if ((checks_to_perform != ARMING_CHECK_ALL) && !(checks_to_perform & ARMING_CHECK_RC))
6 {
7 return true;
8 }
9
10 bool ret = true;
11
12 const char *channel_names[] = { "Roll", "Pitch", "Throttle", "Yaw" };
13
14 for (uint8_t i=0; i<ARRAY_SIZE(channel_names);i++)
15 {
16 const RC_Channel *channel = channels[i];
17 const char *channel_name = channel_names[i];
18 //检查是否radio已经矫正
19 if (channel->get_radio_min() > 1300)
20 {
21 check_failed(ARMING_CHECK_RC, display_failure, "%s radio min too high", channel_name);
22 ret = false;
23 }
24 if (channel->get_radio_max() < 1700)
25 {
26 check_failed(ARMING_CHECK_RC, display_failure, "%s radio max too low", channel_name);
27 ret = false;
28 }
29 bool fail = true;
30 if (i == 2)
31 {
32 // 跳过检查油门的修整，因为旧代码未检查
33 fail = false;
34 }
35 if (channel->get_radio_trim() < channel->get_radio_min())
36 {
37 check_failed(ARMING_CHECK_RC, display_failure, "%s radio trim below min", channel_name);
38 if (fail)
39 {
40 ret = false;
41 }
42 }
43 if (channel->get_radio_trim() > channel->get_radio_max())
44 {
45 check_failed(ARMING_CHECK_RC, display_failure, "%s radio trim above max", channel_name);
46 if (fail)
47 {
48 ret = false;
49 }
50 }
51 }
52 return ret;
53 } 1 bool AP_Arming::rc_calibration_checks(bool report)
2 {
3 bool check_passed = true;
4 const uint8_t num_channels = RC_Channels::get_valid_channel_count();
5 for (uint8_t i = 0; i < NUM_RC_CHANNELS; i++)
6 {
7 const RC_Channel *c = rc().channel(i);
8 if (c == nullptr)
9 {
10 continue;
11 }
12 if (i >= num_channels && !(c->has_override()))
13 {
14 continue;
15 }
16 const uint16_t trim = c->get_radio_trim();
17 if (c->get_radio_min() > trim)
18 {
19 check_failed(ARMING_CHECK_RC, report, "RC%d_MIN is greater than RC%d_TRIM", i + 1, i + 1);
20 check_passed = false;
21 }
22 if (c->get_radio_max() < trim)
23 {
24 check_failed(ARMING_CHECK_RC, report, "RC%d_MAX is less than RC%d_TRIM", i + 1, i + 1);
25 check_passed = false;
26 }
27 }
28
29 return check_passed;
30 }
31
32 int16_t get_radio_trim() const
33 {
34 return radio_trim.get();
35 }
36
37
38 //使能，输出最小值到motors
39 void Copter::enable_motor_output()
40 {
41 motors->output_min();
42 }one_hz_loop()
Copter的one_hz_loop()任务定义在任务列表中
1 ////运行频率1HZ,最长时间是100
2 SCHED_TASK(one_hz_loop, 1, 100),
1 // 1hz
2 void Copter::one_hz_loop()
3 {
4
5 arming.update();
6
7 //记录地形数据
8 terrain_logging();
9 }
AP_Arming_Copter arming;
1 //辅助开关枚举
2 enum class AUX_FUNC
3 {
4 DO_NOTHING = 0, // aux switch disabled
5 FLIP = 2, // flip
6 SIMPLE_MODE = 3, // change to simple mode
7 RTL = 4, // change to RTL flight mode
8 SAVE_TRIM = 5, // save current position as level
9 SAVE_WP = 7, // save mission waypoint or RTL if in auto mode
10 CAMERA_TRIGGER = 9, // trigger camera servo or relay
11 RANGEFINDER = 10, // allow enabling or disabling rangefinder in flight which helps avoid surface tracking when you are far above the ground
12 FENCE = 11, // allow enabling or disabling fence in flight
13 RESETTOARMEDYAW = 12, // UNUSED
14 SUPERSIMPLE_MODE = 13, // change to simple mode in middle, super simple at top
15 ACRO_TRAINER = 14, // low = disabled, middle = leveled, high = leveled and limited
16 SPRAYER = 15, // enable/disable the crop sprayer
17 AUTO = 16, // change to auto flight mode
18 AUTOTUNE = 17, // auto tune
19 LAND = 18, // change to LAND flight mode
20 GRIPPER = 19, // Operate cargo grippers low=off, middle=neutral, high=on
21 PARACHUTE_ENABLE = 21, // Parachute enable/disable
22 PARACHUTE_RELEASE = 22, // Parachute release
23 PARACHUTE_3POS = 23, // Parachute disable, enable, release with 3 position switch
24 MISSION_RESET = 24, // Reset auto mission to start from first command
25 ATTCON_FEEDFWD = 25, // enable/disable the roll and pitch rate feed forward
26 ATTCON_ACCEL_LIM = 26, // enable/disable the roll, pitch and yaw accel limiting
27 RETRACT_MOUNT = 27, // Retract Mount
28 RELAY = 28, // Relay pin on/off (only supports first relay)
29 LANDING_GEAR = 29, // Landing gear controller
30 LOST_VEHICLE_SOUND = 30, // Play lost vehicle sound
31 MOTOR_ESTOP = 31, // Emergency Stop Switch
32 MOTOR_INTERLOCK = 32,
33 //电机on/off开关
34 BRAKE = 33, // Brake flight mode
35 RELAY2 = 34, // Relay2 pin on/off
36 RELAY3 = 35, // Relay3 pin on/off
37 RELAY4 = 36, // Relay4 pin on/off
38 THROW = 37, // change to THROW flight mode
39 AVOID_ADSB = 38, // enable AP_Avoidance library
40 PRECISION_LOITER = 39, // enable precision loiter
41 AVOID_PROXIMITY = 40, // enable object avoidance using proximity sensors (ie. horizontal lidar)
42 ARMDISARM = 41, // arm or disarm vehicle
43 SMART_RTL = 42, // change to SmartRTL flight mode
44 INVERTED = 43, // enable inverted flight
45 WINCH_ENABLE = 44, // winch enable/disable
46 WINCH_CONTROL = 45, // winch control
47 RC_OVERRIDE_ENABLE = 46, // enable RC Override
48 USER_FUNC1 = 47, // user function #1
49 USER_FUNC2 = 48, // user function #2
50 USER_FUNC3 = 49, // user function #3
51 LEARN_CRUISE = 50, // learn cruise throttle (Rover)
52 MANUAL = 51, // manual mode
53 ACRO = 52, // acro mode
54 STEERING = 53, // steering mode
55 HOLD = 54, // hold mode
56 GUIDED = 55, // guided mode
57 LOITER = 56, // loiter mode
58 FOLLOW = 57, // follow mode
59 CLEAR_WP = 58, // clear waypoints
60 SIMPLE = 59, // simple mode
61 ZIGZAG = 60, // zigzag mode
62 ZIGZAG_SaveWP = 61, // zigzag save waypoint
63 COMPASS_LEARN = 62, // learn compass offsets
64 SAILBOAT_TACK = 63, // rover sailboat tack
65 REVERSE_THROTTLE = 64, // reverse throttle input
66 GPS_DISABLE = 65, // disable GPS for testing
67 RELAY5 = 66, // Relay5 pin on/off
68 RELAY6 = 67, // Relay6 pin on/off
69 STABILIZE = 68, // stabilize mode
70 POSHOLD = 69, // poshold mode
71 ALTHOLD = 70, // althold mode
72 FLOWHOLD = 71, // flowhold mode
73 CIRCLE = 72, // circle mode
74 DRIFT = 73, // drift mode
75 SAILBOAT_MOTOR_3POS = 74, // Sailboat motoring 3pos
76 SURFACE_TRACKING = 75, // Surface tracking upwards or downwards
77 STANDBY = 76, // Standby mode
78 TAKEOFF = 77, // takeoff
79 RUNCAM_CONTROL = 78, // control RunCam device
80 RUNCAM_OSD_CONTROL = 79, // control RunCam OSD
81 VISODOM_CALIBRATE = 80, // calibrate visual odometry camera's attitude
82 DISARM = 81, // disarm vehicle
83 Q_ASSIST = 82, // disable, enable and force Q assist
84 ZIGZAG_Auto = 83, // zigzag auto switch
85 AIRMODE = 84, // enable / disable airmode for copter
86 GENERATOR = 85, // generator control
87 TER_DISABLE = 86, // disable terrain following in CRUISE/FBWB modes
88 CROW_SELECT = 87, // select CROW mode for diff spoilers;high disables,mid forces progressive
89 SOARING = 88, // three-position switch to set soaring mode
90
91 // entries from 100 onwards are expected to be developer
92 // options used for testing
93 KILL_IMU1 = 100, // disable first IMU (for IMU failure testing)
94 KILL_IMU2 = 101, // disable second IMU (for IMU failure testing)
95 CAM_MODE_TOGGLE = 102, // Momentary switch to cycle camera modes
96 EKF_LANE_SWITCH = 103, // trigger lane switch attempt
97 EKF_YAW_RESET = 104, // trigger yaw reset attempt
98 GPS_DISABLE_YAW = 105, // disable GPS yaw for testing
99 // if you add something here, make sure to update the documentation of the parameter in RC_Channel.cpp!
100 // also, if you add an option >255, you will need to fix duplicate_options_exist
101
102 // inputs from 200 will eventually used to replace RCMAP
103 MAINSAIL = 207, // mainsail input
104 FLAP = 208, // flap input
105 FWD_THR = 209, // VTOL manual forward throttle
106
107 // inputs for the use of onboard lua scripting
108 SCRIPTING_1 = 300,
109 SCRIPTING_2 = 301,
110 SCRIPTING_3 = 302,
111 SCRIPTING_4 = 303,
112 SCRIPTING_5 = 304,
113 SCRIPTING_6 = 305,
114 SCRIPTING_7 = 306,
115 SCRIPTING_8 = 307,
116 }; 1 void AP_Arming::check_failed(bool report, const char *fmt, ...) const
2 {
3 if (!report)
4 {
5 return;
6 }
7 char taggedfmt[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1];
8 hal.util->snprintf(taggedfmt, sizeof(taggedfmt), "PreArm: %s", fmt);
9 va_list arg_list;
10 va_start(arg_list, fmt);
11 gcs().send_textv(MAV_SEVERITY_CRITICAL, taggedfmt, arg_list);
12 va_end(arg_list);
13 } 1 //执行预arm检查
2 void AP_Arming_Copter::update(void)
3 {
4 pre_arm_checks(display_fail);
5 }
6
7
8 bool AP_Arming_Copter::pre_arm_checks(bool display_failure)
9 {
10 const bool passed = run_pre_arm_checks(display_failure);
11 set_pre_arm_check(passed);
12 return passed;
13 }
1 // 执行预arm检查，如果检查成功通过，返回真
2 bool AP_Arming_Copter::run_pre_arm_checks(bool display_failure)
3 {
4 //如果已经armed，就立即退出
5 if (copter.motors->armed())
6 {
7 return true;
8 }
9
10 //检查是否电机互锁和紧急停止辅助按钮在同时使用，这是不被允许的。
11 if (rc().find_channel_for_option(RC_Channel::AUX_FUNC::MOTOR_INTERLOCK) &&
12 rc().find_channel_for_option(RC_Channel::AUX_FUNC::MOTOR_ESTOP))
13 {
14 check_failed(display_failure, "Interlock/E-Stop Conflict");
15 return false;
16 }
17
18 //检查是否motor互锁辅助开关是在使用，如果有，开关需要在失能位置来arm，否则立即退出，
19 //由于状态可以任意时刻改变，这个检查是重复的，
20 if (copter.ap.using_interlock && copter.ap.motor_interlock_switch)
21 {
22 check_failed(display_failure, "Motor Interlock Enabled");
23 }
24
25 //如果pre arm checks是失能的，就只运行强制检查
26 if (checks_to_perform == 0)
27 {
28 return mandatory_checks(display_failure);
29 }
30
31 return fence_checks(display_failure)
32 & parameter_checks(display_failure)
33 & motor_checks(display_failure)
34 & pilot_throttle_checks(display_failure)
35 & oa_checks(display_failure)
36 & gcs_failsafe_check(display_failure) &
37 AP_Arming::pre_arm_checks(display_failure);
38 }
39 bool AP_Arming::pre_arm_checks(bool report)
40 {
41 return hardware_safety_check(report)
42 & barometer_checks(report)
43 & ins_checks(report)
44 & compass_checks(report)
45 & gps_checks(report)
46 & battery_checks(report)
47 & logging_checks(report)
48 & manual_transmitter_checks(report)
49 & mission_checks(report)
50 & rangefinder_checks(report)
51 & servo_checks(report)
52 & board_voltage_checks(report)
53 & system_checks(report)
54 & can_checks(report)
55 & generator_checks(report)
56 & proximity_checks(report)
57 & camera_checks(report)
58 & visodom_checks(report)
59 & aux_auth_checks(report)
60 & disarm_switch_checks(report);
61 }
1 void AP_Arming_Copter::set_pre_arm_check(bool b)
2 {
3 copter.ap.pre_arm_check = b;
4 AP_Notify::flags.pre_arm_check = b;
5 }
# AP_Terrain
1 void Copter::terrain_logging()
2 {
3 terrain.log_terrain_data();
4 }
5
6
7 AP_Terrain terrain{mode_auto.mission};
8
9 class ModeAuto : public Mode
10 {
11 AP_Mission mission
12 {
13 FUNCTOR_BIND_MEMBER(&ModeAuto::start_command, bool, const AP_Mission::Mission_Command &),
14 FUNCTOR_BIND_MEMBER(&ModeAuto::verify_command, bool, const AP_Mission::Mission_Command &),
15 FUNCTOR_BIND_MEMBER(&ModeAuto::exit_mission, void)
16 };
17 }
18
19
20
21 //构造函数
22 AP_Mission(mission_cmd_fn_t cmd_start_fn, mission_cmd_fn_t cmd_verify_fn, mission_complete_fn_t mission_complete_fn) :
23 _cmd_start_fn(cmd_start_fn),
24 _cmd_verify_fn(cmd_verify_fn),
25 _mission_complete_fn(mission_complete_fn),
26 _prev_nav_cmd_id(AP_MISSION_CMD_ID_NONE),
27 _prev_nav_cmd_index(AP_MISSION_CMD_INDEX_NONE),
28 _prev_nav_cmd_wp_index(AP_MISSION_CMD_INDEX_NONE),
29 _last_change_time_ms(0)
30 {
31 }
32
33
34
35 class AP_Terrain
36 {
37 }
38
39 void AP_Terrain::log_terrain_data()
40 {
41 Location loc;
42 AP::ahrs().get_position(loc)
43 struct log_TERRAIN pkt
44 AP::logger().WriteBlock(&pkt, sizeof(pkt));
45 }ArduPilot中Copter的Parm_motors_check()
copter的arm_motors_check()任务定义在任务列表中
1 //频率10hz，最长时间50
2 SCHED_TASK(arm_motors_check, 10, 50),
arm_motors_check
1 // 检查pilot的输入，arm或disarm copter
2 void Copter::arm_motors_check()
3 {
4 //检查是否使用rudder arming/disarm 是允许的
5 AP_Arming::RudderArming arming_rudder = arming.get_rudder_arming_type();
6
7 // 确保油门是关闭
8 if (channel_throttle->get_control_in() > 0)
9 {
10 arming_counter = 0;
11 return;
12 }
13 int16_t yaw_in = channel_yaw->get_control_in();
14
15 //完全右
16 if (yaw_in > 4000)
17 {
18 }
19 else if( (yaw_in < -4000) && (arming_rudder == AP_Arming::RudderArming::ARMDISARM))
20 {
21 arming.disarm(AP_Arming::Method::RUDDER);
22 }
23 else
24 {
25 arming_counter = 0;
26 }
27
28 }libraries/ap_arming.h
1 // 舵arm支持
2 enum class RudderArming
3 {
4 IS_DISABLED = 0, // DISABLED leaks in from vehicle defines.h
5 ARMONLY = 1,
6 ARMDISARM = 2
7 };
8
9 enum ArmingChecks
10 {
11 ARMING_CHECK_ALL = (1U << 0),//所有
12 ARMING_CHECK_BARO = (1U << 1),//气压
13 ARMING_CHECK_COMPASS = (1U << 2),//指南针
14 ARMING_CHECK_GPS = (1U << 3),//GPS
15 ARMING_CHECK_INS = (1U << 4), //惯性导航
16 ARMING_CHECK_PARAMETERS = (1U << 5), //参数
17 ARMING_CHECK_RC = (1U << 6), //rc
18 ARMING_CHECK_VOLTAGE = (1U << 7), //电压
19 ARMING_CHECK_BATTERY = (1U << 8), //电池
20 ARMING_CHECK_AIRSPEED = (1U << 9), //空速计
21 ARMING_CHECK_LOGGING = (1U << 10), //日志
22 ARMING_CHECK_SWITCH = (1U << 11), //开关
23 ARMING_CHECK_GPS_CONFIG = (1U << 12), //GPS配置
24 ARMING_CHECK_SYSTEM = (1U << 13), //系统
25 ARMING_CHECK_MISSION = (1U << 14), //任务
26 ARMING_CHECK_RANGEFINDER = (1U << 15), //测距仪
27 ARMING_CHECK_CAMERA = (1U << 16), //视频
28 ARMING_CHECK_AUX_AUTH = (1U << 17),
29 ARMING_CHECK_VISION = (1U << 18), //视觉
30 ARMING_CHECK_FFT = (1U << 19), //FFT
31 };
32
33
34 enum class Method
35 {
36 RUDDER = 0,
37 MAVLINK = 1,
38 AUXSWITCH = 2,
39 MOTORTEST = 3,
40 SCRIPTING = 4,
41 TERMINATION = 5, // only disarm uses this...
42 CPUFAILSAFE = 6, // only disarm uses this...
43 BATTERYFAILSAFE = 7, // only disarm uses this...
44 SOLOPAUSEWHENLANDED = 8, // only disarm uses this...
45 AFS = 9, // only disarm uses this...
46 ADSBCOLLISIONACTION = 10, // only disarm uses this...
47 PARACHUTE_RELEASE = 11, // only disarm uses this...
48 CRASH = 12, // only disarm uses this...
49 LANDED = 13, // only disarm uses this...
50 MISSIONEXIT = 14, // only disarm uses this...
51 FENCEBREACH = 15, // only disarm uses this...
52 RADIOFAILSAFE = 16, // only disarm uses this...
53 DISARMDELAY = 17, // only disarm uses this...
54 GCSFAILSAFE = 18, // only disarm uses this...
55 TERRRAINFAILSAFE = 19, // only disarm uses this...
56 FAILSAFE_ACTION_TERMINATE = 20, // only disarm uses this...
57 TERRAINFAILSAFE = 21, // only disarm uses this...
58 MOTORDETECTDONE = 22, // only disarm uses this...
59 BADFLOWOFCONTROL = 23, // only disarm uses this...
60 EKFFAILSAFE = 24, // only disarm uses this...
61 GCS_FAILSAFE_SURFACEFAILED = 25, // only disarm uses this...
62 GCS_FAILSAFE_HOLDFAILED = 26, // only disarm uses this...
63 TAKEOFFTIMEOUT = 27, // only disarm uses this...
64 AUTOLANDED = 28, // only disarm uses this...
65 PILOT_INPUT_FAILSAFE = 29, // only disarm uses this...
66 TOYMODELANDTHROTTLE = 30, // only disarm uses this...
67 TOYMODELANDFORCE = 31, // only disarm uses this...
68 }
69
70 enum class Required
71 {
72 NO = 0,
73 YES_MIN_PWM = 1,
74 YES_ZERO_PWM = 2
75 };copter.h
1 //arm/disarm管理类
2 AP_Arming_Copter arming;
ArduCopter/ap_arming.h
1 class AP_Arming_Copter : public AP_Arming
2 {
3
4 }
5
6 //disarm电机
7 bool AP_Arming_Copter::disarm(const AP_Arming::Method method)
8
9
10
11 //当disarm成功发生，返回真
12 bool AP_Arming::disarm(const AP_Arming::Method method)
13 {
14 }copter.h
1 struct
2 {
3 uint32_t last_heartbeat_ms;
4 //从CGS收到的 最后HEARTBEAT时间，用来触发gcs failsafe
5 uint32_t terrain_first_failure_ms;
6 //第一次地形数据访问失败的时间，用来计算failsafe的持续时间
7 uint32_t terrain_last_failure_ms;
8 //最近地形数据访问错误的时间
9 int8_t radio_counter;
10 //油门值低于throttle_fs_value的迭代次数
11 uint8_t radio : 1;
12 // radio failsafe的标志
13 uint8_t gcs : 1;
14 //GCS failsafe的一个状态标志
15 uint8_t ekf : 1;
16 //ekf failsafe
17 uint8_t terrain : 1;
18 //丢失的地形数据failsafe发生
19 uint8_t adsb : 1;
20 //adsb相关的failsafe已经发生
21 } failsafe;libraries/ap_ahrs.h
1 class AP_AHRS
2 {
3 }libraries/ap_ahrs_dcm.h
1 class AP_AHRS_DCM : public AP_AHRS
2 {
3 };libraries/ap_ahrs_NavEKF.h
1 class AP_AHRS_NavEKF : public AP_AHRS_DCM
2 {
3 };