Go Down

Topic: [Multicotteri] Elettronica : IMU, MCU, Sensori ed algoritmi di controllo (Read 140010 times) previous topic - next topic

lesto

io ho bisogno di essere rianimato, da quando ho postato la gioda non ho più scritto una linea di codice... son troppo stanco, spero di combinare qualcosa nel week-end!
sei nuovo? non sai da dove partire? leggi qui: http://playground.arduino.cc/Italiano/Newbie

Federico

Io per aggiornarvi sto facendo un po' di voli al parco e ho iniziato a provare pid a casa mentre attendo il mio ricevitore BT per provare pid al parco. Si direbbe dai primi test che nel mio caso la P<4 sia la cosa migliore, altro che valori come 15... Domani far un giro al radiant per scoprire se trovo qualcosa che mi incuriosisce.
F
Federico - Sideralis
Arduino &C: http://www.sideralis.org
Foto: http://blackman.amicofigo.com

lesto

lentamente vado avanti a programmare, oggi con la piacevole sorpresa di scoprire che l'IDE 1.0 implementa l'operatore NEW.

giusto in caso vogliate testare i vostri sensori, vi passo il codice. sarebbe carino sapere se vi funzionano o sono da cambiare gli indirizzi nelle vostre IMU.

ADXL345
Code: [Select]

#include <Wire.h>

#define address 0x53

void setup(){
 //Initialize Serial and I2C communications
 Serial.begin(9600);
 delay(1000);
 Wire.begin();
 Serial.print("START ");
 
 //IMPORTANT!!!!
 //THIS WILL WAKE UP ADXL!!!!
 Wire.beginTransmission(address);
 Wire.send(0x2D);  
 Wire.send(0x08);
 Wire.endTransmission();
 
/
}

void loop(){
 
 int x,y,z; //triple axis data
 Serial.print("START READING 1");
 
 Wire.beginTransmission(0x53);
 Wire.send(0x32);
 Wire.endTransmission();
 
Serial.print("START READING 2");
//Read data from each axis, 2 registers per axis
 Wire.requestFrom(0x53, 6);
 Serial.print("available: ");
 Serial.println(Wire.available(), DEC);
 while(Wire.available()<6){
   Serial.print("available: ");
   Serial.println(Wire.available(), DEC);
   delay(250);
 }
 if(6<=Wire.available()){
   x = Wire.receive()<<8; //X msb
   x |= Wire.receive(); //X lsb
   z = Wire.receive()<<8; //Z msb
   z |= Wire.receive(); //Z lsb
   y = Wire.receive()<<8; //Y msb
   y |= Wire.receive(); //Y lsb
 }
 
 //Print out values of each axis
 Serial.print("x: ");
 Serial.print(x);
 Serial.print("  y: ");
 Serial.print(y);
 Serial.print("  z: ");
 Serial.println(z);
 
 delay(250);
}


ITG3200 (very raro, difficile da trovare codice così "basico" da leggere per questo sensore, usano tutti classi wire esterne e incasinate)
Code: [Select]
#include <Wire.h> //I2C Arduino Library

#define address 0x68

void setup(){
 //Initialize Serial and I2C communications
 Serial.begin(9600);
 delay(1000);
 Wire.begin();
 Serial.print("START ");
 
 Wire.beginTransmission(address);
 Wire.write(0x3E);
 Wire.write(0x80);  //send a reset to the device
 Wire.endTransmission(); //end transmission
 
 Wire.beginTransmission(address);
 Wire.write( 0x15 );
 Wire.write( byte(0x00) );   //sample rate divider to 0
 Wire.endTransmission(); //end transmission

 Wire.beginTransmission(address);
 Wire.write(0x16);
 Wire.write(0x18); // ±2000 degrees/s 8kHz sample rate(default value)
 Wire.endTransmission(); //end transmission

}

void loop(){
 
 int x,y,z; //triple axis data
 Serial.print("START READING 1");
 //Tell the HMC5883 where to begin reading data
 Wire.beginTransmission(address);
 Wire.write(0x1D); //select register 3, X MSB register
 Wire.endTransmission();
 
Serial.print("START READING 2");
//Read data from each axis, 2 registers per axis
 Wire.requestFrom(address, 6);
 while(Wire.available()<6){
   Serial.print("available: ");
   Serial.println(Wire.available(), DEC);
 }
 if(6<=Wire.available()){
   x = Wire.read()<<8; //X msb
   x |= Wire.read(); //X lsb
   z = Wire.read()<<8; //Z msb
   z |= Wire.read(); //Z lsb
   y = Wire.read()<<8; //Y msb
   y |= Wire.read(); //Y lsb
 }
 
 //Print out values of each axis
 Serial.print("x: ");
 Serial.print(x);
 Serial.print("  y: ");
 Serial.print(y);
 Serial.print("  z: ");
 Serial.println(z);
 
 delay(250);
}


HMC5883L (non scritto da me)
Code: [Select]
/*
An Arduino code example for interfacing with the HMC5883

by: Jordan McConnell
SparkFun Electronics
created on: 6/30/11
license: OSHW 1.0, http://freedomdefined.org/OSHW

Analog input 4 I2C SDA
Analog input 5 I2C SCL
*/

#include <Wire.h> //I2C Arduino Library

#define address 0x1E //0011110b, I2C 7bit address of HMC5883

void setup(){
 //Initialize Serial and I2C communications
 Serial.begin(9600);
 delay(1000);
 Wire.begin();
 Serial.print("START ");
 
 //Put the HMC5883 IC into the correct operating mode
 Wire.beginTransmission(address); //open communication with HMC5883
 Serial.print("SETUP OK0");
 Wire.send(0x02); //select mode register
 Serial.print("SETUP OK1");
 Wire.send(0x00); //continuous measurement mode
 Serial.print("SETUP OK2");
 Wire.endTransmission();
 Serial.print("SETUP OK");
}

void loop(){
 
 int x,y,z; //triple axis data
 Serial.print("START READING 1");
 //Tell the HMC5883 where to begin reading data
 Wire.beginTransmission(address);
 Wire.send(0x03); //select register 3, X MSB register
 Wire.endTransmission();
 
Serial.print("START READING 2");
//Read data from each axis, 2 registers per axis
 Wire.requestFrom(address, 6);
 if(6<=Wire.available()){
   x = Wire.receive()<<8; //X msb
   x |= Wire.receive(); //X lsb
   z = Wire.receive()<<8; //Z msb
   z |= Wire.receive(); //Z lsb
   y = Wire.receive()<<8; //Y msb
   y |= Wire.receive(); //Y lsb
 }
 
 //Print out values of each axis
 Serial.print("x: ");
 Serial.print(x);
 Serial.print("  y: ");
 Serial.print(y);
 Serial.print("  z: ");
 Serial.println(z);
 
 delay(250);
}
sei nuovo? non sai da dove partire? leggi qui: http://playground.arduino.cc/Italiano/Newbie

astrobeed


ITG3200 (very raro, difficile da trovare codice così "basico" da leggere per questo sensore, usano tutti classi wire esterne e incasinate)


C'è un motivo per cui è deprecabile l'uso della wire di Arduino su un quadricottero, la libreria in caso di errori sulla I2C ha il brutto vizio di bloccare completamente l'esecuzione dello sketch.
Di questa cosa ne avevamo parlato in modo esaustivo nel vecchio thread su i quadricotteri, a suo tempo avevo anche fatto notare che nella versione 1.7 di Multiwii era possibile arrivare lo stesso al blocco dello sketch in caso di errori I2C anche se solo in condizioni molto particolari ed improbabili, problema risolto dalla 1.8 in poi.
Ti consiglio di prendere la gestione I2C dell'ultima release di multiwii se non vuoi fare botti per colpa della wire.

lesto

non intendo usare wire per la release, ma almeno per iniziare a fare i test voglio tenere il codice più pulito possibile. In oltre il codice scritto con la wire è comprensibile anche ai profani :)

comunque manca la seconda parte del post, che posterò stasera, in cui ci sono gli sketch per magnetometro e barometro (non scritti da me) e una piccola spiegazione del file allegato, che è il proseguimento della guida agli oggetti, in cui spiegerò come, grazie alla classe astratta Sensor e all'operatore new (di default nell'IDE 1.0) sia facilissimo implementare un nuovo sensore minimizzando l'uso di ifdef
sei nuovo? non sai da dove partire? leggi qui: http://playground.arduino.cc/Italiano/Newbie

lesto

*comunque mancava la seconda parte del post sui sensori, maledetto errore 500 del forum*


BMP085 (non scritto da me)
Code: [Select]
/* BMP085 Extended Example Code
  by: Jim Lindblom
  SparkFun Electronics
  date: 1/18/11
  license: CC BY-SA v3.0 - http://creativecommons.org/licenses/by-sa/3.0/
 
  Get pressure and temperature from the BMP085 and calculate altitude.
  Serial.print it out at 9600 baud to serial monitor.

  Update (7/19/11): I've heard folks may be encountering issues
  with this code, who're running an Arduino at 8MHz. If you're
  using an Arduino Pro 3.3V/8MHz, or the like, you may need to
  increase some of the delays in the bmp085ReadUP and
  bmp085ReadUT functions.
*/

#include <Wire.h>

#define BMP085_ADDRESS 0x77  // I2C address of BMP085

const unsigned char OSS = 0;  // Oversampling Setting

// Calibration values
int ac1;
int ac2;
int ac3;
unsigned int ac4;
unsigned int ac5;
unsigned int ac6;
int b1;
int b2;
int mb;
int mc;
int md;

// b5 is calculated in bmp085GetTemperature(...), this variable is also used in bmp085GetPressure(...)
// so ...Temperature(...) must be called before ...Pressure(...).
long b5;

short temperature;
long pressure;

void setup()
{
  Serial.begin(9600);
  delay(2000);
  Wire.begin();
  bmp085Calibration();
}

void loop()
{
  temperature = bmp085GetTemperature(bmp085ReadUT());
  pressure = bmp085GetPressure(bmp085ReadUP());
  Serial.print("Temperature: ");
  Serial.print(temperature, DEC);
  Serial.println(" *0.1 deg C");
  Serial.print("Pressure: ");
  Serial.print(pressure, DEC);
  Serial.println(" Pa");
  Serial.println();
  delay(1000);
}

// Stores all of the bmp085's calibration values into global variables
// Calibration values are required to calculate temp and pressure
// This function should be called at the beginning of the program
void bmp085Calibration()
{
  ac1 = bmp085ReadInt(0xAA);
  ac2 = bmp085ReadInt(0xAC);
  ac3 = bmp085ReadInt(0xAE);
  ac4 = bmp085ReadInt(0xB0);
  ac5 = bmp085ReadInt(0xB2);
  ac6 = bmp085ReadInt(0xB4);
  b1 = bmp085ReadInt(0xB6);
  b2 = bmp085ReadInt(0xB8);
  mb = bmp085ReadInt(0xBA);
  mc = bmp085ReadInt(0xBC);
  md = bmp085ReadInt(0xBE);
}

// Calculate temperature given ut.
// Value returned will be in units of 0.1 deg C
short bmp085GetTemperature(unsigned int ut)
{
  long x1, x2;
 
  x1 = (((long)ut - (long)ac6)*(long)ac5) >> 15;
  x2 = ((long)mc << 11)/(x1 + md);
  b5 = x1 + x2;

  return ((b5 + 8)>>4); 
}

// Calculate pressure given up
// calibration values must be known
// b5 is also required so bmp085GetTemperature(...) must be called first.
// Value returned will be pressure in units of Pa.
long bmp085GetPressure(unsigned long up)
{
  long x1, x2, x3, b3, b6, p;
  unsigned long b4, b7;
 
  b6 = b5 - 4000;
  // Calculate B3
  x1 = (b2 * (b6 * b6)>>12)>>11;
  x2 = (ac2 * b6)>>11;
  x3 = x1 + x2;
  b3 = (((((long)ac1)*4 + x3)<<OSS) + 2)>>2;
 
  // Calculate B4
  x1 = (ac3 * b6)>>13;
  x2 = (b1 * ((b6 * b6)>>12))>>16;
  x3 = ((x1 + x2) + 2)>>2;
  b4 = (ac4 * (unsigned long)(x3 + 32768))>>15;
 
  b7 = ((unsigned long)(up - b3) * (50000>>OSS));
  if (b7 < 0x80000000)
    p = (b7<<1)/b4;
  else
    p = (b7/b4)<<1;
   
  x1 = (p>>8) * (p>>8);
  x1 = (x1 * 3038)>>16;
  x2 = (-7357 * p)>>16;
  p += (x1 + x2 + 3791)>>4;
 
  return p;
}

// Read 1 byte from the BMP085 at 'address'
char bmp085Read(unsigned char address)
{
  unsigned char data;
 
  Wire.beginTransmission(BMP085_ADDRESS);
  Wire.send(address);
  Wire.endTransmission();
 
  Wire.requestFrom(BMP085_ADDRESS, 1);
  while(!Wire.available())
    ;
   
  return Wire.receive();
}

// Read 2 bytes from the BMP085
// First byte will be from 'address'
// Second byte will be from 'address'+1
int bmp085ReadInt(unsigned char address)
{
  unsigned char msb, lsb;
 
  Wire.beginTransmission(BMP085_ADDRESS);
  Wire.send(address);
  Wire.endTransmission();
 
  Wire.requestFrom(BMP085_ADDRESS, 2);
  while(Wire.available()<2)
    ;
  msb = Wire.receive();
  lsb = Wire.receive();
 
  return (int) msb<<8 | lsb;
}

// Read the uncompensated temperature value
unsigned int bmp085ReadUT()
{
  unsigned int ut;
 
  // Write 0x2E into Register 0xF4
  // This requests a temperature reading
  Wire.beginTransmission(BMP085_ADDRESS);
  Wire.send(0xF4);
  Wire.send(0x2E);
  Wire.endTransmission();
 
  // Wait at least 4.5ms
  delay(5);
 
  // Read two bytes from registers 0xF6 and 0xF7
  ut = bmp085ReadInt(0xF6);
  return ut;
}

// Read the uncompensated pressure value
unsigned long bmp085ReadUP()
{
  unsigned char msb, lsb, xlsb;
  unsigned long up = 0;
 
  // Write 0x34+(OSS<<6) into register 0xF4
  // Request a pressure reading w/ oversampling setting
  Wire.beginTransmission(BMP085_ADDRESS);
  Wire.send(0xF4);
  Wire.send(0x34 + (OSS<<6));
  Wire.endTransmission();
 
  // Wait for conversion, delay time dependent on OSS
  delay(2 + (3<<OSS));
 
  // Read register 0xF6 (MSB), 0xF7 (LSB), and 0xF8 (XLSB)
  Wire.beginTransmission(BMP085_ADDRESS);
  Wire.send(0xF6);
  Wire.endTransmission();
  Wire.requestFrom(BMP085_ADDRESS, 3);
 
  // Wait for data to become available
  while(Wire.available() < 3)
    ;
  msb = Wire.receive();
  lsb = Wire.receive();
  xlsb = Wire.receive();
 
  up = (((unsigned long) msb << 16) | ((unsigned long) lsb << 8) | (unsigned long) xlsb) >> (8-OSS);
 
  return up;
}


A breve l'implementazione della DCM, vi allego anche la guida sul codice aggiornata ad oggi, manca però ancora il pezzo dove affronto il motivo per cui ho fatto tutto questo sbatti ovvero:
Code: [Select]

Sensor *sensori[2];
[...]
sensori[0]=new ITG3200();
  (*sensori[0]).setup(&dataSensor);


i più perspicaci noteranno come ora per implementare un giroscopio differente basterà creare la classe estendendo Sensor, e poi al posto dell'assegnazione "brutale", fare una assegnazione basata sulle #ifdef. In questo modo si hanno le ifdef sono in fase di crazione ed assegnazione delle classi, e non sparse per tutto il codice che non si capisce una mazza!  :smiley-mr-green:
sei nuovo? non sai da dove partire? leggi qui: http://playground.arduino.cc/Italiano/Newbie

sciorty

Ragazzi scusatemi la domanda, però il topic è un po' dispersivo e per estrapolare l'informazione che mi serve dovrei spulciare 48 pagine :\
Mi sono promesso che prima o poi proverò a costruire un multicottero però non mi sento ancora all'altezza per vari motivi, tra questi c'è l'algoritmo..
Ora, sinceramente parlando di solito mi ha spaventato l'elettronica nei miei progetti perchè frequento un indirizzo informatico all'ITIS e problemi con la logica e programmazione non ne ho avuti nel mio piccolo.
Quindi, tornando a noi, vorrei chiarire i miei dubbi:
La logica è (detta proprio terra terra): controllo il giroscopio, se pende a destra, aumento il motore di destra?
Se così fosse, ciò non mi torna. Alla fine quando voglio che il multicottero vada avanti non devo diminuire il motore frontale? Dovrei quindi ignorare per quel momento il giroscopio?

Come vedete sono assai confuso, illuminatemi :D

astrobeed


Come vedete sono assai confuso, illuminatemi :D


Prima di tutto devi leggerti completamente i tre thread dedicati ai quadricotteri, sopratutto quello principale, ormai chiuso, di oltre 100 pagine perché li trovi di tutto di più.
Se vuoi cominciare comprati un telaio pronto e usa MultiWii come software, così hai fin da subito un qualcosa sicuramente funzionante che puoi studiare e modificare a tuo piacere con calma.

sciorty



Prima di tutto devi leggerti completamente i tre thread dedicati ai quadricotteri, sopratutto quello principale, ormai chiuso, di oltre 100 pagine perché li trovi di tutto di più.
Se vuoi cominciare comprati un telaio pronto e usa MultiWii come software, così hai fin da subito un qualcosa sicuramente funzionante che puoi studiare e modificare a tuo piacere con calma.



Non so..se devo spenderci l'equivalente per costruirmelo da solo...
Cosa intendi per telaio?

Comunque la mia era solo una coriosità per la logica del programma, la mia ipotesi è del tutto sbagliata ? :)

lesto

nel mio codice io faccio così: all'input della ricevente assegno un angolo. se non fai niente l'angolo è 0, se dici avanti +5°, se dici indietro -5° (per esempio)

ora, integrando il giroscopio (che restituisce la velocità angolare, NON l'angolo) e i dati degli altri sensori, ottengo l'inclinazione del mezzo. Confronto l'inclinazione del mezzo con quella desiderata, e poi intervengo sui motori come hai detto tu.

Per sapere QUANTO devo agire sui motori mi affido ad un basico algoritmo PID.
sei nuovo? non sai da dove partire? leggi qui: http://playground.arduino.cc/Italiano/Newbie

sciorty

ahaha sei il mio preferito lesto!
Io pensavo che il giroscopio tornasse un angolo, e invece..! Comunque più o meno ci sono: se gli dico di andare avanti inclinandosi di x allora agisco sui motori fino a che l'inclinazione non ha raggiunto x ?

lesto

esatto, però puoi seguire altri metodi. Per esempio, se dici avanti al massimo, col mio caso non ti pieghi più di 5° (è un esempio, nella realtà il limite è 30°), invece potresti dire che quando dici avanti al massimo, di AGGIUNGERE 5° all'angolo attuale

un altro sistema angora, usando solo i giroscopi, è legare agli stick non l'angolo, ma la velocità angolare desiderata. (questo credo sia il sistema usato dalla KKboard)
sei nuovo? non sai da dove partire? leggi qui: http://playground.arduino.cc/Italiano/Newbie

astrobeed


un altro sistema angora, usando solo i giroscopi, è legare agli stick non l'angolo, ma la velocità angolare desiderata. (questo credo sia il sistema usato dalla KKboard)


Tutti i sistemi che usano solo il giroscopio implementano una logica del tipo "tenere velocità angolare uguale a 0", in pratica se dai comando con lo stick, o se arriva una folata di vento, vai a perturbare il sistema che reagisce cercando di correggere con un movimento contrario al fine di azzerare la velocità di rotazione dell'asse, però l'assetto rimane quello imposto.
Se ci fai caso tutti i quadri che volano solo col giroscopio una volta che li metti con un certo assetto poi ci rimangono anche se porti gli stick a zero, per rimetterli in piano devi dare comando contrario a differenza del modo di volo stabile, dove serve anche l'accelerometro, che riporta sempre il quadri in assetto ortogonale al terreno azzerando di fatto anche la velocità di avanzamento.

lesto

un altro sistema è quello di dare il giro in pasto alla DCM con un finto input dell'accelerometro a 0, 0, 1... in questo modo puoi leggere un angolo assoluto dato dal giroscopio, che però per via del finto accelerometro che continua a dire di essere dritto tende ad azzerarsi più o meno in fretta (dipende dal PI della DCM).

Si tratta di un comportamento che ho notato testando la DCM, che mi piacerebbe testare in volo

edit: il vantaggio sta nell'eliminare il drift e gli errori di integrazione avendo però un sistema un poco più "intelligente" della sola velocità angolare
sei nuovo? non sai da dove partire? leggi qui: http://playground.arduino.cc/Italiano/Newbie

sciorty


Tutti i sistemi che usano solo il giroscopio implementano una logica del tipo "tenere velocità angolare uguale a 0", in pratica se dai comando con lo stick, o se arriva una folata di vento, vai a perturbare il sistema che reagisce cercando di correggere con un movimento contrario al fine di azzerare la velocità di rotazione dell'asse, però l'assetto rimane quello imposto.
Se ci fai caso tutti i quadri che volano solo col giroscopio una volta che li metti con un certo assetto poi ci rimangono anche se porti gli stick a zero, per rimetterli in piano devi dare comando contrario a differenza del modo di volo stabile, dove serve anche l'accelerometro, che riporta sempre il quadri in assetto ortogonale al terreno azzerando di fatto anche la velocità di avanzamento.


Capisco..dite che sia troppo per me pretendere di sviluppare un mio algoritmo basandomi sui dati di un giroscopio?

P.S: ma allora per ottenere il grado di inclinazione devo ricorrere per forza ad algoritmi tipo questo, specifico per quel giro?

Go Up