• 四轴飞行器MiniFly学习02——姿态融合算法01——获取6轴传感器数据和气压数据

    姿态融合算法——获取6轴传感器数据和气压数据

    概述

    姿态控制任务为一个单独的线程，线程中按照不通的频率执行不通的函数

    1. 获取6轴传感器数据和气压数据（500Hz）
    2. 四元数和欧拉角的计算（250Hz）
    3. 位置预估（250Hz）
    4. 目标姿态和飞行模式设定（100Hz）
    5. 快速调整高度（250Hz）
    6. 读取光流数据（100Hz）
    7. 翻滚检测（500Hz）
    8. 异常检测（实时）
    9. PID控制（实时）
    10. 控制电机输出（500Hz）
    while(1) 
{
	vTaskDelayUntil(&lastWakeTime, MAIN_LOOP_DT);		/*1ms周期延时*/

	//获取6轴和气压数据（500Hz）
	if (RATE_DO_EXECUTE(RATE_500_HZ, tick))
	{
		sensorsAcquire(&sensorData, tick);				/*获取6轴和气压数据*/
	}

	//四元数和欧拉角计算（250Hz）
	if (RATE_DO_EXECUTE(ATTITUDE_ESTIMAT_RATE, tick))
	{
		imuUpdate(sensorData.acc, sensorData.gyro, &state, ATTITUDE_ESTIMAT_DT);
	}

	//位置预估计算（250Hz）
	if (RATE_DO_EXECUTE(POSITION_ESTIMAT_RATE, tick))
	{
		positionEstimate(&sensorData, &state, POSITION_ESTIMAT_DT);
	}
		
	//目标姿态和飞行模式设定（100Hz）	
	if (RATE_DO_EXECUTE(RATE_100_HZ, tick) && getIsCalibrated()==true)
	{
		commanderGetSetpoint(&setpoint, &state);	/*目标数据和飞行模式设定*/	
	}
	
	if (RATE_DO_EXECUTE(RATE_250_HZ, tick))
	{
		fastAdjustPosZ();	/*快速调整高度*/
	}		
	
	/*读取光流数据(100Hz)*/
	if (RATE_DO_EXECUTE(RATE_100_HZ, tick))
	{
		getOpFlowData(&state, 0.01f);	
	}
	
	/*翻滚检测(500Hz) 非定点模式*/
	if (RATE_DO_EXECUTE(RATE_500_HZ, tick) && (getCommanderCtrlMode() != 0x03))
	{
		flyerFlipCheck(&setpoint, &control, &state);	
	}
	
	/*异常检测*/
	anomalDetec(&sensorData, &state, &control);			
	
	/*PID控制*/	
	
	stateControl(&control, &sensorData, &state, &setpoint, tick);
			
	
	//控制电机输出（500Hz）
	if (RATE_DO_EXECUTE(RATE_500_HZ, tick))
	{
		powerControl(&control);	
	}
	
	tick++;
}

    • 1
    • 2
    • 3
    • 4
    • 5
    • 6
    • 7
    • 8
    • 9
    • 10
    • 11
    • 12
    • 13
    • 14
    • 15
    • 16
    • 17
    • 18
    • 19
    • 20
    • 21
    • 22
    • 23
    • 24
    • 25
    • 26
    • 27
    • 28
    • 29
    • 30
    • 31
    • 32
    • 33
    • 34
    • 35
    • 36
    • 37
    • 38
    • 39
    • 40
    • 41
    • 42
    • 43
    • 44
    • 45
    • 46
    • 47
    • 48
    • 49
    • 50
    • 51
    • 52
    • 53
    • 54
    • 55
    • 56
    • 57
    • 58
    • 59
    • 60
    • 61

    获取6轴传感器数据

    执行流程

    当传感器数据可读时，MPU6050中断引脚发出上升沿信号，外部中断检测到中断信号，释放信号量，传感器数据读取线程读取数据，将读取的数据放入消息队列中，姿态控制线程每2ms从消息队列中读取数据

    程序

    • 获取数据函数

      //获取6轴和气压数据（500Hz）
if (RATE_DO_EXECUTE(RATE_500_HZ, tick))
{
	sensorsAcquire(&sensorData, tick);				/*获取6轴和气压数据*/
}

      • 1
      • 2
      • 3
      • 4
      • 5

    • 将传感器获取的数据放入sensors结构体中对应的结构体变量中去

      /*获取传感器数据*/
void sensorsAcquire(sensorData_t *sensors, const u32 tick)	
{	
	sensorsReadGyro(&sensors->gyro);	//陀螺仪数据
	sensorsReadAcc(&sensors->acc);		//加速度计数据
	sensorsReadMag(&sensors->mag);		//磁力计数据
	sensorsReadBaro(&sensors->baro);	//气压计数据
}

      • 1
      • 2
      • 3
      • 4
      • 5
      • 6
      • 7
      • 8

    • 当消息队列中有数据时，从对应的消息队列获取数据

      /*从队列读取陀螺数据*/
bool sensorsReadGyro(Axis3f *gyro)
{
	return (pdTRUE == xQueueReceive(gyroDataQueue, gyro, 0));
}
/*从队列读取加速计数据*/
bool sensorsReadAcc(Axis3f *acc)
{
	return (pdTRUE == xQueueReceive(accelerometerDataQueue, acc, 0));
}
/*从队列读取磁力计数据*/
bool sensorsReadMag(Axis3f *mag)
{
	return (pdTRUE == xQueueReceive(magnetometerDataQueue, mag, 0));
}
/*从队列读取气压数据*/
bool sensorsReadBaro(baro_t *baro)
{
	return (pdTRUE == xQueueReceive(barometerDataQueue, baro, 0));
}

      • 1
      • 2
      • 3
      • 4
      • 5
      • 6
      • 7
      • 8
      • 9
      • 10
      • 11
      • 12
      • 13
      • 14
      • 15
      • 16
      • 17
      • 18
      • 19
      • 20

    • 当传感器数据准备完成，将从传感器读取的数据放入消息队列中去

      while (1)
{
	if (pdTRUE == xSemaphoreTake(sensorsDataReady, portMAX_DELAY))
	{
		/*确定数据长度*/
		u8 dataLen = (u8) (SENSORS_MPU6500_BUFF_LEN +
			(isMagPresent ? SENSORS_MAG_BUFF_LEN : 0) +
			(isBaroPresent ? SENSORS_BARO_BUFF_LEN : 0));

		i2cdevRead(I2C1_DEV, MPU6500_ADDRESS_AD0_HIGH, MPU6500_RA_ACCEL_XOUT_H, dataLen, buffer);
		
		/*处理原始数据，并放入数据队列中*/
		processAccGyroMeasurements(&(buffer[0]));

		if (isMagPresent)
		{
			processMagnetometerMeasurements(&(buffer[SENSORS_MPU6500_BUFF_LEN]));
		}
		if (isBaroPresent)
		{
			processBarometerMeasurements(&(buffer[isMagPresent ?
				SENSORS_MPU6500_BUFF_LEN + SENSORS_MAG_BUFF_LEN : SENSORS_MPU6500_BUFF_LEN]));
		}
		
		vTaskSuspendAll();	/*确保同一时刻把数据放入队列中*/
		xQueueOverwrite(accelerometerDataQueue, &sensors.acc);
		xQueueOverwrite(gyroDataQueue, &sensors.gyro);
		if (isMagPresent)
		{
			xQueueOverwrite(magnetometerDataQueue, &sensors.mag);
		}
		if (isBaroPresent)
		{
			xQueueOverwrite(barometerDataQueue, &sensors.baro);
		}
		xTaskResumeAll();
	}
}	

      • 1
      • 2
      • 3
      • 4
      • 5
      • 6
      • 7
      • 8
      • 9
      • 10
      • 11
      • 12
      • 13
      • 14
      • 15
      • 16
      • 17
      • 18
      • 19
      • 20
      • 21
      • 22
      • 23
      • 24
      • 25
      • 26
      • 27
      • 28
      • 29
      • 30
      • 31
      • 32
      • 33
      • 34
      • 35
      • 36
      • 37
      • 38

    1. 当二值信号量sensorsDataReady被释放后，则表明传感器数据准备完成
      二值信号量sensorsDataReady在外部中断EXTI4_Callback中被释放，MPU6050的外部中断线接在PA4上（上升沿触发）

      /*传感器中断初始化*/
static void sensorsInterruptInit(void)
{
	GPIO_InitTypeDef GPIO_InitStructure;
	EXTI_InitTypeDef EXTI_InitStructure;

	/*使能MPU6500中断*/
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;
	GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_DOWN;
	GPIO_Init(GPIOA, &GPIO_InitStructure);

	SYSCFG_EXTILineConfig(EXTI_PortSourceGPIOA, EXTI_PinSource4);

	EXTI_InitStructure.EXTI_Line = EXTI_Line4;
	EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
	EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Rising;
	EXTI_InitStructure.EXTI_LineCmd = ENABLE;
	portDISABLE_INTERRUPTS();
	EXTI_Init(&EXTI_InitStructure);
	EXTI_ClearITPendingBit(EXTI_Line4);
	portENABLE_INTERRUPTS();
}

      • 1
      • 2
      • 3
      • 4
      • 5
      • 6
      • 7
      • 8
      • 9
      • 10
      • 11
      • 12
      • 13
      • 14
      • 15
      • 16
      • 17
      • 18
      • 19
      • 20
      • 21
      • 22
      • 23
      void __attribute__((used)) EXTI4_Callback(void)
{
	portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
	xSemaphoreGiveFromISR(sensorsDataReady, &xHigherPriorityTaskWoken);	//释放信号量

	if (xHigherPriorityTaskWoken)
	{
		portYIELD();								//让出CPU进行任务切换
	}
}

      • 1
      • 2
      • 3
      • 4
      • 5
      • 6
      • 7
      • 8
      • 9
      • 10
      • 函数 xSemaphoreGiveFromISR()
        此函数用于在中断中释放信号量， 此函数只能用来释放二值信号量和计数型信号量，绝对不能用来在中断服务函数中释放互斥信号量！
        • 函数原型
          BaseType_t xSemaphoreGiveFromISR( SemaphoreHandle_t xSemaphore,BaseType_t * pxHigherPriorityTaskWoken)

          • 1
          • xSemaphore： 要释放的信号量句柄。
          • pxHigherPriorityTaskWoken：标记退出此函数以后是否进行任务切换，这个变量的值由这三个函数来设置的，用户不用进行设置，用户只需要提供一个变量来保存这个值就行了。当此值为 pdTRUE 的时候在退出中断服务函数之前一定要进行一次任务切换。

    2. 为什么写数据队列用xQueueOverwrite
      此函数在当队列满了以后会覆写掉旧的数据，不管这个旧数据有没有被其他任务或中断取走。这个函数常用于向那些长度为 1 的队列发送消息，而传感器数据队列创建时，数据队列的大小为1，size为对应的结构体大小。

      accelerometerDataQueue = xQueueCreate(1, sizeof(Axis3f));
gyroDataQueue = xQueueCreate(1, sizeof(Axis3f));
magnetometerDataQueue = xQueueCreate(1, sizeof(Axis3f));
barometerDataQueue = xQueueCreate(1, sizeof(baro_t));

      • 1
      • 2
      • 3
      • 4

      在这里插入图片描述

    参考文档

    1. 《四元数解算姿态完全解析及资料汇总》
    2. 《ATK-MiniFly微型四轴开发指南_V1.3》
  • 相关阅读:
    React报错之useNavigate() may be used only in context of Router
    中贝通信-603220 三季报分析（20231120）
    【missing-semester】The shell
    工作10年，浅谈经历过的高并发架构设计实战经验
    【5G NR】无线承载SRB和DRB
    基于 elementUI / elementUI plus，实现 主要色（主题色）的一件换色（换肤）
    2022杭电多校联赛第十场 题解
    k8s的pod内部打包工程镜像
    将二叉树的子树映射成一个数，用于判断子树相同
    golang-bufio 缓冲扫描
  • 原文地址：https://blog.csdn.net/weixin_43739167/article/details/126192057