移植EKF代码后，仿真硬件错误中断

最新推荐文章于 2024-05-14 19:09:54 发布

The_embedded

最新推荐文章于 2024-05-14 19:09:54 发布

阅读量487

点赞数

分类专栏： STM32

本文链接：https://blog.csdn.net/The_embedded/article/details/116265590

版权

STM32 专栏收录该内容

5 篇文章 0 订阅

订阅专栏

STM32在移植EKF代码后，编译没有问题，仿真的时候出现硬件错误中断：

void HardFault_Handler(void)
{
/* USER CODE BEGIN HardFault_IRQn 0 */

/* USER CODE END HardFault_IRQn 0 */
while (1)
{
/* USER CODE BEGIN W1_HardFault_IRQn 0 */
/* USER CODE END W1_HardFault_IRQn 0 */
}
}

查了网上的论坛，得到以下两个原因：STM32出现HardFault_Handler故障的原因主要有两个方面：

1、内存溢出或者访问越界。这个需要自己写程序的时候规范代码，遇到了需要慢慢排查。

2、堆栈溢出。增加堆栈的大小。

移植的EKF部分代码：

void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
  real32_T z[9], const real32_T x_aposteriori_k[12], const real32_T
  P_aposteriori_k[144], const real32_T q[12], real32_T r[9], real32_T
  eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T
  P_aposteriori[144])
{
	memset(wak,0,sizeof(wak));
	memset(b_A_lin,0,sizeof(b_A_lin));
	memset(b_q,0,sizeof(b_q));
	memset(c_A_lin,0,sizeof(c_A_lin));
	memset(d_A_lin,0,sizeof(d_A_lin));
	memset(e_A_lin,0,sizeof(e_A_lin));
	memset(b_P_apriori,0,sizeof(b_P_apriori));
	memset(K_k,0,sizeof(K_k));
	
  real32_T O[9];
  real_T dv0[9];
  real32_T a[9];
  int32_T i;
  real32_T b_a[9];
  real32_T x_n_b[3];
  real32_T b_x_aposteriori_k[3];
  real32_T z_n_b[3];
  real32_T c_a[3];
  real32_T d_a[3];
  int32_T i0;
  real32_T x_apriori[12];
  real_T dv1[144];
  real32_T A_lin[144];
  static const int8_T iv0[36] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0,
    0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };

//  real32_T b_A_lin[144];
//  real32_T b_q[144];
//  real32_T c_A_lin[144];
//  real32_T d_A_lin[144];
//  real32_T e_A_lin[144];
  int32_T i1;
  real32_T P_apriori[144];
//  real32_T b_P_apriori[108];
  static const int8_T iv1[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0,
    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
    0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
    0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
    0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 };

//  real32_T K_k[108];
  real32_T fv0[81];
  static const int8_T iv2[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0,
    0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0,
    1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0,
    0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 };

  real32_T b_r[81];
  real32_T fv1[81];
  real32_T f0;
  real32_T c_P_apriori[36];
  static const int8_T iv3[36] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0 };

  real32_T fv2[36];
  static const int8_T iv4[36] = { 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0,
    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };

  real32_T c_r[9];
  real32_T b_K_k[36];
  real32_T d_P_apriori[72];
  static const int8_T iv5[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
    1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
    1, 0, 0, 0 };

  real32_T c_K_k[72];
  static const int8_T iv6[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0,
    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
    0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
    0, 0, 0, 0 };

  real32_T b_z[6];
  static const int8_T iv7[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
    0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0,
    0, 0, 0, 1 };

  static const int8_T iv8[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0,
    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0,
    0, 0, 0, 1 };

  real32_T fv3[6];
  real32_T c_z[6];

  /*  Extended Attitude Kalmanfilter */
  /*  */
  /*  state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */
  /*  measurement vector z has the following entries [ax,ay,az||mx,my,mz||wmx,wmy,wmz]' */
  /*  knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */
  /*  */
  /*  [x_aposteriori,P_aposteriori] = AttKalman(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) */
  /*  */
  /*  Example.... */
  /*  */
  /*  $Author: Tobias Naegeli $    $Date: 2012 $    $Revision: 1 $ */
  /* coder.varsize('udpIndVect', [9,1], [1,0]) */
  /* udpIndVect=find(updVect); */
  /* process and measurement noise covariance matrix */
  /* Q = diag(q.^2*dt); */
  /* observation matrix */
  /* 'attitudeKalmanfilter:33' wx=  x_aposteriori_k(1); */
  /* 'attitudeKalmanfilter:34' wy=  x_aposteriori_k(2); */
  /* 'attitudeKalmanfilter:35' wz=  x_aposteriori_k(3); */
  /* 'attitudeKalmanfilter:37' wax=  x_aposteriori_k(4); */
  /* 'attitudeKalmanfilter:38' way=  x_aposteriori_k(5); */
  /* 'attitudeKalmanfilter:39' waz=  x_aposteriori_k(6); */
  /* 'attitudeKalmanfilter:41' zex=  x_aposteriori_k(7); */
  /* 'attitudeKalmanfilter:42' zey=  x_aposteriori_k(8); */
  /* 'attitudeKalmanfilter:43' zez=  x_aposteriori_k(9); */
  /* 'attitudeKalmanfilter:45' mux=  x_aposteriori_k(10); */
  /* 'attitudeKalmanfilter:46' muy=  x_aposteriori_k(11); */
  /* 'attitudeKalmanfilter:47' muz=  x_aposteriori_k(12); */
  /* % prediction section */
  /* body angular accelerations */
  /* 'attitudeKalmanfilter:51' wak =[wax;way;waz]; */
  wak[0] = x_aposteriori_k[3];
  wak[1] = x_aposteriori_k[4];
  wak[2] = x_aposteriori_k[5];
//  wak[0] = 0;
//  wak[1] = 0;
//  wak[2] = 0;
//return;

  /* body angular rates */
  /* 'attitudeKalmanfilter:54' wk =[wx;  wy; wz] + dt*wak; */
  /* derivative of the prediction rotation matrix */
  /* 'attitudeKalmanfilter:57' O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]'; */
  O[0] = 0.0F;
  O[1] = -x_aposteriori_k[2];
  O[2] = x_aposteriori_k[1];
  O[3] = x_aposteriori_k[2];
  O[4] = 0.0F;
  O[5] = -x_aposteriori_k[0];
  O[6] = -x_aposteriori_k[1];
  O[7] = x_aposteriori_k[0];
  O[8] = 0.0F;

通过观察仿真时候的汇编代码，我发现每次都是在执行下面的语句时候出现错误中断：

wak[0] = x_aposteriori_k[3];
wak[1] = x_aposteriori_k[4];

对应的汇编代码：

汇编不是很熟悉，大致映像中的意识是：

0x08004F7E ED950A03 VLDR s0,[r5,#0x0C] //把r5寄存器中的地址加上0XC得到新的地址，将新的地址中的内容赋值给S0
0x08004F82 ED870A00 VSTR s0,[r7,#0x00] //把s0内容数据，放到（r7+0x00）这个地址中，r7寄存器中放的是一个地址
209: wak[1] = x_aposteriori_k[4];
0x08004F86 ED950A04 VLDR s0,[r5,#0x10]
0x08004F8A ED870A01 VSTR s0,[r7,#0x04]

LDR ：表示从存储器装入寄存器里面

STR：表示寄存器数据送入存储器

接下来，我把函数代码提前return，如下面。发现不再进入中断

wak[0] = x_aposteriori_k[3];
wak[1] = x_aposteriori_k[4];
wak[2] = x_aposteriori_k[5];

//return;

这时候推测：堆栈溢出？通过将attitudeKalmanfilter函数中的局部变量提升为全局变量，发现也不会进入错误中断。因此，在STM32中写程序的时候，局部变量太大，可能会导致堆栈溢出

给自己留给作业。为什么会溢出呢？不应该在编译的时候，就会把堆栈空间大小准备好吗？还是只是给了一个地址而已呢？后面接着实验：

把局部变量全部初始化定义，再看会不会进入错误中断，如果进入，那么说明编译的时候给的堆栈空间确实就是一个指针，没有安排空间大小；如果没有出现中断，那就是给了大小

The_embedded

关注

0
点赞
踩
0

收藏

觉得还不错? 一键收藏
1
评论
移植EKF代码后，仿真硬件错误中断

STM32在移植EKF代码后，编译没有问题，仿真的时候出现硬件错误中断：void HardFault_Handler(void){ /* USER CODE BEGIN HardFault_IRQn 0 */ /* USER CODE END HardFault_IRQn 0 */ while (1) { /* USER CODE BEGIN W1_HardFault_IRQn 0 */ /* USER CODE END W1_HardFault_IRQn 0 ...
复制链接

扫一扫