Карманная БИНС

На базе Ардуино Arduino pro mini 5 v 16 mhz, сделал БИНС.

24-25.10.14
Собал первую рабочую схему.
GY-52 - акселерометр + гироскоп,  YL-30 - регистратор на SD-карту.
Подключение GY-52: протокол I2C, INT - 2, VCC - 5 v, SCL - A6,SDA - A5,GND - GND.
Общение: подключаю файл I2Cdev.cpp .h или просто Wire.h. Идет работа с регистрами по id.

Подключение YL-30 - GND-GND,MISO-12,SCK-13,MOSI-11,CS-4,3.3v,GND,5v.
Подключаются три библиотеки SD.h и еще несколько. Протокол SPI

27.10
На днях проверил алгоритм дмп6. Уплыла далеко-далеко траектория... Разбираюсь с квартернионом.


09.11
Заработал алгоритм счисления пространственного положения: курс, тангаж, крен. Оказывается нужно использовать показания акселерометров для коррекции. Алгоритм называется MadgwickAHRS. Хороший - устойчивые показания

Питание - 3,6 в на адаптер - и запитываю АГ и регистратор. Регистратор требует питание 3,3 в и 5 в. Но похоже нормально работает и от 5 в только. АГ работает только от 5 в.

Регистрация 100 Гц. При 1000 Гц захлебывается - не успевает писать.


Итак, прога, которая пишет сырец на SD-карту:


#include "MPU6050.h"
#include "Wire.h"
#include "SD.h"
#include <math.h>

#define LED_PIN 13


MPU6050 accelgyro;
int16_t ax, ay, az;
int16_t gx, gy, gz;
float time,ax1, ay1, az1, gx1, gy1, gz1;
int16_t tmp;
File dataFile;
long t = 0;


bool blinkState = false;

void setup() {
        Wire.begin();
        Serial.begin(115200);
        accelgyro.initialize();
accelgyro.setRate(0);//1000 пїЅпїЅ
accelgyro.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);//+-2g
accelgyro.setFullScaleGyroRange(MPU6050_GYRO_FS_250);//250 gr/s
accelgyro.setXAccelOffset(265);
accelgyro.setYAccelOffset(-1835);
accelgyro.setZAccelOffset(976);
accelgyro.setXGyroOffset(132);
accelgyro.setYGyroOffset(241);
accelgyro.setZGyroOffset(1669);
pinMode(LED_PIN, OUTPUT);
if (!SD.begin(4)) {
Serial.println("Card failed, or not present");
// don't do anything more:
return;
}
Serial.println("card initialized.");
}

void loop() {
    // read raw accel/gyro measurements from device
       t=millis();
       time=t/1000.0;
       accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
       tmp=accelgyro.getTemperature();
       ax1=ax/32768.0*2.0;
       ay1=ay/32768.0*2.0;
       az1=az/32768.0*2.0;
       gx1=(gx-48)/32768.0*250;//-9
       gy1=(gy+86)/32768.0*250;//+6
       gz1=(gz-21)/32768.0*250;//-2
        // display tab-separated accel/gyro x/y/z values
        Serial.print(time,3); Serial.print("\t");
        Serial.print(ax1,6); Serial.print("\t");
        Serial.print(ay1,6); Serial.print("\t");
        Serial.print(az1,6); Serial.print("\t");
        Serial.print(gx1,6); Serial.print("\t");
        Serial.print(gy1,6); Serial.print("\t");
        Serial.print(gz1,6);Serial.print("\t");
        Serial.println(((tmp+ 12412.0) / 340.0),6);
dataFile = SD.open("datalog.txt", FILE_WRITE);
// if the file is available, write to it:
if (dataFile) {
        dataFile.print(time,3); dataFile.print("\t");
        dataFile.print(ax1,6); dataFile.print("\t");
        dataFile.print(ay1,6); dataFile.print("\t");
        dataFile.print(az1,6); dataFile.print("\t");
        dataFile.print(gx1,6); dataFile.print("\t");
        dataFile.print(gy1,6); dataFile.print("\t");
        dataFile.print(gz1,6);dataFile.print("\t");
        dataFile.println(((tmp+ 12412.0) / 340.0),6);
dataFile.close();
    // blink LED to indicate activity
    blinkState = !blinkState;
    digitalWrite(LED_PIN, blinkState);
} else {digitalWrite(LED_PIN, LOW);}
}


Подключаются еще 3 библиотеки: SD, MPU6050, I2Cdev. Архив с кодом: https://drive.google.com/file/d/0B1AR0G3hSwPZUlN4RXRibnN4SXc/view?usp=sharing