Arduino.ru

Есть датчик гироскопа/акселерометра mpu6050, NodeMCU и умные светодиоды ws2812. Для полноты картины привожу скетч, все просто и понятно. Вызывают вопросы оффсеты, их я забиваю вручную, устанавливаю при инициализации.

Внимание вопрос, как вместо забивания вручную, при подключении питания считать положение нулевым, забивать это в офсеты, и в дальнейшем выводить градусы относительно положения при включении?

int PIN = D2;               // пин ленты Din

#define BRIGHTNESS 150        // яркость (0 - 255)

int numPixel = 10;

int offsets[6] = { -1094, -1274, 1177, 32, 17, -89};

#include <Adafruit_NeoPixel.h>
Adafruit_NeoPixel strip = Adafruit_NeoPixel(numPixel, PIN, NEO_GRB + NEO_KHZ800);
#include "I2Cdev.h"
#include "MPU6050.h"
MPU6050 mpu;

float mpuPitch;
float mpuRoll;
int16_t ax, ay, az;
int16_t gx, gy, gz;

#define TO_DEG 57.29577951308232087679815481410517033f
#define FK 0.15 // коэффициент фильтра

float angle_ax, angle_gx, angle_cpl;
int dt = 0;
long int t_next, p_next;

void calc ()
 {  
  //Serial.println("calc open");
  //strip.clear(); 
  
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);                     // получить ускорения
 
  //Serial.println("value okay");
  
  float mpuPitcha = ((float)ax / 16384);  // 16384 это величина g с акселерометр
  float mpuRolla = ((float)ay / 16384);   // зависит от чувствительности, default 2G
 
  mpuPitcha = constrain(mpuPitcha, -1.0, 1.0);   //величина проекции силы тяжести на ось
  mpuRolla = constrain(mpuRolla, -1.0, 1.0);     //величина проекции силы тяжести на ось

  mpuPitch = mpuPitch * (1 - FK) + mpuPitcha*FK;    //отфильтрованные значения в радианах
  mpuRoll = mpuRoll * (1 - FK) + mpuRolla*FK;       //отфильтрованные значения в радианах
  
  
  float mpuP = 90 - (acos(mpuPitch))*TO_DEG;  //преобразование значений из радиан в градусы
  float mpuR = 90 - (acos(mpuRoll))*TO_DEG;   //преобразование значений из радиан в градусы
  calcColor(mpuP, mpuR);                      //передаем значения градусов для расчета цвета


  Serial.print ("   ");
  Serial.print (mpuP);                  //выводим значение в градусах
  Serial.print ("   ");
  Serial.print (mpuR);                  //выводим значение в градусах
  Serial.print ("   ");
 }

void calcColor(float Pitch, float Roll)  //функция расчета цвета в зависимости от поворота
 { 
  //Serial.println("calcColor open");
 
  Pitch = constrain (Pitch, -90, 90);   // ограничим входные значения
  Pitch = map(Pitch, -90, 90, 0, 250);  // раскинем угол поворота на  шкалу цвета
  
  Roll = constrain (Roll, -90, 90);
  Roll = map(Roll, -90, 90, 0, 250);
 
  //Serial.println("calcColor done");

  color(Pitch, Roll);                     //передаем значения цвета в функцию рисования 
  
 }

void color (int red, int green)    // функция рисования (установки цвета)
 {
  //Serial.println("color open");
  for (int i = 0; i < numPixel; i++)     //на каждый светодиод установить значения цвета 
   {                                     // переданные с вызовом функции
    strip.setPixelColor(i, strip.Color(red,green,0));
    strip.show();
   }
  Serial.print(red);              
  Serial.print("    ");
  Serial.println(green);
  //Serial.println("color done");
   
 }

void setup() {
  Serial.begin(9600);             // начнем общение
  mpuSetup();                     // идем в функцию инициализации датчика, для удобства
  strip.begin();                  // старт светодиодной ленты
  //strip.setBrightness(BRIGHTNESS);  // установим яркость
  Serial.println("setup okay");       // все окей
  for(int i=0;i<10;i++){              // в setup программы установим цвет

    strip.setPixelColor(i, strip.Color(50,50,30)); 
    strip.show(); 
  };
}

void loop()                 
 {
   calc();                //начинаем подсчет, каждая функция вызывается предыдущей, 
 }                        // функция calc вызывается в loop

void mpuSetup() {         // функция инициализации датчика
  Wire.begin(D7,D6);      // что бы не искать порты, вызовем общение с датчиком тут, в формате (sda, scl)
  mpu.initialize();       
  Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");

  // ставим оффсеты 
  mpu.setXAccelOffset(offsets[0]);
  mpu.setYAccelOffset(offsets[1]);
  mpu.setZAccelOffset(offsets[2]);
  mpu.setXGyroOffset(offsets[3]);
  mpu.setYGyroOffset(offsets[4]);
  mpu.setZGyroOffset(offsets[5]);
 
}