本篇文章的结论最后发现是错的，实际问题的根源在于icm20600的spi mode为3,而我的fpga_pwm为0。因此快速切换的时候会有问题。记录错误的知识对世界没有帮助，但是对我个人而言，踩坑思路变化有记录的价值，因此我仍然将本篇文章留下。
之前Autopilotpi的FPGA实现为一个I2C_PWM模块，并在PX4上增加了相应的驱动，倒也可以正常飞，但是频率只能跑到200+ hz。
于是考虑到把FPGA实现为SPI_PWM，这样SPI 10Mhz的通信速度比I2c 100Khz可是足足提高了100倍。
复习了一下Spinal HDL，并把原来的fpga代码重构了一下，最后效果还可以，仿真中，sclk最高可以支持到1/3的主频，相当于如果主频60M，那么sclk可以跑到20M。
在上板子实际调试时，还是发现了几个RTL的bug的：
- 读写时序有点问题，因为之前我测试都是按最高速度测试的，实际上板时才跑了5Mhz，因此出现了一些问题。最后在仿真中确保高速和低速现象一致，才继续上板调试。
- 未把miso设置为inout的，结果miso变成了两个信号，分别是miso_write 和miso_enable。我只把miso_write接了出去。这就导致，即使我cs拉高了，fpga仍然会输出高电平。实际上此时fpga应该把miso设置为输入，否则为影响其他spi模块
解决了这两个问题后，开始移植px4上的驱动。调试的时候却发现，一旦我这个fpga-pwm的驱动起来，icm20600这个陀螺仪就直接超时了。难道是这个fpga启动时间太长？导致陀螺仪超过1s没更新数据？但是不太可能，毕竟这个fpga-pwm最多就写几个spi寄存器。又或者是fpga-pwm操作spi太慢了？导致icm20600抢不到时间运行？又或者他的调度出了问题？icm20600调度不到？难道是之前我改调度bug的时候写出来的新bug?但是把那个改动revert掉之后，情况还是一样的。而icm20600和fpga-pwm的优先级是一样的，他们都在spi那个workqueue里被调度，应该是一个执行完才能够轮到另一个。
又或者难道是我fpga-pwm里一次写了太多寄存器？但是也才写了8个寄存器，8*2=16 bytes 一次，算上sclk的频率，怎么都不至于让icm抢不到时间。
或者说是fpga-pwm打断了icm的运行？于是我在icm的RUN函数，进入和出去之前，都做了打印"ii"和"io",同时在fpga-pwm那里的Run里面，也如法炮制，打印"fi","fo"。理论上ii 和io 之间，不应该有fi和fo。
实验出来也确实符合预期，并没有发生打断的情况。
那到底是什么情况呢。于是做了各种实验，在各种条件下重启icm或者fpga。只要正常启动一次：
px4 -d -s wing.config &
之后，把fpga-pwm和icm20600都stop，然后先运行icm20600 start，再运行fpga-pwm start，就没问题，陀螺仪可以很稳健地运行。
但是没有更多头绪了，于是又在wing.config中，更换fpga-pwm的启动顺序，试着放在陀螺仪前面，效果也是一样。但是将fpga-pwm放到最后面后，发现可以正常运行了。
这是个重大发现，从fpga-pwm启动到最后，中间还有几句，其中最重要的一句就是load mixer了。于是我开始猜想跟这个是不是有关系，将fpga-pwm挪到原来的位置，并把load mixer注释掉，发现确实可以正常运行了。
好家伙，问题找到了。那么到底为什么呢？
其实现在想来也很简单，先简要介绍一下px4 这些pwm output module的调用流程。
首先，每个pwm output module 要从OutputModuleInterface这个类继承，该类有两个方法，最重要的就是UpdateOutput了。顾名思义，就是更新pwm的输出值。
同时，这个pwm output module 还要继承自CDev,并注册一个设备/dev/pwm_output。 于是px4可以通过ioctl的方式来控制设备。
在我原来的实现中，我是直接在UpdateOutput内，计算当前需要的pwm输出值为CCR，并直接写入spi 寄存器。而UpdateOutput是别的线程调用的，从结果来反推，应该是rate_ctrl这个workqueue来调用的。之前我一直在Run函数（被wq:SPI0调度）找各种fpga_pwm可能打断icm20600的蛛丝马迹，但其实Run函数里他是不会打断的。因为此时fpga_pwm和icm都由workqueue来调度，两个应该是轮流运行，不会出现打断的情况。
真正的打断是发生在UpdateOutput这个函数里，rate_ctrl每次进来直接就往spi里写寄存器，如果此时icm20600正在使用spi，那么显然就有问题了。
更正，今天又仔细看了下有问题的实现，发现上面原因有些细节不对，比如UpdateOutput在原来的实现中，实际上也是在Run函数里调用的：
void FPGA_PWM_Wrapper::Run()
{
if (should_exit()) {
...
}
perf_begin(_cycle_perf);
switch (_state) {
case STATE::INIT:
_state = STATE::RUNNING;
ScheduleOnInterval(1000000 / _schd_rate_limit, 1000000 / _schd_rate_limit);
break;
case STATE::RUNNING:
_mixing_output.update();
_mixing_output.updateSubscriptions(false);
break;
}
perf_end(_cycle_perf);
}在_mixing_output.update(); 中，实际上就会调用UpdateOutput，看看它的注释：
/**
* Call this regularly from Run(). It will call interface.updateOutputs().
* @return true if outputs were updated
*/
bool update();也就是说，原来的实现中，也是在Run中做写入寄存器操作的，但是经过实验，确实是发现陀螺仪是被打断的，那么打断到底是发生在哪里呢？
仔细看一下_mixing_output的构造函数：
MixingOutput::MixingOutput(const char *param_prefix, uint8_t max_num_outputs, OutputModuleInterface &interface,
SchedulingPolicy scheduling_policy, bool support_esc_calibration, bool ramp_up)
: ModuleParams(&interface),
_output_ramp_up(ramp_up),
_control_subs{
{&interface, ORB_ID(actuator_controls_0)},
{&interface, ORB_ID(actuator_controls_1)},
{&interface, ORB_ID(actuator_controls_2)},
{&interface, ORB_ID(actuator_controls_3)},
},发现有个_control_subs的变量，这个变量定义为：uORB::SubscriptionCallbackWorkItem _control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
再看看它的构造函数：
/**
* Constructor
*
* @param work_item The WorkItem that will be scheduled immediately on new publications.
* @param meta The uORB metadata (usually from the ORB_ID() macro) for the topic.
* @param instance The instance for multi sub.
*/
SubscriptionCallbackWorkItem(px4::WorkItem *work_item, const orb_metadata *meta, uint8_t instance = 0) :
SubscriptionCallback(meta, 0, instance), // interval 0
_work_item(work_item)
{
}它的构造函数接收一个WorkItem,在上面中，就是接收一个OutputInterface，这是一个Module。
因此，打断的原理也很简单，这个_control_subs订阅了actuator_control，一旦有新消息来了，就会调度WorkItem来运行，这样可以减少这个消息的响应延迟，但是在这个例子中，会打断陀螺仪的读写。
好了，知道问题之后，怎么解决呢？简单来说，就是不让MixingOutput来调度。在最新的实现如下：
class FPGA_SPI_PWM :public device::SPI,public I2CSPIDriver<FPGA_SPI_PWM>,public OutputModuleInterfaceWrapper
{
public:
FPGA_SPI_PWM(const I2CSPIDriverConfig &config);主要使用I2CSPIDriver来调度，OutputModuleInterfaceWrapper是OutputModuleInterface的一层包装。由于I2CSPIDriver和OutputModuleInterfaceWrapper都是从WorkItem继承而来的，遇到了c++很有名的菱形继承问题，简单来说，就是在WorkItem中，Run是个纯虚函数。而I2CSPIDriver中已经实现了，并设置为了final,而OutputModuleInterface没有实现Run，需要用户自己实现。但是一旦用户自己实现了一个Run，编译器又会认为你实现了一个已经设置为final的方法。
于是，就实现了一个Wrapper，继承自OutputModuleInterface，并且在里面实现一个空的Run，于是FPGA_SPI_PWM继承自它后，编译器就不会再让你实现Run了。
好了，上面这么多，只是说明了为什么要Wrap，但是现在有个问题，就是我这个类实际上包含了两个WorkItem, 而我们真正想用的是I2CSPIDriver里面那个，那另一个咋整呢？答案是在Init方法中把他扔掉：
int FPGA_SPI_PWM::init(){
SPI::init();
writeReg(CONFIG_REG,0x01); // set predivider = 2
register_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH); // register /dev/pwm_output0, then controller could operate it by ioctl
I2CSPIDriver::ScheduleNow();
OutputModuleInterface::Deinit(); // deattach the work_queue item, as we don't use OutputModuleInterface for scheduling
return 0;
}这次的bug折腾了好几天，因为打log实在没能看出些什么东西，最后还是在文件系统里加入了gdb，还学习了一下使用vscode + gdbserver来进行远程调试的方法。以后调试就方便多了。调试手段以后还是要多多更新，多多学习啊，不然调试效率太低了。
./px4-work_queue status
Work Queue: 8 threads RATE INTERVAL
|__ 1) wq:rate_ctrl
| \__ 1) vehicle_angular_velocity 400.2 Hz 2498 us
|__ 2) wq:SPI0
| |__ 1) fpga_pwm_out 400.0 Hz 2500 us (2500 us)
| \__ 2) icm20602 400.2 Hz 2498 us (2500 us)
|__ 3) wq:I2C0
| \__ 1) qmc5883l 50.0 Hz 20000 us (20000 us)
|__ 4) wq:nav_and_controllers
| |__ 1) ekf2_selector 10.0 Hz 100091 us
| |__ 2) flight_mode_manager 0.0 Hz 0 us
| |__ 3) fw_att_control 0.0 Hz 0 us
| |__ 4) sensors 200.1 Hz 4997 us
| |__ 5) vehicle_acceleration 200.1 Hz 4997 us
| |__ 6) vehicle_air_data 20.0 Hz 50087 us
| |__ 7) vehicle_gps_position 3.3 Hz 300068 us
| \__ 8) vehicle_magnetometer 48.7 Hz 20527 us
|__ 5) wq:INS0
| |__ 1) ekf2 200.1 Hz 4997 us
| \__ 2) vehicle_imu 200.1 Hz 4997 us
|__ 6) wq:hp_default
| |__ 1) manual_control 5.0 Hz 200087 us
| \__ 2) rc_update 0.0 Hz 0 us
|__ 7) wq:ttyS1
| \__ 1) rc_input 250.0 Hz 4000 us (4000 us)
\__ 8) wq:lp_default
\__ 1) load_mon 2.0 Hz 499978 us (500000 us)