• Witaj na Forum Arduino Polska! Zapraszamy do rejestracji!
  • Znajdziesz tutaj wiele informacji na temat hardware / software.
Witaj! Logowanie Rejestracja


Ocena wątku:
  • 0 głosów - średnia: 0
  • 1
  • 2
  • 3
  • 4
  • 5
Czujnik GY-91
#31
(06-09-2018, 08:40)Freyeth napisał(a): VCC płytki (5V). Płytka ma konfigurację 5V 16MHz, czujnik pracuje zarówno pod napięciem 3,3 jak i 5V (wewnętrzny regulator).
Wczoraj podpinałem piny VIN, GND, SDA, SCL w czujniku. Poza tym, BMP280 to barometr o ile dobrze kojarzę i jego odczyt nie jest mi niezbędny. Niepodpięcie BMP280 chyba nie powinno mieć wpływu na brak odpowiedzi z pozostałych 3 elementów GY-91: akcelerometru, magnetometru i żyroskopu...

Same układy sa zasilane z 3,3V, sprawdź czy napięcie ma poprawną wartość.
Co ma 16MHz do płytki czujników?
BMP280 może mieć wpływ na działanie pozostałych układów. Jest włączony do wspólnej magistrali I2C i może blokować pozostałe układy na magistrali. Nie pamiętam funkcji pinu SDO ale u siebie na schematach widzę, że jest podłączony z Vcc (3,3V). Sprawdź w nocie jaką funkcje pełni pin SDO. Może to jest wybór magistrali (I2C/SPI)? Nie pamiętam not katalogowych wszystkich układów (to nie lata 80, gdzie dostępne było ich niewiele - wtedy pamiętałem 80% układów krajowych i nie tylko), więc sprawdź to.

Jeśli to zły trop, to podłącz oscyloskop lub analizator, bez tego dalej szybko nie ruszysz.


(06-09-2018, 08:40)Freyeth napisał(a): korzystając z instrukcji zamieszczonych na stronie http://www.edtracker.org.uk/ Tak jak wspomniałem, TAM nie wygląda to na skomplikowane
Można uruchomić super, hyper skomplikowany układ, jak ruszy "od kopa", to ok, jak nie to trzeba mieć wiedze i sprzęt aby znaleźć błąd. Błąd może byc banany, coś niepodłączone, zwarcie, słaby styk, brak jednej literki czy cyferki w kodzie źródłowym.


Wielokrotnie uruchamiałem układy osobom z forum. Błędy zawsze były banalne ale sprzęt i wiedza potrzebna do ich znalezienia już nie. Czasem ciężko uwierzyć, że osoby z 20 letnim stażem w automatyce, nie potrafią poprawnie obliczyć bocznika dla INA219 czy przylutować kondensatora.
- uC: ARM Angel , AVR, Z8, PIC, 8051 / CPU: MC680x0  , Z-80, 6502
- CPLD, FPGA, GAL
- GSM, ISDN, ETH, USB, RS232C/485/422
- C, ASM, CUPL, PHP, BASIC C-64

http://er-mik.prv.pl/projekty_avt.php * http://er-mik.prv.pl/ * http://kolejki.prv.pl/

KA-NUCLEO-F411CE Idea , ESP32, Mega2560, UNO PLUS
 
Odpowiedź
#32
Opis pinów GY-91:
  1. VIN: Voltage Supply Pin
  2. 3V3: 3.3v Regulator output
  3. GND: 0V Power Supply
  4. SCL: I2C Clock / SPI Clock
  5. SDA: I2C Data or SPI Data Input
  6. SDO/SAO: SPI Data output / I2C Slave Address configuration pin
  7. NCS: Chip Select for SPI mode only for MPU-9250
  8. CSB: Chip Select for BMP280
    Oglądając filmy na Youtube dotyczące tego modelu czujnika (https://www.youtube.com/watch?v=N1Mva_A5D7s) widzę, że przylutowane mają tylko piny VIN, GND, SCL, SDA... i działa.
 
Odpowiedź
#33
(06-09-2018, 10:02)Freyeth napisał(a):
  1. Oglądając filmy na Youtube dotyczące tego modelu czujnika (https://www.youtube.com/watch?v=N1Mva_A5D7s) widzę, że przylutowane mają tylko piny VIN, GND, SCL, SDA... i działa.
Ja wole oglądać noty katalogowe a nie filmiki na YT. Moja opinia na temat filmów nie jest odosobniona, ale skoro całą wiedzę można czerpać z YT, to dlaczego pytania zadajesz na forum a nie na YT?
Traktujesz YT jako wyrocznię, wierzysz w to, więc tam szukaj pomocy.

Ja wierzę w noty katalogowe, multimetr, oscyloskop i analizator logiczny. Jak można się przekonać, ta wiara jest lepsza od YT, bo ZAWSZE, udaje mi się uruchomić układ.

Gdybyś miał to wszystko zmontowane na PCB, to napisałbym: przyślij do mnie a znajdę błąd. Niestety masz to w rozsypce, więc w transporcie wszystko może się zdarzyć. Te chińskie kabelki, których używasz, często mają złe styki. Już nie raz się o tym przekonałem ostatnio przy okazji BME280. Ale ja nie wierzyłem w YT, tylko w oscyloskop. Wiara opłaciła się, bo układ uruchomiłem i nie trwało to miesiące, tygodnie, czy dni a ok 2 godzin.
- uC: ARM Angel , AVR, Z8, PIC, 8051 / CPU: MC680x0  , Z-80, 6502
- CPLD, FPGA, GAL
- GSM, ISDN, ETH, USB, RS232C/485/422
- C, ASM, CUPL, PHP, BASIC C-64

http://er-mik.prv.pl/projekty_avt.php * http://er-mik.prv.pl/ * http://kolejki.prv.pl/

KA-NUCLEO-F411CE Idea , ESP32, Mega2560, UNO PLUS
 
Odpowiedź
#34
(06-09-2018, 10:53)es2 napisał(a):
(06-09-2018, 10:02)Freyeth napisał(a):
  1. Oglądając filmy na Youtube dotyczące tego modelu czujnika (https://www.youtube.com/watch?v=N1Mva_A5D7s) widzę, że przylutowane mają tylko piny VIN, GND, SCL, SDA... i działa.
Ja wole oglądać noty katalogowe a nie filmiki na YT. Moja opinia na temat filmów nie jest odosobniona, ale skoro całą wiedzę można czerpać z YT, to dlaczego pytania zadajesz na forum a nie na YT?
Traktujesz YT jako wyrocznię, wierzysz w to, więc tam szukaj pomocy.

Ja wierzę w noty katalogowe, multimetr, oscyloskop i analizator logiczny. Jak można się przekonać, ta wiara jest lepsza od YT, bo ZAWSZE, udaje mi się uruchomić układ.

Gdybyś miał to wszystko zmontowane na PCB, to napisałbym: przyślij do mnie a znajdę błąd. Niestety masz to w rozsypce, więc w transporcie wszystko może się zdarzyć. Te chińskie kabelki, których używasz, często mają złe styki. Już nie raz się o tym przekonałem ostatnio przy okazji BME280. Ale ja nie wierzyłem w YT, tylko w oscyloskop. Wiara opłaciła się, bo układ uruchomiłem i nie trwało to miesiące, tygodnie, czy dni a ok 2 godzin.

Film na YT podałem tylko jako wizualizację, natomiast na różnych forach tematycznych widniały informacje o sposobie podpinania pinów, dokładnie tak jak pisałem.
Mam płytkę PCB, mogę polutować cały układ, może to rzeczywiście kwestia kabelków...
Po reklamacjach w sumie mogę to już zrobić ze spokojnym sumieniem, bo jak spierniczę, to i tak reklamacja się wypnie na mnie.
 
Odpowiedź
#35
(06-09-2018, 11:04)Freyeth napisał(a): Mam płytkę PCB, mogę polutować cały układ, może to rzeczywiście kwestia kabelków...
Po reklamacjach w sumie mogę to już zrobić ze spokojnym sumieniem, bo jak spierniczę, to i tak reklamacja się wypnie na mnie.

Samych modułów nie musisz lutować. W modułach zostaw goldpiny a w PCB wlutuj gniazdo golgpin. Wyprowadź sobie też wyprowadzenia dla analizatora stanów. Bez niego to może i rak będzie za mało aby to uruchomić.
- uC: ARM Angel , AVR, Z8, PIC, 8051 / CPU: MC680x0  , Z-80, 6502
- CPLD, FPGA, GAL
- GSM, ISDN, ETH, USB, RS232C/485/422
- C, ASM, CUPL, PHP, BASIC C-64

http://er-mik.prv.pl/projekty_avt.php * http://er-mik.prv.pl/ * http://kolejki.prv.pl/

KA-NUCLEO-F411CE Idea , ESP32, Mega2560, UNO PLUS
 
Odpowiedź
#36
(06-09-2018, 12:12)es2 napisał(a):
(06-09-2018, 11:04)Freyeth napisał(a): Mam płytkę PCB, mogę polutować cały układ, może to rzeczywiście kwestia kabelków...
Po reklamacjach w sumie mogę to już zrobić ze spokojnym sumieniem, bo jak spierniczę, to i tak reklamacja się wypnie na mnie.

Samych modułów nie musisz lutować. W modułach zostaw goldpiny a w PCB wlutuj gniazdo golgpin. Wyprowadź sobie też wyprowadzenia dla analizatora stanów. Bez niego to może i rak będzie za mało aby to uruchomić.

Przeprowadziłem jeszcze jedną próbę. Wpiąłem kable w płytkę i czujnik "na żywca" - oderwałem wtyki, zdjąłem kawałek izolacji z kabelka i przymocowałem "na okrętkę" do odpowiednich pinów. Uruchomiłem ArduinoIDE, odpaliłem skrypt I2C Scanner.... JEST! Odczytał dwa adresy czujnika, znaczy że cokolwiek się dzieje i, jak mówiłeś, wadliwy montaż.
Wieczorem usiądę jeszcze raz, teraz było na szybko i nie miałem czasu nawet zapisać numerów adresów, ale jest postęp.
Odczytał czujnik GY-91. Po podłączeniu w ten sam sposób MPU9255 - brak odczytu.
 
Odpowiedź
#37
(06-09-2018, 13:56)Freyeth napisał(a): jak mówiłeś, wadliwy montaż.

Nic takiego nie mówiłem, ale pisałem.
Gdybyś miał sprzęt, to błąd namierzył byś w kilkanaście minut, a nie kilkadziesiąt....godzin.


Taki wpadek, jak słaby styk, miałem chyba tysiące. Oczywiście sprzęt, wiedza ułatwiają szukanie błędu. Jeden z ostatnich był ciekawy, szukałem błędu w sofcie, sprzęt powinien być ok ale nie był, złożyło się, że:
- klient, który dostarczył złożony prototyp nie dolutował masy na złączu
- dolutowując złącze "na drutach" pomyliłem styki robiąc zwarcie na wyjściu sygnału
- badając sygnały oscyloskopem, używałem innej sondy niż powinienem (pomyliłem znaczniki na sondach w oscyloskopie wielokanałowym)
Pomijając ostatni błąd (zwykła nieuwaga):
- Nie wierz w to co dostarcza klient mimo zapewnień, ze jest ok. W praktyce wszystko trzeba sprawdzić co kosztuje więcej niż zrobić od początku samemu. Wniosek: uruchamianie sprzętu robionego przez klienta -cena za uruchomienie razy 2 albo i 4.
- Nie rób nic na stykówkach, unikaj uniwersalek. Nawet do prostych rzeczy warto zrobić PCB. Taka prosta PCB z oferty na Allego to koszt 10..30zł. Ile kosztują spalone elementy?


Na forum nie jestem lubiany, za swoje, jak to niektórzy określają "mądrości". Prawdziwy powód jest inny.....przeważnie mam rację. A jak się pomylę! Oooooo, wtedy to jest woda na młyn dla tych co niewiele wiedzą. Normalnie święto we wsi.
- uC: ARM Angel , AVR, Z8, PIC, 8051 / CPU: MC680x0  , Z-80, 6502
- CPLD, FPGA, GAL
- GSM, ISDN, ETH, USB, RS232C/485/422
- C, ASM, CUPL, PHP, BASIC C-64

http://er-mik.prv.pl/projekty_avt.php * http://er-mik.prv.pl/ * http://kolejki.prv.pl/

KA-NUCLEO-F411CE Idea , ESP32, Mega2560, UNO PLUS
 
Odpowiedź
#38
Cholera, a zapowiadało się tak dobrze.

W końcu udało mi się podłączyć czujnik do płytki tak, aby ArduinoIDE "widziało" czujnik pod adresami 0x68 i 0x76. Niestety, tylko tyle udało mi się osiągnąć. Program EdTracker GUI nie widzi go i nie da się go edytować (Program Java), natomiast szkic pod ArduinoIDE jakoś nei chce zaskoczyć... Nie wiem w czym jest problem...
Załączam kod, może to coś podpowie

Kod:
//
//  Head Tracker Sketch
//
const char  infoString []   = "EDTrackerMag V4.1.0";

// Changelog:
//            Release
// Date       Version Author  Comment
// 2014-11-10         RJ      Move to Arduino IDE 158 with newer copmiler = smaller code
//                            Combine Invensense DMP output with continuous magnetometer drift adjustment
// 2014-12-24         RJ      Implements startup auto calibration and new magnetometer drift compensation
// 2015-01-25         RJ      Fix mag wraping/clamping.
// 2015-02-13         RJ      Fix mag heading average when heading is close to +/- PI boundary
//                    RJ      Fix incorrect roll compensation for mag heading
// 2015-03-01         RJ      Fix smoothing val not being saved to EEPROM
// 2015-06-20         DH      Fixed incorrect data type for sensor_data breaking compile; cannot upversion
//                            due to lagging behind pocketmoon repo, but no functional change anyway
// 2015-06-20 4.0.4   DH      Forked from 9150 code and modified for MPU-9250
// 2016-01-27 4.0.4   DH      Non-functional code format change to allow compile on IDE 1.6.7
// 2016-01-30 4.0.5   DH      Removed superfluous debug code, fixed mag scaling for 9250, added compiler
//                            warnings for people choosing wrong hardware
// 2017-01-27 4.1.0   DH      Version uplift - compiled with motion driver 6.12; no functional changes
//                            Will hopefully address "stuck in auto bias forever" issue for some users
//                            Big thanks to sir-maniac for his contribution ;)
/* ============================================
EDTracker device code is placed under the MIT License

Copyright (c) 2014-2018 Rob James, Dan Howell

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
===============================================
*/

#pragma message "Sketch is EDTracker2_9250..."

#ifdef MPU9150
#error "MPU9150 is defined; have you chosen the wrong board in Arduino IDE?!"
#endif

// Comment out for production firmware!
//#define DEBUG
#ifdef DEBUG
#warning "****** DEBUG defined  ******"
#endif

/*
* Starting sampling rate. Ignored if POLLMPU is defined above
* Users building for slower (8MHz) AVRs are advised to tune this down to 100
*/
#define DEFAULT_MPU_HZ    (200)

#define EMPL_TARGET_ATMEGA328

#include <EEPROM.h>
#include <Wire.h>
#include <I2Cdev.h>
#include <helper_3dmath.h>
extern "C" {
#include <inv_mpu.h>
#include <inv_mpu_dmp_motion_driver.h>
}

// In raw reading values from the MPU, these values correspond to 90 and 180 degrees
#define DEG90  32768.0
#define DEG180 65536.0

// Array indices of pitch, yaw and roll
#define PITCH 1
#define ROLL  2
#define YAW   0

// MPU9250 has more sensitive magnetometer than 9150, so scale the values down to keep compatibility with GUI
#ifdef MPU9250
#define MAG_SCALEFACTOR 4
#else
#define MAG_SCALEFACTOR 1
#endif

/* EEPROM Offsets for config and calibration stuff*/
#define EE_VERSION 0                                   // char (1 byte)
#define EE_XGYRO 1                                     // long (4 bytes)
#define EE_YGYRO 5                                     // long (4 bytes)
#define EE_ZGYRO 9                                     // long (4 bytes)
#define EE_XACCEL 13                                   // long (4 bytes)
#define EE_YACCEL 17                                   // long (4 bytes)
#define EE_ZACCEL 21                                   // long (4 bytes)
#define EE_ORIENTATION 25                              // char (1 byte)
#define EE_LPF 26                                      // int (2 bytes)
#define EE_EXPSCALEMODE 28                             // 0 for linear, 1 for exponential
#define EE_YAWSCALE 29                                 // 2x 1 byte  in 6:2   0.25 steps should be ok
#define EE_PITCHSCALE 30                               // 2x 1 byte  in 6:2   0.25 steps should be ok
#define EE_YAWEXPSCALE 31                              // 2x 1 byte  in 6:2   0.25 steps should be ok
#define EE_PITCHEXPSCALE 32                            // 2x 1 byte  in 6:2   0.25 steps should be ok
#define EE_POLLMPU 33                                  // *UNUSED* char (1 byte), poll or int mode
#define EE_AUTOCENTRE 34                               // *UNUSED* char (1 byte), auto-centering enabled or disabled TODO : REDUNDANT?
#define EE_CALIBTEMP    35                             // *UNUSED* int (2 byte) - REDUNDANT?
#define EE_MAGOFFX      40                             // int (2 byte) mag offset for X axis in 9:7 format
#define EE_MAGOFFY      42                             // int (2 byte) mag offset for Y axis in 9:7 format
#define EE_MAGOFFZ      44                             // int (2 byte) mag offset for Z axis in 9:7 format
#define EE_MAGXFORM     50                             // (12 bytes) mag calibration matrix in 6 x 2 byte 2:14 signed format

/*
* Pin defines (assume Sparkfun Pro Micro; change as necessary)
*/
#define SDA_PIN 2
#define SCL_PIN 3
#define LED_PIN 17 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
#define BUTTON_PIN 10

#ifndef cbi
#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
#endif

// Flag for if we're outputting detailed GUI info or not
boolean outputUI = false;

// Used for MPU init
unsigned char revision;

// Scaling-related
boolean expScaleMode = 0;         // Exponential or linear scaling mode?
float scaleF[3];                  // Scaling values for each axis
float outputLPF = 0.5;            // Filtering - 0 is no filter,  max value 0.999...

// Timer related so track operations between loop iterations (LED flashing, etc)
unsigned long lastMillis = 0;
unsigned long lastUpdate = 0;

// Holds the time since sketch stared
unsigned long  nowMillis;
boolean blinkState;

// Mag/drift compensation related...
float compass[4];
float magOffset[3];               // Offset values for mag, since no-one ever sits facing perfect north ;)
float magXform[18];               // Transformation matrix for hard/soft iron calibration of the magnetometer
float xDriftComp = 0.0;           // Currently applied drift compensation
float magHeading, lastMagHeading, rawMagHeading;
float lastDriftX;                 // Used to track drift (difference between gyro and mag readings) over loop iterations and adjust
boolean behind = false;           // When there is a difference between mag and gyro headings, used to flag whether it's behind or ahead
int consecCount = 0;              // ...and by how many counts (loops) it has been behind/ahead of the gyro
boolean offsetMag = false;        // The magnetometer reads within a 180 degree arc, not 360, so we keep track of when we are in the "other half" of the
                                 // circle and flag this as an offset mag situation

//Running count of samples - used when recalibrating
int   sampleCount = 0;
boolean calibrated = true;

//Number of samples to take when recalibrating
byte  recalibrateSamples =  255;

long avgGyro[3] ;//= {0, 0, 0};
long gBias[3];                    // Gyro biases for MPU
volatile boolean new_gyro ;
boolean startup = true;           // Flags when we are in the startup (auto-bias) phase (TODO: remove and just use startupPhase for logic)
int  startupPhase = 0;            // and which type of phase it is
int  startupSamples;

// The "Tracker" object which is used to mimic the joystick; see HID.cpp within the hardware folder
TrackState_t joySt;

/* The mounting matrix below tells the MPL how to rotate the raw
* data from the driver(s).
*/
static byte gyro_orients[6] =
{
 B10001000, // right
 B10000101, // front
 B10101100, // left
 B10100001 // rear
}; //ZYX

byte orientation = 2;

//Need some helper funct to read/write integers
void writeIntEE(int address, int value) {
 EEPROM.write(address + 1, value >> 8); //upper byte
 EEPROM.write(address, value & 0xff); // write lower byte
}

int readIntEE(int address) {
 return (EEPROM.read(address + 1) << 8 | EEPROM.read(address));
}

void writeLongEE(int address,  long value) {
 for (int i = 0; i < 4; i++)
 {
   EEPROM.write(address++, value & 0xff); // write lower byte
   value = value >> 8;
 }
}

long readLongEE(int address) {
 return ((long)EEPROM.read(address + 3) << 24 |
         (long)EEPROM.read(address + 2) << 16 |
         (long)EEPROM.read(address + 1) << 8 |
         (long)EEPROM.read(address));
}

/*******************************************************************************************************
* Initialise function
********************************************************************************************************/
void setup() {

 Serial.begin(115200);

 pinMode(BUTTON_PIN, INPUT_PULLUP);
 pinMode(LED_PIN, OUTPUT);

 // send a I2C stop signal
 digitalWrite(SDA_PIN, HIGH);
 digitalWrite(SDA_PIN, LOW);

 loadSettings();

 loadMagCalibration();

 // join I2C bus (I2Cdev library doesn't do this automatically)
 Wire.begin();
 TWBR = 12;//12; // 12 400kHz I2C clock (200kHz if CPU is 8MHz)

 // Disable internal I2C pull-ups
 cbi(PORTD, 0);
 cbi(PORTD, 1);

 // Initialize the MPU:
 initialize_mpu();
 enable_mpu();

}

/*******************************************************************************************************
* MAIN LOOP Function
********************************************************************************************************/

void loop()
{
 long unsigned int sensor_data;
 short gyro[3], accel[3], sensors;
 unsigned char more ;
 unsigned char magSampled ;
 long quat[4];

 //Loop until MPU interrupts us with a reading
 while (!new_gyro)
   ;

 nowMillis = millis();

 sensor_data = 1;
 dmp_read_fifo(gyro, accel, quat, &sensor_data, &sensors, &more);

 if (!more)
   new_gyro = false;

 if (sensor_data == 0)
 {
   Quaternion q( (float)quat[0]  / 1073741824.0f,
                 (float)quat[1]  / 1073741824.0f,
                 (float)quat[2]  / 1073741824.0f,
                 (float)quat[3]  / 1073741824.0f);

   // Calculate Yaw/Pitch/Roll
   // Update client with yaw/pitch/roll and tilt-compensated magnetometer data

   float newv[3];
   //roll
   newv[2] =  atan2(2.0 * (q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z);

   // pitch
   newv[1] = -asin(-2.0 * (q.x * q.z - q.w * q.y));

   //yaw
   newv[0] = -atan2(2.0 * (q.x * q.y + q.w * q.z), q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z);

   short mag[3];
   magSampled  = mpu_get_compass_reg(mag);
   
   if (magSampled == 0)
   {

     if (outputUI) {
       reportRawMag(mag);
     }

     float ox = (float)mag[0]/MAG_SCALEFACTOR - magOffset[0] ;
     float oy = (float)mag[1]/MAG_SCALEFACTOR - magOffset[1] ;
     float oz = (float)mag[2]/MAG_SCALEFACTOR - magOffset[2] ;

     float rx = ox * magXform[0] + oy * magXform[1] + oz * magXform[2];
     float ry = ox * magXform[3] + oy * magXform[4] + oz * magXform[5];
     float rz = ox * magXform[6] + oy * magXform[7] + oz * magXform[8];

     //  pitch/roll adjusted mag heading
     rawMagHeading = wrap(atan2(-(rz * sin(newv[PITCH]) - ry * cos(newv[PITCH])),
                                rx * cos(newv[ROLL]) +
                                ry * sin(-newv[ROLL]) * sin(newv[PITCH]) +
                                rz * sin(-newv[ROLL]) * cos(newv[PITCH])) * -10430.06);

     magHeading = rawMagHeading;

     if (calibrated)
       magHeading = magHeading - compass[3];

     if (offsetMag)
       magHeading = magHeading + 32768.0;

     magHeading = wrap(magHeading);

     // Auto Gyro Calibration
     if (startup)
     {
       blink();
       parseInput();

       if (startupPhase == 0)
       {
         for (int n = 0; n < 3; n++)
           gBias[n] = avgGyro[n] = 0;

         mpu_set_gyro_bias_reg(gBias);
         startupPhase = 1;
         startupSamples = 500;
         return;
       }

       if (startupPhase == 1)
       {
         if (startupSamples == 0)
         {
           for (int n = 0; n < 3; n++)
           {
             gBias[n] = (avgGyro[n] / -250);
           }
           xDriftComp = 0.0;
           mpu_set_gyro_bias_reg(gBias);
           startupPhase = 2;
           startupSamples = 0;
           lastDriftX = newv[0];
           //return;
         }
         else
         {
           for (int n = 0; n < 3; n++)
             avgGyro[n] += gyro[n];
         }
         startupSamples --;
         return ;
       }

       if (startupPhase == 2)
       {
         if (startupSamples == 1500)
         {
           xDriftComp = (newv[0] - lastDriftX) * 69.53373;
           startup = false;
           recenter();
           Serial.println("H"); // Hello
         }
         startupSamples++;
         return ;
       }
     }
#ifdef DEBUG
   } else {
     // TODO remove
     Serial.print("Mag oops [");
     Serial.print(magSampled);
     Serial.println("]");
#endif
   }

   //scale +/- PI to +/- 32767
   for (int n = 0; n < 3; n++)
     newv[n] = newv[n]   * 10430.06;

   // move to pre-calibrate
   if (!calibrated)
   {
     if (sampleCount < recalibrateSamples)
     { // accumulate samples
       for (int n = 0; n < 3; n++)
         compass[n] += newv[n];

       compass[3] += magHeading;
       sampleCount ++;
     }
     else
     {
       calibrated = true;
       for (int n = 0; n < 4; n++)
         compass[n] = compass[n] / (float)sampleCount;
       if (offsetMag)
         compass[3] = compass[3] - 32768.0;
       offsetMag = false;

       recalibrateSamples = 100;//  calibration next time around
       if (outputUI )
       {
         sendInfo();
       }
     }
     return;
   }

   // apply drift offsets
   for (int n = 0; n < 3; n++)
     newv[n] = newv[n] - compass[n];

   // this should take us back to zero BUT we may have wrapped so ..
   if (newv[0] < -32768.0)
     newv[0] += 65536.0;

   if (newv[0] > 32768.0)
     newv[0] -= 65536.0 ;

   long ii[3];

   for (int n = 0; n < 3; n++)
   {
     if (expScaleMode) {
       // Exponential scaling mode
       ii[n] = (long) (0.000122076 * newv[n] * newv[n] * scaleF[n]) * (newv[n] / abs(newv[n]));
     }
     else
     {
       // Linear scale to our target range plus a 'sensitivity' factor;
       ii[n] = (long)(newv[n] * scaleF[n] );
     }
   }

   // clamp after scaling to keep values within 16 bit range
   for (int n = 0; n < 3; n++)
     ii[n] = constrain(ii[n], -32767, 32767);

   // Do it to it (Robspeak for "set the axis values on the HID object"!)
   if (ii[0] > 30000  || ii[0] < -30000) {
     joySt.xAxis = ii[0] ;
   } else {
     joySt.xAxis = joySt.xAxis * outputLPF + ii[0] * (1.0 - outputLPF) ;
   }
     joySt.yAxis = joySt.yAxis * outputLPF + ii[1] * (1.0 - outputLPF) ;
     joySt.zAxis = joySt.zAxis * outputLPF + ii[2] * (1.0 - outputLPF) ;
   
   Tracker.setState(&joySt);

   // Have we been asked to recalibrate ?
   if (digitalRead(BUTTON_PIN) == LOW)
   {
     recenter();
     // return;
   }

   //reports++;
   if (outputUI )
   {
     Serial.print(joySt.xAxis);
     printtab();
     Serial.print(joySt.yAxis);
     printtab();
     Serial.print(joySt.zAxis);
     printtab();

     tripple(accel);
     tripple(gyro);

#ifdef DEBUG
     Serial.print((int)rawMagHeading ); //
     printtab();
     Serial.print((int)magHeading  );
     printtab();
     Serial.print((int)compass[3] ); //
     printtab();
#endif

     Serial.println("");

   }

   if (nowMillis > lastUpdate)
   {
     blink();

     // apply drift compensation
     compass[0] = compass[0] + xDriftComp;

     //handle wrap
     if (compass[0] > 65536.0)
       compass[0] = compass[0] - 65536.0;
     else if (compass[0] < -65536.0 )
       compass[0] = compass[0] + 65536.0;

     lastUpdate = nowMillis + 100;
     lastDriftX = newv[0];

     if (outputUI )
     {
       long tempNow;
       mpu_get_temperature (&tempNow);
       Serial.print("T\t");
       Serial.println(tempNow);
     }
     
   }

   // magHeading = constrain (magHeading, -32767, 32767);

   if (//magHeading != lastMagHeading  &&
     // abs(magHeading) < 10000  &&
     newv[1] < 6000.0  && newv[1] > -6000.0
   )
   {
     lastMagHeading = magHeading;

     // Mag is so noisy on 9150 we just ask 'is Mag ahead or behind DMP'
     // and keep a count of consecutive behinds or aheads and use this
     // to adjust our DMP heading and also tweak the DMP drift
     float delta = magHeading - newv[0];

     if (delta > 32768.0)
       delta = 65536.0 - delta;
     else if (delta < -32767)
       delta = 65536.0 + delta;

     if (delta > 0.0)
     {
       if (behind )
       {
         consecCount --;
       }
       else
       {
         consecCount = 0;
       }
       behind = true;
     }
     else
     {
       if (!behind)
       {
         consecCount  ++;
       }
       else
       {
         consecCount = 0;
       }
       behind = false;
     }

     // Tweek the yaw offset. 0.01 keeps the head central with no visible twitching
     compass[0] = compass[0] + (float)(consecCount * abs(consecCount)) * 0.012;

     //Also tweak the overall drift compensation.
     //DMP still suffers from 'warm-up' issues and this helps greatly.
     xDriftComp = xDriftComp + (float)(consecCount) * 0.00001;
   }
   parseInput();
 }

}


/*******************************************************************************************************
* Serial Input parsing (UI commands) function
********************************************************************************************************/
void parseInput()
{
 while (Serial.available() > 0)
 {
   // read the incoming byte:
   byte command = Serial.read();

   if (command == 'C')
   {
     int data;

     for (int i = 0; i < 2; i++)
     {
       data = Serial.read();
       data = data | Serial.read() << 8;
       scaleF[i] = (float)data / 256.0;
     }

     data = Serial.read();
     data = data | Serial.read() << 8;
     outputLPF = (float)data / 32767.0;
     Serial.print("M\t");
     Serial.println(outputLPF);

     setScales();
     scl();
   }
   else if (command == 'S')
   {
     outputUI = false;
     Serial.println("S"); //silent
     dmp_set_fifo_rate(DEFAULT_MPU_HZ);
   }
   else if (command == 'H')
   {
     if (startup)
       Serial.println("h");
     else
       Serial.println("H"); // Hello
   }
   else if (command == 't')
   {
     //toggle linear/expoinential mode
     expScaleMode = !expScaleMode;
     getScales();
     EEPROM.write(EE_EXPSCALEMODE, expScaleMode);
     scl();
   }
   else if (command == 'V')
   {
     Serial.println("V"); //verbose
     sendInfo();
     scl();
     outputUI = true;
     // if (DEFAULT_MPU_HZ > 101)
     dmp_set_fifo_rate(DEFAULT_MPU_HZ / 2);
     reportRawMagMatrix();
   }
   else if (command == 'I')
   {
     sendInfo();
     scl();
     reportRawMagMatrix();
     sendByte('O', orientation);
   }
   else if (command == 'P')
   {
     mpu_set_dmp_state(0);
     orientation = (orientation + 1) % 4; //0 to 3
     dmp_set_orientation(gyro_orients[orientation]);
     mpu_set_dmp_state(1);
     sendByte('O', orientation);
     EEPROM.write(EE_ORIENTATION, orientation);
   }
   else if (command == 'R')
   {
     //recalibrate offsets
     recenter();
   }
   else if (command == 'r')
   {
     //20 second calibration
     fullCalib();
   }
   else if (command == 87) // full wipe
   {
     byte len, data;
     len = Serial.read();

     for (int i = 0; i < len; i++)
     {
       data = Serial.read();
       EEPROM.write( i, data );
     }

     loadSettings();
     loadMagCalibration();

     mpu_set_dmp_state(0);
     dmp_set_orientation(gyro_orients[orientation]);
     mpu_set_dmp_state(1);

   }
   else if (command == 36) // Mag Calibration Matrix pushed down from UI with orientation applied
   {
     byte data;
     for (int i = 0 ; i < 6; i++)
     {
       data = Serial.read();
       EEPROM.write(EE_MAGOFFX + i, data );
     }

     for (int i = 0 ; i < 36 ; i++) // 18 + 18 is raw mag xform then orientated xform
     {
       data = Serial.read();
       EEPROM.write(EE_MAGXFORM + i, data );
     }

     loadMagCalibration();
   }
 }
}


/*******************************************************************************************************
* Interrupt service routine for new reading from MPU
********************************************************************************************************/
ISR(INT6_vect) {
 new_gyro = 1;
}

/*******************************************************************************************************
* MPU initialisation
********************************************************************************************************/
void initialize_mpu() {
 mpu_init(&revision);
 mpu_set_compass_sample_rate(100); // defaults to 100 in the libs

 /* Get/set hardware configuration. Start gyro. Wake up all sensors. */
 mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS);
 //  mpu_set_gyro_fsr (2000);//250
 //  mpu_set_accel_fsr(2);//4

 /* Push both gyro and accel data into the FIFO. */
 mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
 mpu_set_sample_rate(DEFAULT_MPU_HZ);

 dmp_load_motion_driver_firmware();
 dmp_set_orientation(gyro_orients[orientation]);

 //dmp_register_tap_cb(&tap_cb);

 unsigned short dmp_features = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_RAW_ACCEL |
                               DMP_FEATURE_SEND_RAW_GYRO ;//| DMP_FEATURE_GYRO_CAL;

 //dmp_features = dmp_features |  DMP_FEATURE_TAP ;

 dmp_enable_feature(dmp_features);
 dmp_set_fifo_rate(DEFAULT_MPU_HZ);

 return ;
}

/*******************************************************************************************************
* Disable MPU routine
********************************************************************************************************/
void disable_mpu() {
 mpu_set_dmp_state(0);
 EIMSK &= ~(1 << INT6);      //deactivate interupt
}

/*******************************************************************************************************
* Enable MPU routine
********************************************************************************************************/
void enable_mpu() {
 EICRB |= (1 << ISC60) | (1 << ISC61); // sets the interrupt type for EICRB (INT6)
 EIMSK |= (1 << INT6); // activates the interrupt. 6 for 6, etc
 mpu_set_dmp_state(1);  // This enables the DMP; at this point, interrupts should commence
}

/*******************************************************************************************************
* Blink LED function
********************************************************************************************************/
void blink()
{
 /**/
 if (nowMillis > lastMillis )
 {
   blinkState = !blinkState;
   digitalWrite(LED_PIN, blinkState);

   if (startup)
     lastMillis = nowMillis + 100;
   else
     lastMillis = nowMillis + 500;
 }/**/
}


/*******************************************************************************************************
* Helper function to wrap an angular value
********************************************************************************************************/
float wrap(float angle)
{
 if (angle > DEG90)
   angle -= (DEG180);
 if (angle < -DEG90)
   angle += (DEG180);
 return angle;
}

/*******************************************************************************************************
* [M] Function to recenter the device to the current positional reading
********************************************************************************************************/
void recenter()
{
 if (outputUI) {
   Serial.println("M\tR");
 }
 sampleCount = 0;

 for (int i = 0; i < 4; i++)
   compass[i] = 0.0;

 calibrated = false;
 offsetMag = false;

 if (rawMagHeading > 28000.0  || rawMagHeading < -28000.0) {
   offsetMag = true;
 }
}

/*******************************************************************************************************
* Convenience function - output a triple array to serial IO
********************************************************************************************************/
void tripple(short * v)
{
 for (int i = 0; i < 3; i++)
 {
   Serial.print(v[i] ); //
   printtab();
 }
}

/*******************************************************************************************************
* Convenience function - output a triple array prefixed by a message to serial IO
********************************************************************************************************/
void mess(char * m, long * v)
{
 Serial.print(m);
 Serial.print(v[0]); Serial.print(" / ");
 Serial.print(v[1]); Serial.print(" / ");
 Serial.println(v[2]);
}

/*******************************************************************************************************
* Get axis scaling from EEPROM
********************************************************************************************************/
void getScales()
{
 if (expScaleMode)
 {
   scaleF[0] = (float)EEPROM.read(EE_YAWEXPSCALE) / 4.0;
   scaleF[1] = (float)EEPROM.read(EE_PITCHEXPSCALE) / 4.0;
 }
 else
 {
   scaleF[0] = (float)EEPROM.read(EE_YAWSCALE) / 4.0;
   scaleF[1] = (float)EEPROM.read(EE_PITCHSCALE) / 4.0;
 }
 scaleF[2] = scaleF[1];
}

/*******************************************************************************************************
* Save axis scaling to EEPROM
********************************************************************************************************/
void setScales()
{
 if (expScaleMode)
 {
   EEPROM.write(EE_YAWEXPSCALE, (int)(scaleF[0] * 4));
   EEPROM.write(EE_PITCHEXPSCALE, (int)(scaleF[1] * 4));
 }
 else
 {
   EEPROM.write(EE_YAWSCALE, (int)(scaleF[0] * 4));
   EEPROM.write(EE_PITCHSCALE, (int)(scaleF[1] * 4));
 }
 scaleF[2] = scaleF[1];

 writeIntEE(EE_LPF, (int)(outputLPF * 32767.0));
}

/*******************************************************************************************************
* Convenience function - send a character and a boolean to serial IO
********************************************************************************************************/
void sendBool(char x, boolean v)
{
 Serial.print(x);
 printtab();
 Serial.println(v);
}

/*******************************************************************************************************
* Convenience function - send a character and a byte to serial IO
********************************************************************************************************/
void sendByte(char x, byte b)
{
 Serial.print(x);
 printtab();
 Serial.println(b);
}

/*******************************************************************************************************
* [I] Output info string to serial IO
********************************************************************************************************/
void sendInfo()
{
 Serial.print("I\t");
 Serial.println(infoString);
}

/*******************************************************************************************************
* [s] Output config data to serial IO (scaling mode, scale settings and LPF setting)
********************************************************************************************************/
void scl()
{
 Serial.print("s\t");
 Serial.print(expScaleMode);
 printtab();
 Serial.print(scaleF[0]);
 printtab();
 Serial.print(scaleF[1]);
 printtab();
 Serial.println(outputLPF);
}

/*******************************************************************************************************
* [Q] Output raw mag data to serial IO
********************************************************************************************************/
void reportRawMag(short mag[])
{
 Serial.print("Q\t");
 for (int i = 0; i < 3; i++)
 {
   Serial.print(mag[i]/MAG_SCALEFACTOR);
   printtab();
 }
 Serial.println("");
}

/*******************************************************************************************************
* [q] Output the raw mag calibration matrix to serial IO
********************************************************************************************************/
void reportRawMagMatrix()
{
 Serial.print("q\t");
 for (int i = 0; i < 3 ; i++)
 {
   Serial.print( magOffset[i], 6);
   printtab();
 }
 //raw mag calibration
 for (int i = 9; i < 18 ; i++)
 {
   Serial.print( magXform[i], 13);
   printtab();
 }
 Serial.println("");
}

/*******************************************************************************************************
* Recenter the device and put it into the startup phase for a full gyro calibration to be performed
********************************************************************************************************/
void fullCalib()
{
 recenter();
 startupPhase = 0;
 startup = true;
}

void printtab()
{
 Serial.print("\t");
}

/*******************************************************************************************************
* Read the mag calibration values from EEPROM and set the runtime vars accordingly
********************************************************************************************************/
void loadMagCalibration()
{
 for (int i = 0; i < 3 ; i ++)
 {
   magOffset[i] = (float)readIntEE(EE_MAGOFFX + (i * 2)) / 64.0;
 }

 for (int i = 0; i < 18 ; i ++)//orientated then raw
 {
   magXform[i] = (float)readIntEE(EE_MAGXFORM + i * 2) / 8192.0;
 }
}

/*******************************************************************************************************
* Wrapper function to do all settings loading - orientation, scaling mode, scaling values and LPF
* setting
********************************************************************************************************/
void loadSettings()
{
 orientation = constrain(EEPROM.read(EE_ORIENTATION), 0, 3);
 expScaleMode = EEPROM.read(EE_EXPSCALEMODE);
 getScales();
 outputLPF = (float)readIntEE(EE_LPF) / 32767.0;
}
 
Odpowiedź
  


Skocz do:


Przeglądający: 1 gości