PPM mit MultiWii und Crius AIO Pro V2

Nachdem die Wiese in meinen ersten Copter gestürzt ist, musste ein neuer her. Hier sollte alles besser werden! So auch die Menge der Fehlerstellen. z.B. die ganzen Kabel zwischen Empfänger und Flugsteuerung. Die Übertragung der Steuerdaten per Summensignal wäre also optimal!

Als Hardware verwende ich das CRIUS All-in-One Pro V2 und den OrangeRX / OpenLRS UHF Empfänger. Dieser ist in der Lage per Konfiguration auf dem mit CH5 beschrifteten Pin ein PPM Signal auszugeben. Problem bei der ganzen Sache war nun wie sich heraustellen sollte die MultiWii Software in Kombination mit dem AIOP V2. Hier gibt es einen Pin welcher mit PPM beschritet ist.

crius_aio_ppm_pinJedoch nachdem ich SERIAL_SUM_PPM in der MultiWii Config aktiviert hatte, funktionierte die Übertragung der Daten nicht! Ich probierte auch PPM_ON_THROTTLE jedoch verhalf dies dem AIO auch nicht auf die Sprünge. Die war beim alten AIO V1/V1.1, laut diversen Beschreibungen im Netz, scheinbar noch über den Throttle-Pin möglich. Bei meinem V2 funktionierte dies jedoch nicht mehr. Ich kann aber auch nicht ausschließen, dass der THROTTLE pin nichtmehr mit dem Mikrocontroller verbunden ist, da ein paar Komponenten schon vom Board runtergebrochen sind, dank diverser Abstürze (siehe z.B. die vier Lötpads der fehlenden Kondensatoren und den losen SMD Kondensator daneben links oben im Bild). Der PPM Pin wie oben im Bild zu sehen, ist beim V2 neu!

crius_messenDa ich nicht wieder 6 Kabel zwischen Empfänger und FC haben wollte (dies war früher mal die Ursache eines Absturzes, als ein Kabel sich während des Flug löste und kein Failsafe ausgelöst wurde), begann ich mit der Fehleranalyse. Dazu wollte ich zuerst rausfinden, ob die FC technisch überhaupt noch in der Lage war, das PPM Signal zu erfassen. Ich schrieb also ein kleines Programm, welches mir Arduino Pin 4 als Eingang konfiguriert und den Zustand kontinuierlich per Serial.write()  ausgibt (Pin 4 wir im aktuellen MultiWii Code per attachInterrupt(4, rxInt, RISING); als Eingang fürs PPM verwendet). Das Programm zeigte keinen Statuswechsel, wenn ich den PPM Pin zwischen 0V und 5V hin und her schaltete! Da ich keine Schaltung des AIOP im Internet finden konnte, ermittelte ich per Durchgangsprüfung, mit welchem Pin des Atmega 2560 der PPM Pin verbunden ist.

PinMap2560sma_

Nach einiger Recherche stellt ich fest, dass beim AIOP V2 der PPM Pin nicht wie von MutltiWii angenommen mit Arduino Pin 4 sondern mit Arduino Pin 48 verbunden ist. Dieser entspricht dem Pin 36 / PL1 (ICP5) am Atmega 2560. Dieser Pin ist NICHT Interruptfähig. Ein binden einer Interrupt Service Routine per attachInterrupt(48, rxInt, RISING); ist also nicht möglich! Nach dem abändern meines Testprogramms konnte ich jedoch Statusänderungen am Pin sehen, dieses benutzt jedoch keinen Interrupt welcher für die korrekte Ermittlung eines PPM Signals nötig wäre, da wir den Pin nicht hochfrequent und vor allem gleichmäßig genug per Software abfragen können.

Der Pin kann jedoch als Input Capture Pin verwendet werden! Hierbei läuft ein Timer (in unserem Fall TIMER5 im Atmega dessen aktueller Zeitstempel gespeichert wird, nachdem ein steigende oder fallende Flanke (konfigurierbar) am PL1 Pin erkannt wird. Dies ist dafür gedacht Eingaben sehr zeitscharf zu erkennen, jedoch erst später in der Software zu verarbeiten. z.B. bei bei Tastatureingaben ist z.B. oft die Reihenfolge der Buchstaben sehr wichtig, die Latenz der Eingabe jedoch nicht. Hier reicht es also den Zeitstempel jedes Tastendruckes zu speichern und dann die Eingaben erst später zu verarbeiten. Wir können genau diese Funktionalität beim Atmega verwenden um unser PPM Signal zu empfangen! Hier ist eine zeitscharfe Abfrage sehr wichtig, da die Breite der Pulse ja die Aussteuerung des einzelnen RC Kanals angeben. Würde man hier nicht zeitscharf genug messen, würde man falsche Werte ermitteln.

Um die Funktionalität zu verwenden, muss der TIMER5 aufgesetzt werden, sowie die Timer Capture Events für den Pin PL1, dies geschieht indem wir 4 Bitmasken in verschiedene Register des ATMega schreiben:

TCCR5A =((1<<WGM50)|(1<<WGM51));
TCCR5B = ((1<<WGM53)|(1<<WGM52)|(1<<CS51)|(1<<ICES5));
OCR5A = 40000; // 0.5us per timertick
TIMSK5 |= (1<<ICIE5); // activate input capture for PL1

Glücklicherweise bietet der ATMega sogar doch einen Interrupt, wenn ein eben solches Timer Capture Event auftritt. Wie binden die ISR des MultiWii Codes zum empfangen von RC Daten an eben diesen Interrupt per: ISR(INT6_vect){rxInt();}. Nachdem die Änderungen eingepflegt sind kompilieren wir MultiWii neu und laden es hoch. Und oh Wunder PPM tut per PPM Pin auf dem AIOP V2 🙂

Ich habe einen Patch zur Aufnahme in den offiziellen MultiWii Code eingereicht, das die oben beschriebene Funktionalität automatisch aktiviert wird, wenn man in der Config.h #define CRIUS_AIO_PRO_V2 setzt. Ich hoffe er wird aufgenommen!

Posted in Uncategorized
6 comments on “PPM mit MultiWii und Crius AIO Pro V2
  1. Mick says:

    Many thanks for this post ! Works perfectly for me. This freed all the Port K for other cool stuff.
    Have good flights.

  2. Fareed says:

    Space Fish

    Thanks for your great work. Can you explain how to use your path? I am beginner with Multiwii.

    Thanks,

    Fareed

  3. Ralf says:

    Hi,

    du schreibst oben:
    >Arduino Pin 4 als Eingang konfiguriert und den Zustand kontinuierlich per Serial.write() ausgibt (Pin 4 wir im aktuellen MultiWii Code per attachInterrupt(4, rxInt, RISING); als Eingang fürs PPM verwendet).

    Mit dem Befehl wird IMHO aber nicht Arduino PIN4 (Mega2560 Pin1, PG5) sondern Interrupt 4 (Mega2560 Digital Pin 19, RX1) verwendet, d.h der PPM-Empfänger müsste doch normalerweise an die AIOP RX1 angeschlossen werden oder hab ich da was falsch verstanden? (Kanns leider gerade nicht testen, mein PPM-Empfänger von HK ist noch immer unterwegs.

    LG
    Ralf

    • Spacefish says:

      Stimmt. Könnte man natürlich mal probieren einfach PPM an RX1 zu hängen ob das dann funktioniert. Mit meinem Fix hatte ich halt den Pin der mit PPM an der Crius AIO v2 mit “PPM Sum” markiert ist als PPM bin aktiviert. In Zwischenzeit fliege ich gar nicht mehr mit der AIO sondern mit einem APM.

  4. Spacefish says:

    See this patch:

    Index: MultiWii_shared/config.h
    ===================================================================
    — MultiWii_shared/config.h (revision 1664)
    +++ MultiWii_shared/config.h (working copy)
    @@ -140,6 +140,7 @@
    //#define Bobs_10DOF_BMP_V1 // BobsQuads 10DOF V1 with ITG3200, BMA180, HMC5883L & BMP180 – BMP180 is software compatible with BMP085
    //#define FLYDUINO_MPU // MPU6050 Break Out onboard 3.3V reg
    //#define CRIUS_AIO_PRO_V1
    + //#define CRIUS_AIO_PRO_V2
    //#define DESQUARED6DOFV2GO // DEsquared V2 with ITG3200 only
    //#define DESQUARED6DOFV4 // DEsquared V4 with MPU6050
    //#define LADYBIRD
    Index: MultiWii_shared/def.h
    ===================================================================
    — MultiWii_shared/def.h (revision 1664)
    +++ MultiWii_shared/def.h (working copy)
    @@ -1388,6 +1388,32 @@
    #define SERVO_3_PIN_LOW PORTL &= ~(1<<3); #endif +#if defined(CRIUS_AIO_PRO_V2) + #define MPU6050 + #define HMC5883 + #define MS561101BA + #define ACC_ORIENTATION(X, Y, Z) {imu.accADC[ROLL] = -X; imu.accADC[PITCH] = -Y; imu.accADC[YAW] = Z;} + #define GYRO_ORIENTATION(X, Y, Z) {imu.gyroADC[ROLL] = Y; imu.gyroADC[PITCH] = -X; imu.gyroADC[YAW] = -Z;} + #define MAG_ORIENTATION(X, Y, Z) {imu.magADC[ROLL] = X; imu.magADC[PITCH] = Y; imu.magADC[YAW] = -Z;} + #define MPU6050_I2C_AUX_MASTER // MAG connected to the AUX I2C bus of MPU6050 + #undef INTERNAL_I2C_PULLUPS + #define I2C_SPEED 400000L //400kHz fast mode + //servo pins on AIO board is at pins 44,45,46, then release pins 33,34,35 for other usage + //eg. pin 33 on AIO can be used for LEDFLASHER output + #define SERVO_1_PINMODE pinMode(44,OUTPUT); // TILT_PITCH + #define SERVO_1_PIN_HIGH PORTL |= 1<<5; + #define SERVO_1_PIN_LOW PORTL &= ~(1<<5); + #define SERVO_2_PINMODE pinMode(45,OUTPUT); // TILT_ROLL + #define SERVO_2_PIN_HIGH PORTL |= 1<<4; + #define SERVO_2_PIN_LOW PORTL &= ~(1<<4); + #define SERVO_3_PINMODE pinMode(46,OUTPUT); // CAM TRIG + #define SERVO_3_PIN_HIGH PORTL |= 1<<3; + #define SERVO_3_PIN_LOW PORTL &= ~(1<<3); + #if defined(SERIAL_SUM_PPM) + #define PPM_ON_PL1 + #endif +#endif + #if defined(LADYBIRD) #define MPU6050 #define ACC_ORIENTATION(X, Y, Z) {imu.accADC[ROLL] = -X; imu.accADC[PITCH] = -Y; imu.accADC[YAW] = Z;} Index: MultiWii_shared/MultiWii.cpp =================================================================== --- MultiWii_shared/MultiWii.cpp (revision 1664) +++ MultiWii_shared/MultiWii.cpp (working copy) @@ -689,6 +689,13 @@ plog.armed_time = 0; // lifetime in seconds //plog.running = 0; // toggle on arm & disarm to monitor for clean shutdown vs. powercut #endif + #ifdef PPM_ON_PL1 + // initialize timer 5 and trigger on rising flank + TCCR5A =((1<

  5. Andre says:

    Hi,
    could you please explain, where these lines of code have to be implemented?
    I tried in rx.cpp, but that didn´t work.

    Greetz
    Andre

Leave a Reply

Your email address will not be published. Required fields are marked *

*