|
本帖最后由 点点&木木 于 2019-4-12 23:57 编辑 硬件组件 STM32 Nucleo-F746ZG × 1 博世BMI088 × 13 跳线（通用） × 1 手动工具和制造机器 烙铁（通用） 焊料空气流动站 信号分析仪 介绍 在这个项目中，我描述了我在Ada中的小型奥德赛，用于嵌入式ARM从头开始创建冗余IMU（使用多个6DoF陀螺仪/加速传感器）和软件环境来控制它。 我还将向您展示我用来达到目标的工具和方法（调试，分析器......）来估算一组6DoF IMU的音高和滚动： file:///C:/Users/ADMINI~1/AppData/Local/Temp/msohtmlclip1/01/clip_image001.png
局限性： 在本教程中，我没有描述控制火箭副翼或如何减少偏航旋转的PID过程，而只描述如何获取数据并估算滚转和俯仰。工作正在进行中，将在单独的教程中进行描述。 动机： 对于正在进行的业余火箭项目，我们需要一个能够容忍各种加速度（和陀螺仪）的IMU，而无需在飞行范围内重新配置。此外，我们希望有一个多IMU板用于可靠性和过滤目的。 出于这个原因，我设计了一个冗余的BMI088（13x IMU），试图在高振动环境中获得最佳结果，一个业余火箭和高精度无人机。 在我们的使用案例中，它将被放置在如下的火箭中，连接到托管微控制器的板，该微控制器能够控制和处理来自传感器的数据。 
使用AdaCore框架和工具链将使我们能够从他们提供的强大编码环境中受益（在安全/可靠性导向的行业中得到高度认可，如航空航天和运输）。 我注意到在代码实现过程中，Ada代码，特别是在AdaCore GitHubrepo中编辑的代码很容易阅读。对于像我这样习惯于C /C ++和java环境的人来说，使用Ada并不是特别难。 语境： SuperIMU已被开发用于CNRS PlaneteScience的业余火箭队。它将有助于在高速和变化的振动环境中计算精确俯仰/滚转，以在飞行期间使用副翼稳定火箭。 
火箭的第一个目标 SuperIMU必须嵌入芯径小于90mm中。 
第1步：系统描述 SuperIMU具有13倍冗余6DoF IMU，可以直径50mm。它将使用SPI接口与微控制器通信，以获取原始数据（加速度和陀螺仪数据）。 
板载配置 我们使用简单的架构来可视化和测试SuperIMU。 微控制器使用SPI端口从SuperIMU过滤原始数据（加速度，角度速率），并估算角度。出于测试目的，它使用UART端口发送角度值。 
测试configuartion UART端口连接到计算机，以串行到USB模块可视化数据。 该项目的下一步是为SuperIMU创建一个特定的主机板。主机板将是圆形的，并嵌入STM32F746微控制器。 IMU按以下方式分布，呈星形： 
角度分布遵循45°模式： 
选择了先前的模式以最大化表面上IMU的存在。根据在德克萨斯大学（RedundantIMU Configurations：paper）进行的几何研究，其中测试了不同的几何配置： 传感器的3D空间中的几何分布比计划中的分布更好的结果（我们的情况） 分布对加速度没有影响（aDOP Accelaration Dilution of Precision） 分布对旋转精度有影响（wDOP角精度稀释） 我们添加的IMU越多，我们的精确度就越高 IMU的方向对结果没有显着影响 
我们记住这个研究，为了达到像配置这样的立方体，我们可以将两个SuperIMU并行叠加。目前我们继续使用UnitCircle配置，尝试从中获取最大值。 
灵敏度范围配置：火箭模式 在业余火箭中，在飞行的最初几秒内，加速度可以达到8到9克。我们在滑槽开口和落地时观察到了一阵骚动。 这是一个典型的飞行日志（加速度/速度）的业余火箭与三个阶段（推进，上升无推进，滑槽开放，潜水与滑槽，地面接触）： 
在短途飞行期间，典型的加速曲线如下： 发射<7秒：加速度> 6g 在降落伞开启前<11s，无推进上升：<3g 用降落伞潜水<50s：<3 
在飞行期间，感应范围需要容忍低和高，以无缝地进行精确控制。IMU的范围将在发布前配置。目前我将尝试四种不同的配置，当然这些配置不是确定的，它们需要通过实验进行调整： 
IMU分布，范围和名称 在飞行期间，将根据配置的范围忽略传感器原始值： 
过滤过程 在开始使用它们的俯仰/滚转估计之前，根据它们的范围过滤来自IMU的采集数据（加速度，角速度）。 范围过滤器： 过滤器获取原始数据，并根据每个IMU的配置范围，仅输出低于阈值的值：配置范围的95％。 范围滤波器还根据配置的范围提供每个测量数据的权重。权重用于取消高范围值的影响，它由平均过滤器使用。 权重具有以下值：0,1 / 2,1 / 4,1 / 8。 例如，如果IMU的范围是3G并且测量的加速度是2.9G，则范围滤波器输出来自该IMU的值为0的权重。 
例如，如果测量的加速度<1G，则最准确的IMU是配置有3G范围的IMU。3G IMU的重量为1，6G IMU的重量为1/2，12G IMU的重量为1/4，24G IMU的重量为1/8。 权重根据下表计算： 
例外，当测量数据> 24％> 95％时，它不会为零加权。它会避免产生价值。 范围滤波器的输出值是每个IMU的四倍（加速度，加速度，陀螺仪，陀螺仪等）。 平均过滤器： 该过滤器根据每个重量计算平均值。当权重为空时，不考虑测量值。 平均滤波器输出滤波数据，该数据是3轴的耦合（accel_filtered-xyz，gyro_filtered-xyz）。 滚动/间距估算器： 然后使用经过滤的数据来估计滚转角和俯仰角。使用众所周知的互补滤波器来估计飞行角度。 
TimeSampling 目标st具有估计角度@ 1ms的采样率。这意味着我们需要测量所有13xIMU，在1ms的时间范围内进行滤波和补充估计。 第2步：使用硬件 IMU传感器BMI088 使用的传感器是BMI088，它专为无人机和机器人设计： 它可以承受24g的加速度。 SPI运行速度最高为10Mhz。 分辨率：0.09mg，0.004°/ s 低TCO为0.2 mg / K，低光谱噪声仅为230 pg / sqrt（Hz）最大值 输出数据速率：1.6kHz - 每个加速度为0.6ms - 陀螺仪最大为2000Hz - 每个0.5ms Nucleo-F746ZG的描述： 核板最有趣的是它们嵌入了ST Link V2-1编程接口以允许调试（例如使用OpenOCD或St-Util）。 它运行@ 216Mhz，非常适合需要快速采集和计算以获得最佳采样时间的用例。 您只需使用micro usb电缆将电路板连接到计算机。对于我的情况，我使用Ubuntu 18.x，自动检测到电路板，无需安装任何驱动程序。 
Nucleo主板的另一个有趣之处在于它们与ArduinoUno标头保持兼容。在下面以绿色（D0 ... D13）呈现。 
资料来源：https：//os.mbed.com/platforms/ST-Nucleo-F746ZG/ 由于我们需要在读取BMI088传感器（13x IMU）时将延迟降至最大，因此我们使用SPI端口获取Accel和Gyro数据（在这种情况下，I2C不适用）。 对于代码，我们将使用STM32F746的SPI1： SCK：PA_5 MISO：PA_6 MOSI：PA_7 Super IMU的描述： SuperIMU基于BMI088，它使用SPI接口配置和获取IMU的数据。从数据表中，用户电路如下： 
在SuperIMU的情况下，没有使用中断，它按顺序获取数据。ACCEL和GYRO的SDO在电路板中输出相同的输出。 我必须单独选择每个Gyro和Accel。 
AdaIMU BMI088 Circle（SuperIMU）的原理图 SuperIMU的设计可以使用单个SPI获取数据（下一个版本将使用多个SPI），它具有13x BMI088： 
这是它的样子。我需要焊接接头以连接杜邦线： 
在后面有引脚的描述，SPI（SDO / SCK / SDI）以及用于选择陀螺仪和引脚的引脚： 
完成焊接后的样子如下： 
La pieuvre： 
SuperIMU使用3v3电源和SPI端口1连接到Nucleo板。 
GPIO引脚用于选择Gyro，13x IMU的Accel将按如下方式完成： 
用于选择IMU的陀螺仪/加速度计的GPIO引脚的定义： -- IMU select IMU1_SPI1_SELC : GPIO_Point renames PC8; IMU1_SPI1_SELG : GPIO_Point renames PC9; IMU2_SPI1_SELC : GPIO_Point renames PC10; IMU2_SPI1_SELG : GPIO_Point renames PC11; IMU3_SPI1_SELC : GPIO_Point renames PC12; IMU3_SPI1_SELG : GPIO_Point renames PD2; IMU4_SPI1_SELC : GPIO_Point renames PG2; IMU4_SPI1_SELG : GPIO_Point renames PG3; IMU5_SPI1_SELC : GPIO_Point renames PD7; IMU5_SPI1_SELG : GPIO_Point renames PD6; IMU6_SPI1_SELC : GPIO_Point renames PD5; IMU6_SPI1_SELG : GPIO_Point renames PD4; IMU7_SPI1_SELC : GPIO_Point renames PD3; IMU7_SPI1_SELG : GPIO_Point renames PE2; IMU8_SPI1_SELC : GPIO_Point renames PE4; IMU8_SPI1_SELG : GPIO_Point renames PE5; IMU9_SPI1_SELC : GPIO_Point renames PE6; IMU9_SPI1_SELG : GPIO_Point renames PE3; IMU10_SPI1_SELC : GPIO_Point renames PF8; IMU10_SPI1_SELG : GPIO_Point renames PF7; IMU11_SPI1_SELC : GPIO_Point renames PF9; IMU11_SPI1_SELG : GPIO_Point renames PG1; IMU12_SPI1_SELC : GPIO_Point renames PA3; IMU12_SPI1_SELG : GPIO_Point renames PC0; IMU13_SPI1_SELC : GPIO_Point renames PC3; IMU13_SPI1_SELG : GPIO_Point renames PF3; IMU_SPI1_SEL_Points : constant STM32.GPIO.GPIO_Points := (IMU1_SPI1_SELC, IMU1_SPI1_SELG, IMU2_SPI1_SELC, IMU2_SPI1_SELG, IMU3_SPI1_SELC, IMU3_SPI1_SELG, IMU4_SPI1_SELC, IMU4_SPI1_SELG, IMU5_SPI1_SELC, IMU5_SPI1_SELG, IMU6_SPI1_SELC, IMU6_SPI1_SELG, IMU7_SPI1_SELC, IMU7_SPI1_SELG, IMU8_SPI1_SELC, IMU8_SPI1_SELG, IMU9_SPI1_SELC, IMU9_SPI1_SELG, IMU10_SPI1_SELC, IMU10_SPI1_SELG, IMU11_SPI1_SELC, IMU11_SPI1_SELG, IMU12_SPI1_SELC, IMU12_SPI1_SELG, IMU13_SPI1_SELC, IMU13_SPI1_SELG); IMU_SPI1_Accel_SEL_Points : array (1 .. 13) of access GPIO_Point'Class := ( IMU1_SPI1_SELC'Access, IMU2_SPI1_SELC'Access, IMU3_SPI1_SELC'Access, IMU4_SPI1_SELC'Access, IMU5_SPI1_SELC'Access, IMU6_SPI1_SELC'Access, IMU7_SPI1_SELC'Access, IMU8_SPI1_SELC'Access, IMU9_SPI1_SELC'Access, IMU10_SPI1_SELC'Access, IMU11_SPI1_SELC'Access, IMU12_SPI1_SELC'Access, IMU13_SPI1_SELC'Access ); IMU_SPI1_Gyro_SEL_Points : array (1 .. 13) of access GPIO_Point'Class := ( IMU1_SPI1_SELG'Access, IMU2_SPI1_SELG'Access, IMU3_SPI1_SELG'Access, IMU4_SPI1_SELG'Access, IMU5_SPI1_SELG'Access, IMU6_SPI1_SELG'Access, IMU7_SPI1_SELG'Access, IMU8_SPI1_SELG'Access, IMU9_SPI1_SELG'Access, IMU10_SPI1_SELG'Access, IMU11_SPI1_SELG'Access, IMU12_SPI1_SELG'Access, IMU13_SPI1_SELG'Access ); 第3步：软件设置/ IDE 使用Adacore GPS IDE 我从Adacore网站安装了GPS IDE：gnat-community-2018-20180528-x86_64-linux-bin，它有一个很酷的编辑器，包括debuging界面和Ada和SPARK的所有检查环境。 我从Adacore网站安装了Arm-ELF编译器：gnat-community-2018-20180524-arm-elf-linux64-bin，它为ARM（以及bb-runtimes）提供编译器和构建器工具链。 使用项目的代码 要使用该软件，您需要克隆此分支http://github.com/LaetitiaEl/Ada_Drivers_Library，此存储库基于Ada驱动程序库：AdaCore /Ada_Drivers_Library 使用./GNAT/2018/bin/gps 打开项目文件：Ada_Drivers_Library / examples /STM32F746_Nucleo / SuperIMU.gpr 你编译/上传，这就是全部！ 该项目针对STM32F746，如果需要可以轻松移植到STM32F4（通过github或黑客联系我寻求帮助）。 第4步：首先进行眨眼测试！ 在继续之前，我需要确保可以正确编程电路板。 我保留了现有文件：blinky_f7disco.gpr，并修改了以下路径。继续使用stm32f746_discovery_full.gpr没问题 blinky_f7disco使用以下主条目： Ada_Drivers_Library/examples/shared/hello_world_blinky/src/blinky.adbblinky的内容如下： with STM32.Board; use STM32.Board;with STM32.GPIO; use STM32.GPIO; with Ada.Real_Time; use Ada.Real_Time; procedure Blinky is Period : constant Time_Span := Milliseconds (200); -- arbitrary Next_Release : Time := Clock; begin STM32.Board.Initialize_LEDs; loop Toggle (All_LEDs); Next_Release := Next_Release + Period; delay until Next_Release; end loop; end Blinky; 似乎这个代码对所有主板都是通用的。使用GPS IDE我遵循All_LED的定义，它定义于： Ada_Drivers_Library/boards/stm32_common/stm32f746disco/stm32-board.ads值All_LEDs是索引为1的集合，指向PI1： Green_LED : User_LED renames PI1;LED1 : User_LED renames Green_LED; LCH_LED : User_LED renames Green_LED; All_LEDs : GPIO_Points := (1 => Green_LED); 在Nucleo板上没有LED连接到PI1，找出我可以使用的输出，简单的方法是检查核板的原理图http://www.st.com/resource/en/sc ... leo_144pins_sch.zip 
我把蓝色LED连接到PB7。然后代码更新为蓝色：Blue_LED : User_LED renames PB7; LED1 : User_LED renames Blue_LED; LCH_LED : User_LED renames Blue_LED; All_LEDs : GPIO_Points := (1 => Blue_LED); 然后使用上传按钮上传代码（重要的是您需要配置上传/调试，在我的情况下，我使用OpenOCD，如下所述） 
代码编译得很好，正确上传并且蓝色LED闪烁： 
让我们开始认真的工作。为BMI088制作SPI驱动程序。 第5步：为BMI088制作驱动程序 很酷的是，Ada_Drivers_Library已经在“components / src / motion”目录中提供了一些SPI IMU示例。 我只需要使用l3gd20来启动bmi088的驱动程序以获得灵感。 
要正确选择寄存器和协议特性，请使用以下资源： - BST-BMI088-DS001- officiel C驱动程序：有助于获得时间 有关信息，现有驱动程序的演示位于路径中：Ada_Drivers_Library/ arch / ARM / STM32 / driver_demos / 更新板定义：定义SPI1在STM32.Board包体中的文件stm32-board.ads中，我们添加： -- SPI1 / IMU BMI088 example -- --------------------------------- -- SPI Port IMU_SPI : SPI_Port renames SPI_1; -- SPI IOs SPI1_SCK : GPIO_Point renames PA5; -- D13 in connector CN10 SPI1_MISO : GPIO_Point renames PA6; -- D14 in connector CN10 SPI1_MOSI : GPIO_Point renames PA7; -- D15 in connector CN10 -- IMU select IMU_SEL1 : GPIO_Point renames PG9; -- D0 in connector CN10 -- To initialize the SPI1 port for the BMI088 procedure Initialize_SPI1_For_BMI088; 在STM32F7中使用8位SPI的技巧 BIM088与8位通信非常重要。但是数据tx / rx DR寄存器是16位，默认情况下它发送16位。首先写入/读取：NOK 
根据参考手册，诀窍是将寄存器DS（数据大小）配置为4位，并正确格式化DR寄存器中写入的数据。 
这就是为什么我更新了SPI（stm32-spi.adb）的默认驱动程序，如下面的写/读8位模式。 begin ... This.Periph.CR1.DFF := Conf.Data_Size = HAL.SPI.Data_Size_16b; This.Periph.CR2.DS := STM32_SVD.SPI.Size_4Bit; -- add this line This.Periph.CR1.CPOL := Conf.Clock_Polarity = High; ... procedure Send_8bit_Mode (This : in out SPI_Port; Outgoing : HAL.SPI.SPI_Data_8b) is Tx_Count : Natural := Outgoing'Length; Index : Natural := Outgoing'First; Tmp : UInt16; begin if Current_Mode (This) = Slave or else Tx_Count = 1 then Tmp := UInt16 ((Outgoing (Index) and 16#0F#))*256 + UInt16((Outgoing (Index) and 16#F0#))/16; This.Periph.DR.DR := Tmp; Index := Index + 1; Tx_Count := Tx_Count - 1; end if; ... procedure Receive_8bit_Mode (This : in out SPI_Port; Incoming : out HAL.SPI.SPI_Data_8b) is Generate_Clock : constant Boolean := Current_Mode (This) = Master; Tmp : Uint16; begin for K of Incoming loop if Generate_Clock then This.Periph.DR.DR := 0; end if; while Rx_Is_Empty (This) loop null; end loop; Tmp := This.Periph.DR.DR; K := UInt8 (Tmp and 16#000F#)*16+ UInt8 ((Tmp and 16#0F00#)/256); ... 这样我获得了更好的时钟，BMI088寄存器中的读/写在8位模式下工作正常： 
但还是不行： 因为我们需要在读取加速度计时（不是陀螺仪的情况）总是丢弃BMI088中的第一个接收字节。这就是为什么每次我进行读取时，我都必须先读取虚拟字节。通过此更新，我获得了更好的结果。 在BMI088.adb中： ---------- -- Read -- ---------- procedure Accel_Read (This : Six_Axis_Imu; Addr : Register; Data : out UInt8) is Status : SPI_Status; Tmp_Data : SPI_Data_8b (1 .. 1); begin SPI_Mode (This, Enabled => True); -- select the chip This.Port.Transmit (SPI_Data_8b'(1 => UInt8 (Addr) or ReadWrite_CMD), Status); if Status /= Ok then raise Program_Error; end if; This.Port.Receive (Tmp_Data, Status); -- dummy constraint BMI088 This.Port.Receive (Tmp_Data, Status); if Status /= Ok then raise Program_Error; end if; Data := Tmp_Data (Tmp_Data'First); SPI_Mode (This, Enabled => False); -- unselect the chip end Read; ---------------- -- Read_UInt8s -- ---------------- procedure Accel_Read_UInt8s (This : Six_Axis_Imu; Addr : Register; Buffer : out SPI_Data_8b; Count : Natural) is Index : Natural := Buffer'First; Status : SPI_Status; Tmp_Data : SPI_Data_8b (1 .. 1); begin SPI_Mode (This, Enabled => True); This.Port.Transmit (SPI_Data_8b'(1 => UInt8 (Addr) or ReadWrite_CMD), Status); if Status /= Ok then raise Program_Error; end if; This.Port.Receive (Tmp_Data, Status); -- dummy constraint BMI088 for K in 1 .. Count loop This.Port.Receive (Tmp_Data, Status); if Status /= Ok then raise Program_Error; end if; Buffer (Index) := Tmp_Data (Tmp_Data'First); Index := Index + 1; end loop; SPI_Mode (This, Enabled => False); end Read_UInt8s; 最好从寄存器中读取加速数据。在下图中，我们仅表示SDO和时钟（SDI未表示，我只有2个探测......：'（）： 
对于陀螺仪数据，虚拟字节没有问题。 这就是为什么必须要有Accel_Read和Gyro_Read不同的程序。 初始化BMI088的步骤 首先，您需要通过调用以下过程正确初始化SPI以与BMI088通信： Config : GPIO_Port_Configuration; Config_SPI : SPI_Configuration; begin -- Enable the clocks Enable_Clock (IMU_SPI); Enable_Clock (SPI1_Points); Enable_Clock (IMU_SEL1); -- Configure the SPI IOs Config := (Mode => Mode_AF, AF => GPIO_AF_SPI1_5, AF_Speed => Speed_100MHz, AF_Output_Type => Push_Pull, Resistors => Floating); Configure_IO(SPI1_Points, Config); -- Configure the SPI select chip IOs Config := (Mode => Mode_Out, Speed => Speed_25MHz, Output_Type => Push_Pull, Resistors => Pull_Up); Configure_IO(IMU_SEL_Points, Config); -- Configure the SPI1 port Config_SPI := (Direction => D2Lines_FullDuplex, Mode => Master, Data_Size => HAL.SPI.Data_Size_8b, Clock_Polarity => Low, Clock_Phase => P1Edge, Slave_Management => Software_Managed, Baud_Rate_Prescaler => BRP_32 First_Bit => MSB, CRC_Poly => 7); Configure (IMU_SPI, Config_SPI); STM32.SPI.Enable (IMU_SPI); end Initialize_SPI1_For_BMI088; 然后按照以下步骤获取加速度计（寄存器和命令的值在数据表中定义，您可以在最后的驱动程序中找到它们）： Bmi.Read(ACC_CHIP_ID_ADDR, Result); while Result /= ACC_CHIP_ID loop Bmi.Read(ACC_CHIP_ID_ADDR, Result); delay until Clock + Milliseconds (1); end loop; ---- reset the accel Bmi.Write(ACC_SOFT_RESET_ADDR,ACC_RESET_CMD); delay until Clock + Milliseconds (50); Turn_ON(IMU_SEL1); delay until Clock + Milliseconds (5); Turn_Off(IMU_SEL1); delay until Clock + Milliseconds (5); ---- enable the accel (rundandant to make sure it works) Bmi.Write(ACC_PWR_CNTRL_ADDR,ACC_ENABLE_CMD); delay until Clock + Milliseconds (50); Bmi.Read(ACC_PWR_CNTRL_ADDR, Result); while Result /= ACC_ENABLE_CMD loop Bmi.Write(ACC_SOFT_RESET_ADDR,ACC_RESET_CMD); --delay until Clock + Milliseconds (1); Bmi.Write(ACC_PWR_CNTRL_ADDR,ACC_ENABLE_CMD); --delay until Clock + Milliseconds (50); Bmi.Read(ACC_PWR_CNTRL_ADDR, Result); delay until Clock + Microseconds (4); end loop; ---- enter active mode Bmi.Write(ACC_PWR_CONF_ADDR,ACC_ACTIVE_MODE_CMD); delay until Clock + Milliseconds (5); Bmi.Read(ACC_PWR_CONF_ADDR, Result); while Result /= ACC_ACTIVE_MODE_CMD loop Bmi.Write(ACC_PWR_CONF_ADDR,ACC_ACTIVE_MODE_CMD); delay until Clock + Milliseconds (5); Bmi.Read(ACC_PWR_CONF_ADDR, Result); end loop; ---- set range 24G Bmi.Write(ACC_RANGE_ADDR,RANGE_24G); delay until Clock + Milliseconds (1); Bmi.Read(ACC_RANGE_ADDR, Result); while Result /= RANGE_24G loop Bmi.Read(ACC_RANGE_ADDR, Result); end loop; ---- set default ODR Bmi.Write(ACC_ODR_ADDR,ODR_1600HZ_BW_280HZ); delay until Clock + Milliseconds (1); Bmi.Read(ACC_ODR_ADDR, Result); while Result /= ODR_1600HZ_BW_280HZ loop Bmi.Read(ACC_ODR_ADDR, Result); end loop; ---- check if there is an error Bmi.Read(ACC_ERR_CODE_ADDR, Result); while Result /= 16#0# loop Bmi.Read(ACC_ERR_CODE_ADDR, Result); end loop; ---- read the value loop Bmi.Read_UInt8s(ACC_ACCEL_DATA_ADDR,Received,6); delay until Clock + Microseconds (100); end loop; 按照上面的步骤，我使用来自BMI088的SPI获取值。 步骤6：过滤和估计描述 应用几何变换： SuperIMU中IMU的配置不一样，它们被旋转（45°和90°）： 
只有加速度X和Y受到旋转的影响。陀螺仪没有受到影响（转速是相对的）。 根据每个IMU位置完成旋转变换。旋转按以下过程编码： -------------------- -- Accel rotation -- -------------------- Sqrt2 : constant Float := 1.41421356237; IMU_Alpha_Index : constant array (1 .. 3) of Integer := (1,3,7); IMU_Beta_Index : constant array (1 .. 2) of Integer := (5,11); IMU_Gamma_Index : constant array (1 .. 2) of Integer := (4,9); IMU_Delta_Index : constant array (1 .. 2) of Integer := (2,13); procedure ApplyIMURotation(i : Integer;xf : in out Float; yf : in out Float) is begin if i in IMU_Alpha_Index'range then null; -- we don't change elsif i in IMU_Beta_index'range then xf := -xf; yf := -yf; elsif i in IMU_Gamma_Index'range then xf := yf; yf := xf; elsif i in IMU_Delta_Index'range then xf := -yf; yf := -xf; elsif i = 8 then xf := Sqrt2*(-yf + xf); yf := Sqrt2*(yf + xf); elsif i = 6 then xf := Sqrt2*(-yf - xf); yf := Sqrt2*(-yf + xf); elsif i = 10 then xf := Sqrt2*(yf + xf); yf := Sqrt2*(yf - xf); elsif i = 12 then xf := Sqrt2*(yf - xf); yf := Sqrt2*(-yf - xf); end if; end; 使用范围： IMU的范围不同。每组IMU都有一个特定的范围： 
IMU分布，范围和名称 配置中使用的范围使用以下代码计算： -- Get the range to use -- -------------------------- Range_3G_Index : constant array (1 .. 5) of Integer := (1,3,4,5,2); Range_6G_Index : constant array (1 .. 4) of Integer := (8,6,12,10); Range_12G_Index : constant array (1 .. 2) of Integer := (9,13); Range_24G_Index : constant array (1 .. 2) of Integer := (7,11); function GetAccelRangeConf(i : Integer) return Accel_Range is begin if i in Range_3G_Index'Range then return ACCEL_RANGE_3G; elsif i in Range_6G_Index'Range then return ACCEL_RANGE_6G; elsif i in Range_12G_Index'Range then return ACCEL_RANGE_12G; elsif i in Range_24G_Index'Range then return ACCEL_RANGE_24G; else return ACCEL_RANGE_24G; end if; end; Range_125DPS_Index : constant array (1 .. 5) of Integer := (1,3,4,5,2); Range_500DPS_Index : constant array (1 .. 4) of Integer := (8,6,12,10); Range_1000DPS_Index : constant array (1 .. 2) of Integer := (9,13); Range_2000DPS_Index : constant array (1 .. 2) of Integer := (7,11); function GetGyroRangeConf(i : Integer) return Gyro_Range is begin if i in Range_125DPS_Index'Range then return GYRO_RANGE_125DPS; elsif i in Range_500DPS_Index'Range then return GYRO_RANGE_500DPS; elsif i in Range_1000DPS_Index'Range then return GYRO_RANGE_1000DPS; elsif i in Range_2000DPS_Index'Range then return GYRO_RANGE_2000DPS; else return GYRO_RANGE_500DPS; end if; end; 过滤代码： 根据每个IMU的配置范围对加速度和陀螺仪速率进行滤波。 权重使用以下代码计算： function GetAccelWeight(rng : Accel_Range; val : Sensor_Accel_Data) return Float is weight : Float := 0.0; refaccel : Float := 3.0; refrange : Float := 3.0; begin if ((abs(val(X))<3.0*0.95*G and abs(val(Y))<3.0*0.95*G and abs(val(Z))<3.0*0.95*G)) then refaccel := 3.0; elsif ((abs(val(X))<6.0*0.95*G and abs(val(Y))<6.0*0.95*G and abs(val(Z))<6.0*0.95*G)) then refaccel := 6.0; elsif ((abs(val(X))<12.0*0.95*G and abs(val(Y))<12.0*0.95*G and abs(val(Z))<12.0*0.95*G)) then refaccel := 12.0; elsif ((abs(val(X))<24.0*0.95*G and abs(val(Y))<24.0*0.95*G and abs(val(Z))<24.0*0.95*G)) then refaccel := 24.0; end if; if rng = ACCEL_RANGE_3G then refrange := 3.0; elsif rng = ACCEL_RANGE_6G then refrange := 6.0; elsif rng = ACCEL_RANGE_12G then refrange := 12.0; elsif rng = ACCEL_RANGE_24G then refrange := 24.0; end if; weight := refaccel / refrange; if weight > 1.0 then return 0.0; end if; return refaccel/refrange; end GetAccelWeight; function GetGyroWeight(rng : Gyro_Range; val : Sensor_Gyro_Data) return Float is weight : Float := 0.0; refgyr : Float := 125.0; refrange : Float := 125.0; begin if ((abs(val(X))<125.0*0.95 and abs(val(Y))<125.0*0.95 and abs(val(Z))<125.0*0.95)) then refgyr := 125.0; elsif ((abs(val(X))<250.0*0.95 and abs(val(Y))<250.0*0.95 and abs(val(Z))<250.0*0.95)) then refgyr := 250.0; elsif ((abs(val(X))<500.0*0.95 and abs(val(Y))<500.0*0.95 and abs(val(Z))<500.0*0.95)) then refgyr := 500.0; elsif ((abs(val(X))<1000.0*0.95 and abs(val(Y))<1000.0*0.95 and abs(val(Z))<1000.0*0.95)) then refgyr := 1000.0; elsif ((abs(val(X))<2000.0*0.95 and abs(val(Y))<2000.0*0.95 and abs(val(Z))<2000.0*0.95)) then refgyr := 2000.0; end if; if rng = GYRO_RANGE_125DPS then refrange := 125.0; elsif rng = GYRO_RANGE_250DPS then refrange := 250.0; elsif rng = GYRO_RANGE_500DPS then refrange := 500.0; elsif rng = GYRO_RANGE_250DPS then refrange := 1000.0; elsif rng = GYRO_RANGE_125DPS then refrange := 2000.0; end if; weight := refgyr / refrange; if weight > 1.0 then return 0.0; end if; return refgyr/refrange; end GetGyroWeight; 使用权重，加速度值是所有值的加权平均值： procedure FilterSuperIMU(facc : out Sensor_Accel_Data;fgyr : out Sensor_Gyro_Data) is sumacc : Sensor_Accel_Data; -- cumulated accel value sumgyr : Sensor_Gyro_Data; -- cumulated gyro value sumwacc : Float := 0.0; -- cumulated accel weight sumwgyr : Float := 0.0; -- cumulated accel weight macc : Sensor_Accel_Data; -- measured accel value mgyr : Sensor_Gyro_Data; -- measured accel value mwacc : Float := 0.0; -- measured accel weight mwgyr : Float := 0.0; -- measured accel weight begin sumacc := NullAccelRate; sumgyr := NullGyroRate; for i in Integer range 1 .. BMIs'Length loop -- get accelerartion BMIs(i).ReadAccelRates(macc); -- get weight according to the range mwacc := GetAccelWeight(BMIs(i).GetCurrentAccelRange, macc); -- apply 45° and inversion tranformation (geometry of the IMU) ApplyIMURotation(i,macc(X),macc(Y)); -- update the cumulated sum sumacc(X) := sumacc(X) + macc(X)*mwacc; sumacc(Y) := sumacc(Y) + macc(Y)*mwacc; sumacc(Z) := sumacc(Z) + macc(Z)*mwacc; sumwacc := sumwacc + mwacc; -- get the gyro rate -- we don't rotate the gyro rate, no need BMIs(i).ReadGyroRates(mgyr); -- compute the weight according to the range mwgyr := GetGyroWeight(BMIs(i).GetCurrentGyroRange, mgyr); -- update the cummulated weighted sum sumgyr(X) := sumgyr(X) + mgyr(X)*mwgyr; sumgyr(Y) := sumgyr(Y) + mgyr(Y)*mwgyr; sumgyr(Z) := sumgyr(Z) + mgyr(Z)*mwgyr; -- sumwgyr := sumwgyr + mwgyr; end loop; facc(X) := sumacc(X) / sumwacc; facc(Y) := sumacc(Y) / sumwacc; facc(Z) := sumacc(Z) / sumwacc; fgyr(X) := sumgyr(X) / sumwgyr; fgyr(Y) := sumgyr(Y) / sumwgyr; fgyr(Z) := sumgyr(Z) / sumwgyr; end FilterSuperIMU; 估算代码： 我用一个简单的互补滤波器来估算音高和滚动。我们从IMU获得加速度和陀螺仪速率数据。 代码在这里： procedure ComplementaryFilter(accelData : Sensor_Accel_Data; gyroData : Sensor_Gyro_Data) is pitchAccel : Float := 0.0; -- pitch computed from the accel rollAccel : Float := 0.0; -- roll computed from the accel begin -- integrate the gyroscope data estimatedPitch := estimatedPitch + gyroData(X)*dt; estimatedRoll := estimatedRoll + gyroData(Y)*dt; -- compensate the drift using the acceleration pitchAccel := Ada.Numerics.Elementary_Functions.Arctan(accelData(Y),accelData(Z))*RadToDeg; estimatedPitch := estimatedPitch * 0.98 + pitchAccel * 0.02; rollAccel := Ada.Numerics.Elementary_Functions.Arctan(accelData(X),accelData(Z))*RadToDeg; estimatedRoll := estimatedRoll * 0.98 + rollAccel * 0.02; null; end ComplementaryFilter; 第7步：退役 使用OpenOCD 您也可以使用st-util，我已经在其他项目中完成了openocd的工作，这就是我使用它的原因。 重要（3小时获取时间评论）：你需要在sudo模式下运行gnat gps，否则openocd无法访问usb端口。 安装OpenOCD 在ubuntu上使用OpenOCD的最佳方法是构建它。这样你就拥有了最新版本。我使用了gnu-mcu-eclipse/ openocd的最新版OpenOCD 。 要构建它很简单，只需确保安装了通用的C / C ++构建工具： 用于OpenOCD的配置： - 本地主机的端口为3333 - 我使用了来自openocd github 的stm32f 74discovery的配置文件。 
用于OpenOCD的配置 使用带有Analyzer2go的CYUSB3KIT-003对SPI进行分配 Analyzer2Go只是一个用于小预算的神奇工具。结合电路板CYUSB3KIT-003（45 $），您可以在200MHz采样率下使用功能强大的16x通道分析仪！ 对于我的情况，与SPI / UART的交换低于10Mhz，并且读取Nucleo板的MCO（在100MHz下的PLL），这就足够了。 SPI测试 SPI的速度配置为STM32F746的6MHz（波特率DIV 32）。 我可以使用来自BMI088实例的SPI使用以下过程正确获取IMU运动数据： procedure ReadGyroRates (This : in out Six_Axis_Imu; Result : out Sensor_Gyro_Data); 第8步：PLL时钟测试： 在核板中，外部高速时钟（HSE）不是来自25MHz晶体（如Adacore内置STM32F746Disco），而是来自ST-Link芯片@ 8MHz。 经过数小时的调试后，我最终修改了adl_config.ads和s-bbbopa.ads（作为workarround）文件以设置HSE时钟的正确值。我终于为Nucleo-F746创建了一个新的BSP，而不是因为这种差异而使用stm32f746disco。 现在时钟正常运行，测量显示时间： 在调试模式下读取加速值需要35us 。 
在从IMU读取值结束时，我使用流动代码切换引脚PG1： -- ReadAccelRates -- -------------------- procedure ReadAccelRates (This : in out Six_Axis_Imu; Result : out Sensor_Data) is Received : SPI_Data_8b (1 .. 6); -- 2 bytes per axis AccelRates : BMI088.IMU_Rates;-- todo create AccelRate type begin PG1.Set; This.AccelRead_UInt8s(ACC_ACCEL_DATA_ADDR,Received,6); AccelRates.X := As_IMU_Rates_Pointer (Received (1)'Address).all; AccelRates.Y := As_IMU_Rates_Pointer (Received (3)'Address).all; AccelRates.Z := As_IMU_Rates_Pointer (Received (5)'Address).all; Result(X) := Float (AccelRates.X) / 32768.0 * Accel_Range_MSS; Result(Y) := Float (AccelRates.Y) / 32768.0 * Accel_Range_MSS; Result(Z) := Float (AccelRates.Z) / 32768.0 * Accel_Range_MSS; PG1.Clear; end ReadAccelRates; Reading在调试模式下，完整的陀螺仪值需要34us 。 
------------------- -- ReadGyroRates -- ------------------- procedure ReadGyroRates (This : in out Six_Axis_Imu; Result : out Sensor_Data) is Received : SPI_Data_8b (1 .. 6); -- 2 bytes per axis GyroRates : BMI088.IMU_Rates;-- todo create AccelRate type begin PG1.Set; This.AccelRead_UInt8s(ACC_ACCEL_DATA_ADDR,Received,6); GyroRates.X := As_IMU_Rates_Pointer (Received (1)'Address).all; GyroRates.Y := As_IMU_Rates_Pointer (Received (3)'Address).all; GyroRates.Z := As_IMU_Rates_Pointer (Received (5)'Address).all; Result(X) := Float (GyroRates.X) / 32768.0 * Gyro_Range_Rads; Result(Y) := Float (GyroRates.Y) / 32768.0 * Gyro_Range_Rads; Result(Z) := Float (GyroRates.Z) / 32768.0 * Gyro_Range_Rads; PG1.Clear; end ReadGyroRates; Of 对于真实用例，我需要将编译模式设置为生产以进行正确的优化。 步骤9：使用UART绘制数据 为了可视化输出数据，我想使用UART在超级惊人的酷系列绘图仪中可视化它：https：//github.com/CieNTi/serial_port_plotter 
现在的问题是将Float转换为String ..以使用UART将其发送到Serial_Port_Plotter工具。 为此，我在这里使用神奇的单词'Img in Ada 将Float转换为String。然后我逐字节地将字符串发送到UART。 如果要发送一个值： - 发送“$” - 发送浮点值 - 发送“;” 如果你想发送多个值： - 发送“$”开始 - 发送浮点数值1 - 发送空格“” ... - 发送浮点值i - 发送空格“” - 发送“;” 终止 为了能够向绘图仪发送加速度，我使用了以下代码： data_1B(0) := UInt8(36); -- '$' UART_OUT.Transmit(data_1B,status); for ch of FixedFloat(AccelRates(X))'Img loop c := Character(ch); data_1B(0) := Character'Pos(c); UART_OUT.Transmit(data_1B,status); end loop; data_1B(0) := UInt8(59); -- ';' UART_OUT.Transmit(data_1B,status); 结果非常令人满意，在下面的快照中我实时可视化Accelerometer（X）值： 
创建驱动程序后，该数据通过UART端口正确发送： 
为UART制作驱动程序 不幸的是，在STM32F7器件中还没有UART驱动器（仅适用于STM32F4）。 要为UART创建驱动程序，我从STM32 F4 的stm32-usarts.ads/adb开始。我修改了文件以符合STM32 F7 的SVD 。我还要修改STM32-Device.ads/adb以包含STM32F7的UART / USART定义。 它适用于传输9或8位大小的字节。我没有测试它的DMA风格（因为从未做过  ）代码广告： -------------------------------------------------------------------------------- -- -- Copyright (C) 2015-2016, AdaCore -- -- -- -- Redistribution and use in source and binary forms, with or without -- -- modification, are permitted provided that the following conditions are -- -- met: -- -- 1. Redistributions of source code must retain the above copyright -- -- notice, this list of conditions and the following disclaimer. -- -- 2. Redistributions in binary form must reproduce the above copyright -- -- notice, this list of conditions and the following disclaimer in -- -- the documentation and/or other materials provided with the -- -- distribution. -- -- 3. Neither the name of STMicroelectronics nor the names of its -- -- contributors may be used to endorse or promote products derived -- -- from this software without specific prior written permission. -- -- -- -- THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -- -- "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -- -- LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR -- -- A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT -- -- HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, -- -- SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT -- -- LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -- -- DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY -- -- THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -- -- (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -- -- OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -- -- -- -- -- -- This file is based on: -- -- -- -- @file stm32f4xx_hal_usart.h -- -- @author MCD Application Team -- -- @version V1.1.0 -- -- @date 19-June-2014 -- -- @brief Header file of USARTS HAL module. -- -- -- -- COPYRIGHT(c) 2014 STMicroelectronics -- ------------------------------------------------------------------------------ -- This file provides register definitions for the STM32F4 (ARM Cortex M4F) -- USART from ST Microelectronics. -- Note that there are board implementation assumptions represented by the -- private function APB_Clock. with System; with HAL.UART; use HAL.UART; private with STM32_SVD.USART; package STM32.USARTs is type Internal_USART is limited private; type USART (Periph : not null access Internal_USART) is limited new HAL.UART.UART_Port with private; procedure Enable (This : in out USART) with Post => Enabled (This), Inline; procedure Disable (This : in out USART) with Post => not Enabled (This), Inline; function Enabled (This : USART) return Boolean with Inline; procedure Receive (This : USART; Data : out UInt9) with Inline; -- reads Device.DR into Data function Current_Input (This : USART) return UInt9 with Inline; -- returns Device.DR procedure Transmit (This : in out USART; Data : UInt9) with Inline; function Tx_Ready (This : USART) return Boolean with Inline; function Rx_Ready (This : USART) return Boolean with Inline; type Stop_Bits is (Stopbits_1, Stopbits_2) with Size => 2; for Stop_Bits use (Stopbits_1 => 0, Stopbits_2 => 2#10#); procedure Set_Stop_Bits (This : in out USART; To : Stop_Bits); type Word_Lengths is (Word_Length_8, Word_Length_9); procedure Set_Word_Length (This : in out USART; To : Word_Lengths); type Parities is (No_Parity, Even_Parity, Odd_Parity); procedure Set_Parity (This : in out USART; To : Parities); subtype Baud_Rates is UInt32; procedure Set_Baud_Rate (This : in out USART; To : Baud_Rates); type Oversampling_Modes is (Oversampling_By_8, Oversampling_By_16); -- oversampling by 16 is the default procedure Set_Oversampling_Mode (This : in out USART; To : Oversampling_Modes); type UART_Modes is (Rx_Mode, Tx_Mode, Tx_Rx_Mode); procedure Set_Mode (This : in out USART; To : UART_Modes); type Flow_Control is (No_Flow_Control, RTS_Flow_Control, CTS_Flow_Control, RTS_CTS_Flow_Control); procedure Set_Flow_Control (This : in out USART; To : Flow_Control); type USART_Interrupt is (Parity_Error, Transmit_Data_Register_Empty, Transmission_Complete, Received_Data_Not_Empty, Idle_Line_Detection, Line_Break_Detection, Clear_To_Send, Error); procedure Enable_Interrupts (This : in out USART; Source : USART_Interrupt) with Post => Interrupt_Enabled (This, Source), Inline; procedure Disable_Interrupts (This : in out USART; Source : USART_Interrupt) with Post => not Interrupt_Enabled (This, Source), Inline; function Interrupt_Enabled (This : USART; Source : USART_Interrupt) return Boolean with Inline; type USART_Status_Flag is (Parity_Error_Indicated, Framing_Error_Indicated, USART_Noise_Error_Indicated, Overrun_Error_Indicated, Idle_Line_Detection_Indicated, Read_Data_Register_Not_Empty, Transmission_Complete_Indicated, Transmit_Data_Register_Empty, Line_Break_Detection_Indicated, Clear_To_Send_Indicated); function Status (This : USART; Flag : USART_Status_Flag) return Boolean with Inline; procedure Clear_Status (This : in out USART; Flag : USART_Status_Flag) with Inline; procedure Enable_DMA_Transmit_Requests (This : in out USART) with Inline, Post => DMA_Transmit_Requests_Enabled (This); procedure Disable_DMA_Transmit_Requests (This : in out USART) with Inline, Post => not DMA_Transmit_Requests_Enabled (This); function DMA_Transmit_Requests_Enabled (This : USART) return Boolean with Inline; procedure Enable_DMA_Receive_Requests (This : in out USART) with Inline, Post => DMA_Receive_Requests_Enabled (This); procedure Disable_DMA_Receive_Requests (This : in out USART) with Inline, Post => not DMA_Receive_Requests_Enabled (This); function DMA_Receive_Requests_Enabled (This : USART) return Boolean with Inline; procedure Pause_DMA_Transmission (This : in out USART) renames Disable_DMA_Transmit_Requests; procedure Resume_DMA_Transmission (This : in out USART) with Inline, Post => DMA_Transmit_Requests_Enabled (This) and Enabled (This); procedure Pause_DMA_Reception (This : in out USART) renames Disable_DMA_Receive_Requests; procedure Resume_DMA_Reception (This : in out USART) with Inline, Post => DMA_Receive_Requests_Enabled (This) and Enabled (This); function Data_Receive_Register_Address (This : USART) return System.Address with Inline; function Data_Transmit_Register_Address (This : USART) return System.Address with Inline; -- Returns the address of the USART Data Register. This is exported -- STRICTLY for the sake of clients driving a USART via DMA. All other -- clients of this package should use the procedural interfaces Transmit -- and Receive instead of directly accessing the Data Register! -- Seriously, don't use this function otherwise. ----------------------------- -- HAL.UART implementation -- ----------------------------- overriding function Data_Size (This : USART) return HAL.UART.UART_Data_Size; overriding procedure Transmit (This : in out USART; Data : UART_Data_8b; Status : out UART_Status; Timeout : Natural := 1000) with Pre'Class => Data_Size (This) = Data_Size_8b; overriding procedure Transmit (This : in out USART; Data : UART_Data_9b; Status : out UART_Status; Timeout : Natural := 1000) with Pre'Class => Data_Size (This) = Data_Size_9b; overriding (This : in out USART; Data : out UART_Data_8b; Status : out UART_Status; Timeout : Natural := 1000) with Pre'Class => Data_Size (This) = Data_Size_8b; overriding procedure Receive (This : in out USART; Data : out UART_Data_9b; Status : out UART_Status; Timeout : Natural := 1000) with Pre'Class => Data_Size (This) = Data_Size_9b; private function APB_Clock (This : USART) return UInt32 with Inline; -- Returns either APB1 or APB2 clock rate, in Hertz, depending on the -- USART. For the sake of not making this package board-specific, we assume -- that we are given a valid USART object at a valid address, AND that the -- USART devices really are configured such that only 1 and 6 are on APB2. -- Therefore, if a board has additional USARTs beyond USART6, eg USART8 on -- the F429I Discovery board, they better conform to that assumption. -- See Note # 2 in each of Tables 139-141 of the RM on pages 970 - 972. type Internal_USART is new STM32_SVD.USART.USART_Peripheral; type USART (Periph : not null access Internal_USART) is limited new HAL.UART.UART_Port with null record; end STM32.USARTs; 代码adb： -------------------------------------------------------------------------------- -- -- Copyright (C) 2015-2017, AdaCore -- -- -- -- Redistribution and use in source and binary forms, with or without -- -- modification, are permitted provided that the following conditions are -- -- met: -- -- 1. Redistributions of source code must retain the above copyright -- -- notice, this list of conditions and the following disclaimer. -- -- 2. Redistributions in binary form must reproduce the above copyright -- -- notice, this list of conditions and the following disclaimer in -- -- the documentation and/or other materials provided with the -- -- distribution. -- -- 3. Neither the name of STMicroelectronics nor the names of its -- -- contributors may be used to endorse or promote products derived -- -- from this software without specific prior written permission. -- -- -- -- THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -- -- "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -- -- LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR -- -- A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT -- -- HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, -- -- SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT -- -- LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -- -- DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY -- -- THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -- -- (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -- -- OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -- -- -- -- -- -- This file is based on: -- -- -- -- @file stm32f4xx_hal_usart.c -- -- @author MCD Application Team -- -- @version V1.1.0 -- -- @date 19-June-2014 -- -- @brief USARTS HAL module driver. -- -- -- -- COPYRIGHT(c) 2014 STMicroelectronics -- ------------------------------------------------------------------------------ with System; use System; with STM32_SVD.USART; use STM32_SVD, STM32_SVD.USART; with STM32.Device; use STM32.Device; package body STM32.USARTs is --------------- -- APB_Clock -- --------------- function APB_Clock (This : USART) return UInt32 is Clocks : constant RCC_System_Clocks := System_Clock_Frequencies; begin if This.Periph.all'Address = USART1_Base or This.Periph.all'Address = USART6_Base then return Clocks.PCLK2; else return Clocks.PCLK1; end if; end APB_Clock; ------------ -- Enable -- ------------ procedure Enable (This : in out USART) is begin This.Periph.CR1.UE := True; end Enable; ------------- -- Disable -- ------------- procedure Disable (This : in out USART) is begin This.Periph.CR1.UE := False; end Disable; ------------- -- Enabled -- ------------- function Enabled (This : USART) return Boolean is (This.Periph.CR1.UE); ------------------- -- Set_Stop_Bits -- ------------------- procedure Set_Stop_Bits (This : in out USART; To : Stop_Bits) is begin This.Periph.CR2.STOP := Stop_Bits'Enum_Rep (To); end Set_Stop_Bits; --------------------- -- Set_Word_Length -- --------------------- procedure Set_Word_Length (This : in out USART; To : Word_Lengths) is begin This.Periph.CR1.M0 := To = Word_Length_9; end Set_Word_Length; ---------------- -- Set_Parity -- ---------------- procedure Set_Parity (This : in out USART; To : Parities) is begin case To is when No_Parity => This.Periph.CR1.PCE := False; This.Periph.CR1.PS := False; when Even_Parity => This.Periph.CR1.PCE := True; This.Periph.CR1.PS := False; when Odd_Parity => This.Periph.CR1.PCE := True; This.Periph.CR1.PS := True; end case; end Set_Parity; ------------------- -- Set_Baud_Rate -- ------------------- procedure Set_Baud_Rate (This : in out USART; To : Baud_Rates) is Clock : constant UInt32 := APB_Clock (This); Over_By_8 : constant Boolean := This.Periph.CR1.OVER8; Int_Scale : constant UInt32 := (if Over_By_8 then 2 else 4); Int_Divider : constant UInt32 := (25 * Clock) / (Int_Scale * To); Frac_Divider : constant UInt32 := Int_Divider rem 100; begin -- the integer part of the divi if Over_By_8 then This.Periph.BRR.DIV_Fraction := BRR_DIV_Fraction_Field (((Frac_Divider * 8) + 50) / 100 mod 8); else This.Periph.BRR.DIV_Fraction := BRR_DIV_Fraction_Field (((Frac_Divider * 16) + 50) / 100 mod 16); end if; This.Periph.BRR.DIV_Mantissa := BRR_DIV_Mantissa_Field (Int_Divider / 100); end Set_Baud_Rate; --------------------------- -- Set_Oversampling_Mode -- --------------------------- procedure Set_Oversampling_Mode (This : in out USART; To : Oversampling_Modes) is begin This.Periph.CR1.OVER8 := To = Oversampling_By_8; end Set_Oversampling_Mode; -------------- -- Set_Mode -- -------------- procedure Set_Mode (This : in out USART; To : UART_Modes) is begin This.Periph.CR1.RE := To /= Tx_Mode; This.Periph.CR1.TE := To /= Rx_Mode; end Set_Mode; ---------------------- -- Set_Flow_Control -- ---------------------- procedure Set_Flow_Control (This : in out USART; To : Flow_Control) is begin case To is when No_Flow_Control => This.Periph.CR3.RTSE := False; This.Periph.CR3.CTSE := False; when RTS_Flow_Control => This.Periph.CR3.RTSE := True; This.Periph.CR3.CTSE := False; when CTS_Flow_Control => This.Periph.CR3.RTSE := False; This.Periph.CR3.CTSE := True; when RTS_CTS_Flow_Control => This.Periph.CR3.RTSE := True; This.Periph.CR3.CTSE := True; end case; end Set_Flow_Control; --------- -- Put -- --------- procedure Transmit (This : in out USART; Data : UInt9) is begin This.Periph.TDR.TDR := TDR_TDR_Field (Data); end Transmit; --------- -- Get -- --------- procedure Receive (This : USART; Data : out UInt9) is begin Data := Current_Input (This); end Receive; ------------------- -- Current_Input -- ------------------- function Current_Input (This : USART) return UInt9 is (Uint9 (This.Periph.RDR.RDR)); -------------- -- Tx_Ready -- -------------- function Tx_Ready (This : USART) return Boolean is begin return This.Periph.ISR.TXE; end Tx_Ready; -------------- -- Rx_Ready -- -------------- function Rx_Ready (This : USART) return Boolean is begin return This.Periph.ISR.RXNE; end Rx_Ready; ------------ -- Status -- ------------ function Status (This : USART; Flag : USART_Status_Flag) return Boolean is begin case Flag is when Parity_Error_Indicated => return This.Periph.ISR.PE; when Framing_Error_Indicated => return This.Periph.ISR.FE; when USART_Noise_Error_Indicated => return This.Periph.ISR.NF; when Overrun_Error_Indicated => return This.Periph.ISR.ORE; when Idle_Line_Detection_Indicated => return This.Periph.ISR.IDLE; when Read_Data_Register_Not_Empty => return This.Periph.ISR.RXNE; when Transmission_Complete_Indicated => return This.Periph.ISR.TC; when Transmit_Data_Register_Empty => return This.Periph.ISR.TXE; when Line_Break_Detection_Indicated => return This.Periph.ISR.LBDF; when Clear_To_Send_Indicated => return This.Periph.ISR.CTS; end case; end Status; ------------------ -- Clear_Status -- ------------------ procedure Clear_Status (This : in out USART; Flag : USART_Status_Flag) is begin case Flag is when Parity_Error_Indicated => This.Periph.ISR.PE := False; when Framing_Error_Indicated => This.Periph.ISR.FE := False; when USART_Noise_Error_Indicated => This.Periph.ISR.NF := False; when Overrun_Error_Indicated => This.Periph.ISR.ORE := False; when Idle_Line_Detection_Indicated => This.Periph.ISR.IDLE := False; when Read_Data_Register_Not_Empty => This.Periph.ISR.RXNE := False; when Transmission_Complete_Indicated => This.Periph.ISR.TC := False; when Transmit_Data_Register_Empty => This.Periph.ISR.TXE := False; when Line_Break_Detection_Indicated => This.Periph.ISR.LBDF := False; when Clear_To_Send_Indicated => This.Periph.ISR.CTS := False; end case; end Clear_Status; ----------------------- -- Enable_Interrupts -- ----------------------- procedure Enable_Interrupts (This : in out USART; Source : USART_Interrupt) is begin case Source is when Parity_Error => This.Periph.CR1.PEIE := True; when Transmit_Data_Register_Empty => This.Periph.CR1.TXEIE := True; when Transmission_Complete => This.Periph.CR1.TCIE := True; when Received_Data_Not_Empty => This.Periph.CR1.RXNEIE := True; when Idle_Line_Detection => This.Periph.CR1.IDLEIE := True; when Line_Break_Detection => This.Periph.CR2.LBDIE := True; when Clear_To_Send => This.Periph.CR3.CTSIE := True; when Error => This.Periph.CR3.EIE := True; end case; end Enable_Interrupts; ------------------------ -- Disable_Interrupts -- ------------------------ procedure Disable_Interrupts (This : in out USART; Source : USART_Interrupt) is begin case Source is when Parity_Error => This.Periph.CR1.PEIE := False; when Transmit_Data_Register_Empty => This.Periph.CR1.TXEIE := False; when Transmission_Complete => This.Periph.CR1.TCIE := False; when Received_Data_Not_Empty => This.Periph.CR1.RXNEIE := False; when Idle_Line_Detection => This.Periph.CR1.IDLEIE := False; when Line_Break_Detection => This.Periph.CR2.LBDIE := False; when Clear_To_Send => This.Periph.CR3.CTSIE := False; when Error => This.Periph.CR3.EIE := False; end case; end Disable_Interrupts; ----------------------- -- Interrupt_Enabled -- ----------------------- function Interrupt_Enabled (This : USART; Source : USART_Interrupt) return Boolean is begin case Source is when Parity_Error => return This.Periph.CR1.PEIE; when Transmit_Data_Register_Empty => return This.Periph.CR1.TXEIE; when Transmission_Complete => return This.Periph.CR1.TCIE; when Received_Data_Not_Empty => return This.Periph.CR1.RXNEIE; when Idle_Line_Detection => return This.Periph.CR1.IDLEIE; when Line_Break_Detection => return This.Periph.CR2.LBDIE; when Clear_To_Send => return This.Periph.CR3.CTSIE; when Error => return This.Periph.CR3.EIE; end case; end Interrupt_Enabled; ---------------------------------- -- Enable_DMA_Transmit_Requests -- ---------------------------------- procedure Enable_DMA_Transmit_Requests (This : in out USART) is begin This.Periph.CR3.DMAT := True; end Enable_DMA_Transmit_Requests; --------------------------------- -- Enable_DMA_Receive_Requests -- --------------------------------- procedure Enable_DMA_Receive_Requests (This : in out USART) is begin This.Periph.CR3.DMAR := True; end Enable_DMA_Receive_Requests; ----------------------------------- -- Disable_DMA_Transmit_Requests -- ----------------------------------- procedure Disable_DMA_Transmit_Requests (This : in out USART) is begin This.Periph.CR3.DMAT := False; end Disable_DMA_Transmit_Requests; ---------------------------------- -- Disable_DMA_Receive_Requests -- ---------------------------------- procedure Disable_DMA_Receive_Requests (This : in out USART) is begin This.Periph.CR3.DMAR := False; end Disable_DMA_Receive_Requests; ----------------------------------- -- DMA_Transmit_Requests_Enabled -- ----------------------------------- function DMA_Transmit_Requests_Enabled (This : USART) return Boolean is (This.Periph.CR3.DMAT); ---------------------------------- -- DMA_Receive_Requests_Enabled -- ---------------------------------- function DMA_Receive_Requests_Enabled (This : USART) return Boolean is (This.Periph.CR3.DMAR); ----------------------------- -- Resume_DMA_Transmission -- ----------------------------- procedure Resume_DMA_Transmission (This : in out USART) is begin Enable_DMA_Transmit_Requests (This); if not Enabled (This) then Enable (This); end if; end Resume_DMA_Transmission; -------------------------- -- Resume_DMA_Reception -- -------------------------- procedure Resume_DMA_Reception (This : in out USART) is begin Enable_DMA_Receive_Requests (This); if not Enabled (This) then Enable (This); end if; end Resume_DMA_Reception; --------------------------- -- Data_Register_Address -- --------------------------- function Data_Receive_Register_Address (This : USART) return System.Address is (This.Periph.RDR'Address); --------------------------- -- Data_Register_Address -- --------------------------- function Data_Transmit_Register_Address (This : USART) return System.Address is (This.Periph.TDR'Address); --------------- -- Data_Size -- --------------- overriding function Data_Size (This : USART) return HAL.UART.UART_Data_Size is begin if This.Periph.CR1.M0 then return Data_Size_9b; else return Data_Size_8b; end if; end Data_Size; -------------- -- Transmit -- -------------- overriding procedure Transmit (This : in out USART; Data : UART_Data_8b; Status : out UART_Status; Timeout : Natural := 1000) is pragma Unreferenced (Status, Timeout); begin for Elt of Data loop loop exit when This.Tx_Ready; end loop; This.Transmit (UInt9 (Elt)); end loop; Status := Ok; end Transmit; -------------- -- Transmit -- -------------- overriding procedure Transmit (This : in out USART; Data : UART_Data_9b; Status : out UART_Status; Timeout : Natural := 1000) is pragma Unreferenced (Status, Timeout); begin for Elt of Data loop loop exit when This.Tx_Ready; end loop; This.Transmit (Elt); end loop; Status := Ok; end Transmit; ------------- -- Receive -- ------------- overriding procedure Receive (This : in out USART; Data : out UART_Data_8b; Status : out UART_Status; Timeout : Natural := 1000) is pragma Unreferenced (Status, Timeout); begin for Elt of Data loop loop exit when This.Rx_Ready; end loop; This.Receive (UInt9 (Elt)); end loop; Status := Ok; end Receive; ------------- -- Receive -- ------------- overriding procedure Receive (This : in out USART; Data : out UART_Data_9b; Status : out UART_Status; Timeout : Natural := 1000) is pragma Unreferenced (Status, Timeout); begin for Elt of Data loop loop exit when This.Rx_Ready; end loop; This.Receive (Elt); end loop; Status := Ok; end Receive; end STM32.USARTs; 第10步：结果 过滤后的SuperIMU在振动时更稳定。来自振动的噪声由多量程配置值和冗余度补偿。 结果非常酷，在图片中，我在垂直握手时抓住了滚动，桌子上有一些小的震动（更多的测试会有严重的振动）： 
红色：单个IMU，黄色：过滤SuperIMU（滚动） 测量的加速度和陀螺仪速率的加权平均值有助于减少振动环境中的噪声（例如，握手）。然而，成本与获得性能的使用可能不值得结果 
红色：单个IMU，黄色：过滤的SuperIMU 最后，使用以下命令使用生产编译模式： sudo gprbuild --target=arm-eabi -d superimu.gpr -XLCH=led -XRTS_Profile=ravenscar-full -XLOADER=ROM -XADL_BUILD_CHECKS=Disabled -XADL_BUILD=Production superimu.adb我能够在1ms内达成执行： 
结论 经滤波的SuperIMU允许相对于振动和高加速度计算显着的俯仰和行的稳定值。 使用STM32F746 @200MHz通过过滤13x IMU并在1毫秒内估计一段时间内的音调，取得了良好的效果。 似乎对于火箭的需求，配置12G范围的单个BMI088足以满足火箭需求，而不是为超精确IMU支付300美元，使用两个BMI088突破可能会很有趣，每个25美元。 另一方面，在这个项目中，我学会了如何为STM32制作驱动程序（更新SPI驱动程序，制作UART驱动程序）以及制作BMI088芯片驱动程序。在嵌入式领域中，进行预检查，检查后具有强类型定义的可能性是一个真正的好处。与以前使用的C / C ++相比，在Ada中我不能错过UInt4和UInt8 。这是一次有趣的体验，在成功完成项目的这一重大步骤后，我感兴趣的是为我的个人项目挖掘更多并为其他板和传感器制作其他Ada驱动程序。并且还使用Adacore框架继续开发火箭的自动驾驶仪。 |
微信公众号
手机版
