九軸感測器各軸數據輸出至excel，及Kalman Filter
各位好，我是程式新手最近寫了一個程式如下，想把九軸sensor各軸的移動數據輸出至試算表中，請問該如何修改?
另外，想請問是否有用過Kalman Filter的範例可分享??
#include "mbed.h"
#include "MPU9255.h"
//use I2C1 inside MPU9255 class
I2C i2c(I2C_SDA, I2C_SCL);
MPU9255 mpu9255(SMPLRT_DIV_250Hz, MGT_100HZ, AFS_4G, GFS_250DPS, MFS_16BITS);
DigitalOut myled(LED1);
Timer t;
Serial pc(USBTX, USBRX); // tx, rx
int main(){
float ax, ay, az, gx, gy, gz, mx, my, mz; // variables to hold latest sensor data values
float temperature;
float yaw, pitch, roll;
float deltat = 0.0f; // integration interval for both filter schemes
int lastUpdate = 0, Now = 0; // used to calculate integration interval
pc.baud(9600);
i2c.frequency(400000);// use fast (400 kHz) I2C
t.start();
// Read WHO_AM_I register for MPU-9255
uint8_t whoami = mpu9255.whoami();
if (whoami == 0x73) // WHO_AM_I should always be 0x73
{
pc.printf("MPU9255 WHO_AM_I is 0x%x\n\r", whoami);
pc.printf("MPU9255 is online...\n\r");
wait(1);
mpu9255.initSensor();
mpu9255.printInfo(&pc);
}
else
{
pc.printf("Could not connect to MPU9255: \n\r");
pc.printf("%#x \n",whoami);
while(1){ myled = 1;} // Loop forever if communication doesn't happen
}
while(1){
if(mpu9255.isDataReady()) { // On interrupt, check if data ready interrupt
mpu9255.readAccelData(&ax, &ay, &az); // Read the accel x/y/z adc values
mpu9255.readGyroData(&gx, &gy, &gz); // Read the gyro x/y/z adc values
mpu9255.readMagData(&mx, &my, &mz); // Read the mag x/y/z adc values
}
Now = t.read_us();
deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
lastUpdate = Now;
// Pass gyro rate as rad/s
// use delat as integration rate
mpu9255.filterUpdate(gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, ax, ay, az, deltat);
pc.printf(" ax = %f", 1000.0f*ax);
pc.printf(" ay = %f", 1000.0f*ay);
pc.printf(" az = %fmg\n\r", 1000*az);
pc.printf(" gx = %f", gx);
pc.printf(" gy = %f", gy);
pc.printf(" gz = %fdeg/s\n\r", gz);
pc.printf(" mx = %f", mx);
pc.printf(" my = %f", my);
pc.printf(" mz = %fmG\n\r", mz);
mpu9255.getEulerDegreeFilter(&yaw, &pitch, &roll);
//pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
pc.printf("Orientation: %f %f %f\n\r", yaw, pitch, roll);
myled = !myled;
}
}
請教一下，您SPI設定與讀取程式如何寫的?
我設定如下，但是ZSC與ZSC兩者的值永遠都一樣
想參考您的SPI read封包，再更改為ZSC使用 謝謝
void SysTick_Handler(void)//1ms
{
SPI_Delay++;
if(SPI_Delay==3)
{
SPI4_Read(ZSC,2);
SPI_Delay=0;
}
}
static void SPI4_Init(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
SPI_InitTypeDefSPI_InitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_SPI4, ENABLE);
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOE | RCC_AHB1Periph_GPIOE | RCC_AHB1Periph_GPIOE, ENABLE);
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOE, ENABLE);
GPIO_PinAFConfig(GPIOE, GPIO_PinSource12, GPIO_AF_SPI4);
GPIO_PinAFConfig(GPIOE, GPIO_PinSource13, GPIO_AF_SPI4);
GPIO_PinAFConfig(GPIOE, GPIO_PinSource14, GPIO_AF_SPI4);
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd= GPIO_PuPd_DOWN;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12;
GPIO_Init(GPIOE, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin =GPIO_Pin_13;
GPIO_Init(GPIOE, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_14;
GPIO_Init(GPIOE, &GPIO_InitStructure);
SPI_I2S_DeInit(SPI4);
SPI_InitStructure.SPI_Direction = SPI_Direction_2Lines_FullDuplex;
SPI_InitStructure.SPI_Mode = SPI_Mode_Master;
SPI_InitStructure.SPI_DataSize = SPI_DataSize_8b;
SPI_InitStructure.SPI_CPOL = SPI_CPOL_Low;
SPI_InitStructure.SPI_CPHA = SPI_CPHA_1Edge;
SPI_InitStructure.SPI_NSS = SPI_NSS_Soft;
SPI_InitStructure.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_128; //ZSC31210 50K-800K
SPI_InitStructure.SPI_FirstBit = SPI_FirstBit_MSB;
SPI_InitStructure.SPI_CRCPolynomial = 7;
SPI_Init(SPI4, &SPI_InitStructure);
SPI_Cmd(SPI4, ENABLE);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
GPIO_InitStructure.GPIO_PuPd= GPIO_PuPd_NOPULL;
GPIO_Init(GPIOE, &GPIO_InitStructure);
GPIO_SetBits(GPIOE, GPIO_Pin_11);
}
void SPI4_Read(uint8_t* pBuffer, uint16_t NumByteToRead)
{
GPIO_ResetBits(GPIOE, GPIO_Pin_11);
while(NumByteToRead > 0x00)
{
*pBuffer = SPI4_SendByte(0xFF);
NumByteToRead--;
pBuffer++;
}
GPIO_SetBits(GPIOE, GPIO_Pin_11);
}
static uint8_t SPI4_SendByte(uint16_t byte)
{
SPI_I2S_SendData(SPI4, (uint16_t)byte);
return (uint8_t)SPI_I2S_ReceiveData(SPI4);
}
页:
[1]