RP6 Sensor Board und Xtra Module: Unterschied zwischen den Versionen

Version vom 30. April 2014, 08:04 Uhr

Was bisher geschah

Siehe auch "Was bisher geschah" im RP6v2 und im RP6 Artikel!

Mit, 27. Nov 2013 - Ankündigung einer Sensorerweiterungsplatine, die an die RP6v2-M256-WIFI angeschlossen werden kann
Sam, 22. Feb 2014 - Auslieferungsbeginn des RP6 Sensor Boards, des I2C GPS Empfängers, des 3D Accelerometer Moduls und des Gyro Moduls
Mon, 17. Mär 2014 - Das RP6 Sensor Board ist "nicht lieferbar", neuer Liefertermin angekündigt für den 12.06.2014 (!?)
Die, 25. Mär 2014 - Das RP6 Sensor Board ist wieder "sofort lieferbar"

Allgemein

Dieser Artikel beschreibt die Grundlagen und Programmierung des RP6 Sensor Boards und der RP6 Xtra Module. Dieses Erweiterungs-Modul und die Xtra Module sind Teile des RP6-Systems, das in den RN-Wissen-Artikeln RP6, RP6v2 und RP6 - Programmierung ausführlich beschrieben wird. Ausgegliedert aus den "Haupt-Artikeln" habe ich diese Module, da sie m.E. zu einer neuen "Ära" des RP6-Systems gehören:

Sie wurden nicht mehr von SlyD (Dominik S. Herwald) in Zusammenarbeit mit AREXX entwickelt, sondern nun von JM3 Engeneering (Dipl.-Ing. Jürgen Maisel). Ab Februar 2014 waren dann das RP6 Sensor Board RP6-JM03-61 (1082384) und die drei RP6 Xtra Module I2C GPS Empfänger JM3-GPS (1082385), Gyro Modul JM3-GYRO (1082386) und 3D Accelerometer Modul JM3-3DA (1082387) bei CONRAD lieferbar. Einen ersten Eindruck vom Sensor Board kann man im RoboterNetz und AREXX Support Forum bekommen,- siehe unter Weblinks!

RP6 Sensor Board

Das RP6 Sensor Board gehört zu den "Erweiterungs-Modulen" des RP6-Systems. Andere "Erweiterungs-Module" werden im RP6v2 und im RP6 Artikel beschrieben!

Das RP6 Sensor Board ist das erste "Erweiterungs-Modul", das nicht über einen eigenen Microprozessor verfügt,- es ist also ein passives Modul, das von einem Microprozessor-System (z.B. RP6v2, M32, M128, M256 Wifi) angesteuert werden muss. Es ist ebenfalls das erste Modul, das nicht mehr über die rautenförmige Einbuchtung an der Vorderkante verfügt, sondern komplett rechteckig ist. Damit passt es (montiert auf den Standard-Abstandbolzen) nur auf den vorderen Platinenstapel 1 des RP6.

RP6 Xtra Module

Die RP6 Xtra Module gehören zu den "Zubehör- und Ersatzteilen" des RP6-Systems. Über die weiteren "Zubehör- und Ersatzteile" zum RP6 informiert der entsprechende Abschnitt im RP6v2 und im RP6 Artikel!

Die RP6 Xtra Module unterscheiden sich von den weiteren "Zubehör- und Ersatzteilen" dadurch, dass sie selbstständige Sensor-Platinen sind, die ideal zum RP6-System passen, aber auch mit anderen Microcontroller-Plattformen nutzbar sind.

RP6 Sensor Board

Das RP6 Sensor Board RP6-JM03-61 (1082384) wurde am 27.11.2013 von SlyD angekündigt (siehe Weblink!).

Beschreibung

(Laut Online-Katalog der Firma CONRAD!)

"Das RP6 Sensor Board erweitert die Möglichkeiten aller RP6 Roboter um wesentliche Sensorsignale für Robotik-Anwendungen wie zum Beispiel Dreharten (Yaw-Gyro), Kipp- und Neigewinkel, Kompassrichtung oder aber auch Zeit und Datum. Sie können einfach mit zusätzlichen Sharp Sensoren Abstände genau messen, z.B. für autonome Fahrrouten oder falls der Sensor nach unten gerichtet ist, kann er Treppenstufen oder ähnliches entdecken. Außerdem gibt es die Möglichkeit z.B. die Sharp Sensoren ein- bzw. auszuschalten um Strom zu sparen. Zusätzlich können bis zu 8 Servos angesteuert werden um z.B. einen Roboterarm zu bewegen, der auch mechanisch einfach zu montieren ist. Um die Möglichkeiten des RP6 Sensor Boards komplett ausnutzen zu können, wird ein RP6V2 und RP6Wifi Control Board empfohlen. Die ermöglicht den Roboter über Wifi fernzusteuern oder Sensordaten auf dem Kommandozentrum (PC) z.B Kompassdaten, Temperatur oder Kipp- und Neigewinkel anzeigen zu lassen."

Ausstattung

• Magnetometer- und Accelerometer für Kompassfunktion
• Gyro Sensor (Yaw)
• Sharp-Sensor Schnittstelle
• LED Scheinwerfer
• LED Treiber mit Smart Funktionen
• I²C Bus mit max. 400 kHz Übertragungsrate; Hot Swap I²C-Bus Buffer
• Taster mit LED
• Echtzeit-Uhr RTC
• Backupbatterie (Aufladung während des Betriebs)
• Expansion Port z.B. für GPS-I²C Bus Modul
• 8 Servo-Ausgänge mit eigenem Schaltregler
• 8 PWM Ausgänge
• 5 LED Ausgänge und 2 Digital-I/O Ausgänge.

Lieferumfang

• RP6 Sensor Board
• Montage-Kit
• Kabel
• Anleitung und CD mit Dokumentation.

Blockschaltbild

Technische Daten

Umbau-Optionen

Hier soll beschrieben werden, welche Umbau-Optionen es für das RP6 Sensor Board gibt. Im Text gibt es Verweise auf den Schaltplan des RP6 Sensor Boards (RP6_Sensor_Board_Rev2_Schema.pdf) in der Form: (SP2: Bauteil). Dies bedeutet, dass man das Bauteil auf Blatt 2 des Schaltplans finden kann. Beispielhaft sind im folgenden Text Bestell-Nummern der Firma CONRAD genannt. Natürlich kann man die Teile auch bei anderen Versendern beziehen.

USRBUS- und XBUS-Wannenstecker

Der zweite Wannenstecker für den XBUS (SP3: K14) und USRBUS (SP3: K16) kann nachbestückt werden. Man braucht dazu:

• 2 Wannenstecker 2x5-polig gerade RM 2,54mm (CONRAD 741648)

Die Wannenstecker lötet man auf die eingezeichneten Positionen.

USRBUS-Belegung:

XBUS-Belegung:

I/O-Patchfield_1

Dieses 10-polige "Patchfield_1" (SP3: K8), beschriftet mit "I/O" liegt neben dem Wannenstecker I/O und weist dieselbe Belegung auf. Möchte man auf dem Sensor Board Verbindungen über den Wannenstecker I/O zu einer angeschlossenen Microprozessor-Platine herstellen, dann kann man auf das "Patchfield_1" eine Stiftleiste löten. Man braucht dazu:

• 2-reihige Stiftleiste RM 2,54mm (z.B. 2x40-polig 741306)

Man trennt von der 2-reihigen Stiftleiste 2x5 Kontakte ab und lötet sie auf die Lötpunkte "I/O". Zuvor muss man u.U. Lötzinnreste aus den Lötpunkten mit Entlötlitze bzw. Entlötsaugpumpe entfernen.

Stecker I/O an M256 IO_PWM/T0/T1:

ADC-Patchfield_2

Dieses 10-polige "Patchfield_2" (SP3: K9), beschriftet mit "ADC" liegt neben dem Wannenstecker ADC und weist dieselbe Belegung auf. Möchte man auf dem Sensor Board Verbindungen über den Wannenstecker ADC zu einer angeschlossenen Microprozessor-Platine herstellen, dann kann man auf das "Patchfield_2" eine Stiftleiste löten. Man braucht dazu:

• 2-reihige Stiftleiste RM 2,54mm (z.B. 2x40-polig 741306)

Man trennt von der 2-reihigen Stiftleiste 2x5 Kontakte ab und lötet sie auf die Lötpunkte "ADC".

Stecker ADC an M256 ADC_IO2/CMP:

Anschluss-Optionen

Hier soll beschrieben werden, welche Anschluss-Optionen es für das RP6 Sensor Board gibt. In erster Linie geht es dabei um die Belegung der vorhandenen Stecker.

I2C-MODULE

I2C-MODULE-Belegung:

CAM_IF

CAM_IF-Belegung:

IO_EXT

IO_EXT-Belegung:

PWM_OUT

PWM_OUT-Belegung:

TP1

Am Testpunkt TP1 (SP4: TP1) kann man das Signal SQW/INT_N der RTC (DS1339) prüfen/messen.

LEDs

Rücklichter

Rücklichter-Belegung:

Display-Hintergrundbeleuchtung

Display-Hintergrundbeleuchtung-Belegung:

Freie Ausgänge

Freier Ausgang 1-Belegung:

Freier Ausgang 2-Belegung:

SHARP Sensoren

Vorn

Vorn-Belegung:

Hinten

Hinten-Belegung:

Servos

Power

SERVO_PWR-Belegung:

Stecker

SERVO_1..7-Belegung:

Roboterarm RA2-HOBBY

RP6 Xtra Module

Von den AREXX RP6 Xtra Modulen sind bei CONRAD für den RP6v2 erhältlich:

• Arexx I2C GPS Empfänger JM3-GPS (1082385)
• Arexx Gyro Modul JM3-GYRO (1082386)
• Arexx 3D Accelerometer Modul JM3-3DA (1082387)

I2C GPS Empfänger JM3-GPS

(Laut Online-Katalog der Firma CONRAD!)

"Universelles GPS-I²C-Bus Modul für RP6 Roboter. Der GPS Empfänger stellt die NMEA-0183 Datensätze GGA, RMC, GSA und VTG in den veschiedenen Registern zur Verfügung. Damit ist eine einfache Weiterverarbeitung möglich."

Beschreibung

Der I2C GPS Empfänger ist eine kleine (38 x 23 mm) Zusatzplatine (RP6 Xtra Modul), die direkt auf das RP6 Sensor Board aufgesteckt werden kann. Sie kann aber auch überall eingesetzt werden, wo eine stabilisierte 5V Versorgungsspannung und ein 5V-I2C-Bus bis 400 kHz zur Verfügung steht.

Die Platine ist bestückt mit dem "Fastrax GPS antenna module" UC530. Als Backup-Batterie dient eine wieder aufladbare Lithium-Batterie (MS621FE). Zur Kommunikation zwischen dem UC530 und dem I2C-Bus ist ein ATtiny1634 an Bord, der auch die Auswertung des NMEA Protokolls übernimmt. Die 3,3 V Versorgungsspannung wird auf der Platine durch einen Spannungsregler (LM3480IM3-3.3) erzeugt. Über den I2C-Bus können folgende Daten der NMEA-0183 Datensätze GGA, RMC, GSA und VTG auf einfache Weise gelesen werden:

• Geograf. Breite
• Geograf. Länge
• GPS Status
• HDOP
• Satellitenzahl
• Kurs
• Geschwindigkeit
• Höhe
• UTC-Zeit
• Datum
• PDOP
• VDOP
• Satellit 1..12

Der ATtiny1634 ist natürlich vorprogrammiert, kann aber über eine vorbereitete 6-polige ISP-Schnittstelle auf der Platine auch umprogrammiert werden.

Lieferumfang

• I2C GPS Empfänger
• CD mit Anleitung und Software-Beispielen.

Technische Daten

• Versorgungsspannung: 5,0 V +/- 5%
• Stromverbrauch: <= 20 mA (typ.)
• I2C-Bus Geschwindigkeit: <= 400 kHz
• GPS Antenne: eingebaut
• Backup Batterie: Lithium, im Betrieb wieder aufladbar
• Temperatur Bereich: -10 bis 65 °C
• Form Faktor: 38 x 23 mm
• Gewicht: ca. 6 g
• Empfänger Spezifikationen (UC530):
  • Empfänger: GPS L1 C/A-Code, SPS
  • Kanäle: 66/22 (Suche/Track)
  • Tracking Empfindlichkeit: -165 dBm typ.
  • Navigations Empfindlichkeit: -148 dbm typ (Kaltstart)
  • Update Rate: 1 Sek.
  • Positions Genauigkeit:
    • 3,0m (67%) typ. horizontal
    • 5,0m (67%) typ. vertikal
    • 0,02 m/s (50%) typ. Geschwindigkeit
  • Zeit bis zum ersten Fix: typ. 31 Sek.
  • Protokoll: NMEA-0183 Rev. 3.01

Anschluss-Schema

Belegung des 9-poligen Anschluss-Steckers (K1) siehe in dieser Tabelle!

Umbau-Optionen

Wenn man den ATtiny1634 selbst programmieren will, muss man Stiftleisten auf die Platine löten.

Man braucht dazu:

• 2-reihige Stiftleiste RM 2,54mm (z.B. 2x40-polig 741306)

Man trennt von der 2-reihigen Stiftleiste 2x3 Kontakte ab und lötet sie auf die Punkte "ISP6". Mit einem ISP-Programmer kann dann eine neue Firmware geflasht werden. Die aktuelle Firmware des ATtiny1634 (Version 1.2) wird als .HEX mitgeliefert, leider ohne Source-Code.

Vorsicht: Dabei kann der Prozessor im schlimmsten Fall nicht mehr ansprechbar sein oder der GPS Empfänger UC530 beschädigt werden. Man sollte diesen Umbau und die ISP-Programmierung nur machen, wenn man genau weiß, was man macht!

Gyro Modul JM3-GYRO

(Laut Online-Katalog der Firma CONRAD!)

"Das universelle Gyro-Sensor Modul für RP6, RP6 V2, RP6 ATMEGA32 und das RP6 V2 Control M256 Wifi-Kit ist ideal zur Erkennung der Blockierung des Roboters in seiner Drehbewegung, sowie der Stabilisierung der Geradeausfahrt."

Beschreibung

Das Gyro Modul ist eine kleine (19 x 17 mm) Zusatzplatine (RP6 Xtra Modul), die im RP6-System genutzt werden kann. Es gibt zwar keinen direkt passenden "Steckplatz" auf einer der Microcontroller-Plattformen (RP6v2, M32, M128, M256 Wifi), jedoch ist der Anschluß einfach: Benötigt werden +5V und GND, sowie ein freier ADC-Kanal, der bis 5V messen kann. Damit kann das Gyro Modul sehr universell eingesetzt werden. Es misst die Drehrate um die Z-Achse (Hochachse oder Yaw).

Die Platine ist bestückt mit dem analogen Gyro-Sensor LY330ALH, dessen Ausgangsspannung durch zwei OP-Amps (LM321, LMP7731MF) in Ruhe bei 2,5 V liegt. Höhere Ausgangsspannungen (bis 5 V) bedeuten eine Drehgeschwindigkeit nach LINKS, niedrigere (bis 0 V) eine Drehgeschwindigkeit nach RECHTS. Die 3,3 V Versorgungsspannung des LY330ALH wird auf der Platine durch einen Spannungsregler (LM3480IM3-3.3) erzeugt.

Lieferumfang

• Gyro Modul
• CD mit Anleitung und Software-Beispielen.

Technische Daten

• VDD: 5,0 V +/- 5%
• Vout(0): 2,5 V +/- 2% (ohne Bewegung)
• Vout: Vout(0) + 10mV / dps +/- 2%
• Messbereich: ca. 200 dps max.

Anschluss-Schema

Umbau-Optionen

Am Gyro Modul gibt es nichts umzubauen.

3D Accelerometer Modul JM3-3DA

(Laut Online-Katalog der Firma CONRAD!)

"Universeller 3-Achsen Beschleunigungssensor für RP6, RP6 V2, RP6 ATMEGA32 und das RP6 V2 Control M256 WIFI-Kit. Ideal zur Winkelmessung (Neigung-, Kipp-Winkel) und vieles mehr."

Beschreibung

Das 3D Accelerometer Modul ist eine kleine (15 x 10 mm) Zusatzplatine (RP6 Xtra Modul), die direkt auf den EEPROM-Sockel (IC5) der Zusatzplatine RP6 CONTROL M32 gesteckt und über den SPI-Bus angesteuert werden kann. Das Accelerometer Modul kann aber auch mit allen Microcontroller-Plattformen betrieben werden, die eine stabilisierte 5V-Versorgungsspannung und einen SPI- oder I2C-Bus bieten.

Die Platine ist bestückt mit dem 3D Beschleunigungs-Sensor LIS302DLH. Die 3,3 V Versorgungsspannung des LIS302DLH wird auf der Platine durch einen Spannungsregler (LM3480IM3-3.3) erzeugt.

Lieferumfang

• 3D Accelerometer Modul
• CD mit Anleitung und Software-Beispielen.

Technische Daten

• VDD: 5,0 V +/- 5%
• Messbereiche: +-2 g, +-4 g, +-8 g
• Datenausgabe: 16 Bit
• Schnittstellen: I2C, SPI

Anschluss-Schema

Umbau-Optionen

Will man das Modul auf der M32 einsetzen, muss man auf dem 3D Accelerometer Modul die Widerstände R2 und R3 (falls vorhanden!) auslöten (siehe Anleitung zum Modul, Seiten 4/5!) und einen IC-Sockel an Position IC5 und einen Kondensator an Position C19 auf der M32 einlöten, siehe hier!

Die Abbildung rechts zeigt das 3D Accelerometer Modul auf der M32.

Programmierung

In diesem Abschnitt geht es um die Programmierung der Sensoren und Aktoren auf dem RP6 Sensor Board und auf den Xtra Modulen. Die hier veröffentlichte Software baut NICHT auf die mitgelieferten C++ Software-Beispiele auf der "RP6 Robot System Xtra Modules CD-ROM" auf, sondern basiert auf Eigenentwicklungen in GCC.

RP6 Sensor Board

Das RP6 Sensor Board RP6-JM03-61 verfügt nicht über einen eigenen Microprozessor. Daher geht es hier eigentlich nicht um die Programmierung des Sensor Boards, sondern um die Programmierung der Microcontroller-Plattform, an die das Sensor Board angeschlossen ist. In der Regel wird das die RP6 M256 WiFi Zusatzplatine sein, aber auch die RP6v2 Base steuert (als I2C-Slave) in der vorgesehenen Konfiguration gewisse Funktionen des Sensor Boards an. Prinzipiell kann das Sensor Board aber auch an der RP6 CONTROL M32 (passende Library auf Anfrage beim Autor!) oder der RP6 CCPRO M128 Zusatzplatine betrieben werden.

Dokumentation

Die Dokumentation zum RP6 Sensor Board befindet sich auf der mitgelieferten "RP6 Robot System Xtra Modules CD-ROM".

Manuals

Das Handbuch kann auch hier eingesehen werden.

ACHTUNG: Das Manual enthält aktuell noch teils irreführende Beschreibungen!

Schematics

Datasheets

Demo-Programmme

Demo-Programme zum RP6 Sensor Board befinden sich auf der mitgelieferten "RP6 Robot System Xtra Modules CD-ROM".

RP6 M256 WiFi: Sensor Board Library

Die nachfolgende RP6M256_Sensor_BoardLib (GCC) dient zur Ansteuerung des Sensor Boards durch die RP6M256 WiFi.

Configuration Header

Vor dem Kompilieren der Library muss eine Anpassung des Configuration Headers an die eigenen Hardware-Voraussetzungen erfolgen. Dazu gibt es im Configuration Header eine Definition:

• USE_PD5 -> Die Sensor Board Funktion "PD5" steuert den TCA4311A Enable Pin (EN).

Im nachfolgenden Listing ist die Definition USE_PD5 NICHT aktiv (d.h. der TCA4311A Enable Pin (EN) wird vom MAX7311 I/O 1.0 Pin angesteuert!). Dies muss an die eigene Hardware angepaßt werden. Wenn "PD5" den TCA4311A Enable Pin (EN) steuern soll, muss die Zeile mit "USE_PD5" "aktiviert" werden. Dazu entfernt man "//" am Zeilenanfang:

#define USE_PD5

Bevor man Beschleunigungs-, Magnetfeldsensor und Gyroskop nutzen kann, muss man sie kalibrieren:

• Accelerometer:
  • Die X-Achse wird kalibriert, indem man den RP6 mit der rechten Seite nach unten hält und den Maximalwert in MAX_X_A einträgt. Dann neigt man ihn auf seine linke Seite und trägt den negativen Maximalwert in MIN_X_A ein.
  • Die Y-Achse wird kalibriert, indem man den RP6 mit der Vorderseite nach oben hält und den Maximalwert in MAX_Y_A einträgt. Dann kippt man die Vorderseite nach unten und trägt den negativen Maximalwert in MIN_Y_A ein.
  • Die Z-Achse wird kalibriert, indem man den RP6 normal waagerecht hält und den Maximalwert in MAX_Z_A einträgt. Dann dreht man den RP6 "auf den Kopf" und trägt den negativen Maximalwert in MIN_Z_A ein.
  • Sollten die angezeigten Pitch- und Roll-Werte nach der Kalibrierung der Achsen und genau waagerecht stehendem Sensor bzw. RP6 vom Nullwert deutlich abweichen, kann man sie mithilfe der Definitionen OFFSET_PITCH_A und OFFSET_ROLL_A "nullen". Die Einheit dieser Definitionen ist °.

HINWEIS: Die internen Berechnungen werden durch diese Offset-Korrektur
NICHT beeinflußt. Die Offset-Werte können also nicht dazu benutzt werden,
eine Achsenabweichung zwischen Sensor und RP6 zu kompensieren!

• Magnetometer:
  • Zur Kalibrierung des Magnetfeldsensors stellt man den RP6 auf eine waagerechte Fläche. Es dürfen keine Elektrogeräte, Metallteile und stromführende Leitungen in der Nähe sein, die das Magnetfeld verändern können.
  • Zunächst ist es möglich, die sogenannten "hard iron" Effekte auszugleichen. Dies sind feste Magnetfeldabweichungen, die durch das "Gerät" (in unserem Fall also durch den kompletten RP6) verursacht werden, in oder auf dem der Sensor angebracht ist. Mit den Definitionen OFFSET_X_M, OFFSET_Y_M und OFFSET_Z_M können diese Effekte ausgeglichen werden. Diese Kalibrierung wird hier aktuell (noch) nicht beschrieben, so dass man die Werte für diese Definitionen zunächst bei 0 belassen sollte.
  • Die X-Achse und Y-Achse wird kalibriert, indem man den RP6 langsam mehrfach um die eigene (Z-)Achse dreht und die Maximalwerte in MAX_X_M und MAX_Y_M einträgt. Die negativen Maximalwerte trägt man in MIN_X_M und MIN_Y_M ein.
  • Die Z-Achse wird kalibriert, indem man den RP6 auf die rechte oder linke Seite legt, ihn dann langsam mehrfach auf der Stelle (um seine Y-Achse) dreht und den Maximalwert in MAX_Z_M und den negativen Maximalwert in MIN_Z_M einträgt.
  • Mit der Definition DECLINATION_M kann man zur Verbesserung der Genauigkeit der Richtungsangaben den magnetischen Deklinationswert [°] (d.h. die magnetische Missweisung) am aktuellen Standort festlegen. Hier befindet sich ein "Deklinations-Rechner", mit dem man den Deklinationswert für den eigenen Standort ermitteln kann.
  • Wenn man den Temperaturwert des LSM303DLHC nutzen will, muss man die Definition GET_TEMP_M aktivieren (Standard!). Den Temperaturwert kann man noch eichen, indem man die Definition OFFSET_TEMP_M anpasst. Die Einheit von OFFSET_TEMP_M ist °C.
• Gyroskop:
  • In Ruhelage des RP6 wird ein ADC-Wert von ca. 511 angezeigt, wenn die Definition ADCVAL_ZERORATE_LEVEL im Configuration Header (RP6M256_Sensor_Board.h) vor der ersten Kompilierung zunächst auf 0 gesetzt wurde. Die Kalibrierung des Gyroskops ist abgeschlossen, nachdem man den in Ruhelage angezeigten ADC-Wert als ADCVAL_ZERORATE_LEVEL eingetragen hat.

HINWEISE ZUR KALIBRIERUNG:

• Das Demo-Programm ist nicht sehr gut geeignet, um die Kalibrierungen durchzuführen.
• Man sollte mit einem eigenen kurzen Kalibrierungs-Programm nur die Rohwerte der jeweiligen X-, Y- und Z-Achse z.B. zweimal pro Sekunde ausgeben.
• Die Rohwerte sind in Variablen x_axis..., y_axis..., z_axis... enthalten. Da die Rohwerte in den Funktionen normalizeLSM303DLHC_A() bzw. normalizeLSM303DLHC_M() ggf. verändert werden, müssen die Rohwerte VOR Durchlaufen dieser Funktionen ausgegeben werden.
• Ohne eine sorgfältige Kalibrierung wird die Kompass-Funktion (insbesondere die neigungskompensierte Ausgabe der Himmelsrichtung) nicht funktionieren!
• Nach dem Eintragen der Kalibrierungs-Werte in den Configuration Header (RP6M256_Sensor_Board.h) muss die Demo neu kompiliert werden. Vorher sollten die Compiler-Hilfsdateien der Sensor Board-Library (Dateiendungen .st und .o) im Programmverzeichnis gelöscht werden.
• Getestet wird das Ergebnis der Kalibrierung, indem die Ausgabewerte (Temperatur, Pitch, Roll, Heading, Neigungs-kompensiertes Heading) der Demo überprüft werden. Dazu kann man Vergleichswerte (Thermometer, Winkelmesser für Pitch, Roll und externer Kompass für Heading) heranziehen.
• Wenn die Ausgabewerte nicht den Erwartungen entsprechen, muss die Kalibrierung wiederholt werden.
• Eine Kalibrierung der Sensoren ist nur zuverlässig, solange die Position des jeweiligen Sensors und seine komplette Hardware-Umgebung (also der RP6 mit allen seinen Teilen!) nicht verändert wird. Gibt es trotzdem Veränderungen, z.B. durch weitere Aufbauten auf dem RP6, müssen diese Sensoren neu kalibriert werden.

Datei: RP6M256_Sensor_Board.h

/* ****************************************************************************
 *                           _______________________
 *                           \| RP6  ROBOT SYSTEM |/
 *                            \_-_-_-_-_-_-_-_-_-_/           >>> RP6 M256 WIFI
 * ----------------------------------------------------------------------------
 * ----------------------------- [c]2014 - Dirk -------------------------------
 * ****************************************************************************
 * File: RP6M256_Sensor_Board.h
 * Version: 1.0
 * Target: RP6 M256 WIFI - ATMEGA2560 @16.00MHz
 * Author(s): Dirk
 * ****************************************************************************
 * Description:
 * Configuration header file for new RP6 Sensor Board library.
 * The RP6 Sensor Board (CONRAD 1082384) is connected to the RP6M256 with the
 * two plugs:
 *   Sensor Board <---> RP6M256
 *   --------------------------------
 *   I/O (K17)    <---> IO_PWM/T0/T1
 *   ADC (K18)    <---> ADC_IO2/CMP
 * The RP6 Sensor Board function "PD5" (IC6: EN) is now connected to RP6M256
 * OC1B (PB6) and the RP6 Sensor Board function "ADC4" (SHARP_R) to RP6M256
 * ADC9 (PK1).
 * On the RP6 Sensor Board the functions "ADC0", "ADC1", "PWR" and "ADC4" are
 * connected to the USRBUS. The USRBUS may be wired on the RP6 mainboard like
 * this:
 *   Function : Sensor Board - USRBUS: * <---> RP6 Mainboard
 *   --------------------------------------------------------
 *   SHARP_F  : "ADC0"       - Pin 1     <---> ADC0: Pin 3
 *   GYRO     : "ADC1"       - Pin 2     <---> ADC1: Pin 3
 *   PWRON ²  : "PWR"        - Pin 3     <---> PWR
 *   SHARP_R  : "ADC4"       - Pin 5     <---> Not connected!
 * At *) It is also possible not to use the USRBUS, but to connect the RP6
 *       Sensor Board directly to the RP6 mainboard with 3 single cables.
 * At ²) See RP6 manual "4.6.8. Stromsparfunktionen"! 
 *
 * ****************************************************************************
 */

#ifndef RP6M256_SENSOR_BOARD_H
#define RP6M256_SENSOR_BOARD_H


/*****************************************************************************/
// RP6 Sensor Board hardwired components:
// - I2C I/O Port Expander (MAX7311)
// - I2C Real Time Clock (RTC DS1339)
// - I2C Temperature Sensor (LM75B)
// - I2C Servo Controller (PCA9685)
// - I2C LED Driver (TCA6507)
// - I2C 3D Magnetometer & Accelerometer (LSM303DLHC)
// - Analog Yaw-Rate Gyroscope (LY330ALH)
// - Hot Swappable 2-Wire Bus Buffer (TCA4311A)
// - Button
// - Status LED

/*****************************************************************************/
// I2C I/O Port Expander (MAX7311):
#define I2C_SENSOR_PORTEXP_ADR			0x40	// Default

// Interrupt I/O portpin definitions (RP6M256):
#define IO_SENSOR_INT1_IN				INT1_PI12 // PCINT12 PJ3  XBUS Pin 8
#define IO_SENSOR_INT1_DDR				DDRJ
#define IO_SENSOR_INT1_PIN				PINJ
#define IO_SENSOR_INT1_PORT				PORTJ
#define IO_SENSOR_INT2_IN				INT2_PI15 // PCINT15 PJ6  XBUS Pin 11
#define IO_SENSOR_INT2_DDR				DDRJ
#define IO_SENSOR_INT2_PIN				PINJ
#define IO_SENSOR_INT2_PORT				PORTJ
#define IO_SENSOR_INT3_IN				INT3_PI14 // PCINT14 PJ5  XBUS Pin 9
#define IO_SENSOR_INT3_DDR				DDRJ
#define IO_SENSOR_INT3_PIN				PINJ
#define IO_SENSOR_INT3_PORT				PORTJ
#define IO_SENSOR_INTU_IN				INTU_PI13 // PCINT13 PJ4  XBUS Pin 7
#define IO_SENSOR_INTU_DDR				DDRJ
#define IO_SENSOR_INTU_PIN				PINJ
#define IO_SENSOR_INTU_PORT				PORTJ

// I/O PORT pin numbers:
#define PIN7							7
#define PIN6							6
#define PIN5							5
#define PIN4							4
#define PIN3							3
#define PIN2							2
#define PIN1							1
#define PIN0							0

/*****************************************************************************/
// I2C Real Time Clock (RTC DS1339):
#define I2C_SENSOR_RTC_ADR				0xd0	// Default

/*****************************************************************************/
// I2C Temperature Sensor (LM75B):
#define I2C_SENSOR_TEMP_ADR				0x90	// Default

/*****************************************************************************/
// I2C Servo Controller (PCA9685):
#define I2C_SENSOR_SERVO_ADR			0x88	// Default

// Servo and PWM channels:
#define CH_SERVO_1						16		// At K6: 1  "SERVO_1"
#define CH_SERVO_2						15		// At K11: 1 "SERVO_2"
#define CH_SERVO_3						13		// At K29: 1 "SERVO_3"
#define CH_SERVO_4						14		// At K30: 1 "SERVO_4"
#define CH_SERVO_5						11		// At K31: 1 "SERVO_5"
#define CH_SERVO_6						12		// At K32: 1 "SERVO_6"
#define CH_SERVO_7						9		// At K33: 1 "SERVO_7"
#define CH_SERVO_8						10		// At K34: 1 "SERVO_8"

#define CH_PWM_OUT_1					1		// At K12: 1 "PWM_OUT"
#define CH_PWM_OUT_2					2		// At K12: 2
#define CH_PWM_OUT_3					3		// At K12: 3
#define CH_PWM_OUT_4					4		// At K12: 4
#define CH_PWM_OUT_5					5		// At K12: 5
#define CH_PWM_OUT_6					6		// At K12: 6
#define CH_PWM_OUT_7					7		// At K12: 7
#define CH_PWM_OUT_8					8		// At K12: 8

// Servo left touch (LT), right touch (RT), middle position (MP) constants:
// (Hints: - Servo impulse length [ms] = Servo position value / 204.8
//           (Formula only valid for a PWM of 50 Hz!)
//         - Min. servo impulse (0,7 ms) = Servo position 143
//         - Mid. servo impulse (1,5 ms) = Servo position 307
//         - Max. servo impulse (2,3 ms) = Servo position 471
//         - !!! You should NOT use servo position values < 143 or > 471 !!!)
#define SERVO1_LT						205		// Servo impulse ~1ms
#define SERVO1_RT						410		// Servo impulse ~2ms
#define SERVO1_MP						((SERVO1_RT - SERVO1_LT) / 2 + SERVO1_LT)
#define SERVO2_LT						205
#define SERVO2_RT						410
#define SERVO2_MP						((SERVO2_RT - SERVO2_LT) / 2 + SERVO2_LT)
#define SERVO3_LT						205
#define SERVO3_RT						410
#define SERVO3_MP						((SERVO3_RT - SERVO3_LT) / 2 + SERVO3_LT)
#define SERVO4_LT						205
#define SERVO4_RT						410
#define SERVO4_MP						((SERVO4_RT - SERVO4_LT) / 2 + SERVO4_LT)
#define SERVO5_LT						205
#define SERVO5_RT						410
#define SERVO5_MP						((SERVO5_RT - SERVO5_LT) / 2 + SERVO5_LT)
#define SERVO6_LT						205
#define SERVO6_RT						410
#define SERVO6_MP						((SERVO6_RT - SERVO6_LT) / 2 + SERVO6_LT)
#define SERVO7_LT						205
#define SERVO7_RT						410
#define SERVO7_MP						((SERVO7_RT - SERVO7_LT) / 2 + SERVO7_LT)
#define SERVO8_LT						205
#define SERVO8_RT						410
#define SERVO8_MP						((SERVO8_RT - SERVO8_LT) / 2 + SERVO8_LT)

/*****************************************************************************/
// I2C LED Driver (TCA6507):
#define I2C_SENSOR_LEDDRIVER_ADR		0x8a	// Default

// LED numbers:
#define LED0							0		// At K7
#define DISPLAY_BACKLIGHT				0		// Display background light
#define LED1							1		// At K4
#define LED2							2		// At K2
#define LED3							3		// At K10: 1
#define BACKLIGHT_R						3		// Right backlight
#define LED4							4		// At K10: 2
#define BACKLIGHT_L						4		// Left backlight
#define LED5							5		// White LED (D3)
#define HEADLIGHT_R						5		// Right headlight
#define LED6							6		// White LED (D1)
#define HEADLIGHT_L						6		// Left headlight

/*****************************************************************************/
// I2C 3D Magnetometer & Accelerometer (LSM303DLHC):

// LSM303DLHC accelerometer:
#define I2C_SENSOR_LSM303DLHC_A_ADR 	0x32	// Default

// LSM303DLHC accelerometer calibration data:
#define MAX_X_A						1037		// Max. X-axis value
#define MIN_X_A						-1030		// Min. X-axis value
#define MAX_Y_A						1030		// Max. Y-axis value
#define MIN_Y_A						-1019		// Min. Y-axis value
#define MAX_Z_A						1062		// Max. Z-axis value
#define MIN_Z_A						-1056		// Min. Z-axis value
#define OFFSET_PITCH_A				0.0			// Offset Pitch [°]
#define OFFSET_ROLL_A				0.0			// Offset Roll [°]

// LSM303DLHC magnetometer:
#define I2C_SENSOR_LSM303DLHC_M_ADR		0x3c	// Default

// LSM303DLHC magnetometer calibration data:
#define MAX_X_M						-111.0		// Max. X-axis value
#define MIN_X_M						-634.0		// Min. X-axis value
#define OFFSET_X_M					0			// Hard iron X-axis offset
#define MAX_Y_M						297.0		// Max. Y-axis value
#define MIN_Y_M						-215.0		// Min. Y-axis value
#define OFFSET_Y_M					0			// Hard iron Y-axis offset
#define MAX_Z_M						183.0		// Max. Z-axis value
#define MIN_Z_M						-214.0		// Min. Z-axis value
#define OFFSET_Z_M					0			// Hard iron Z-axis offset
#define DECLINATION_M				0.0			// E at local position [°]

// LSM303DLHC temperature sensor definitions:
#define GET_TEMP_M								// Enable temperature sensor
#define OFFSET_TEMP_M				0.0			// Temperature offset [°C]

/*****************************************************************************/
// Analog Yaw-Rate Gyroscope (LY330ALH):

// LY330ALH gyroscope:
#define ADC_SENSOR_LY330ALH				ADC_1	// At USRBUS: 2 (RP6BASE)

// LY330ALH gyroscope calibration data:
// (Higher values [505..1023] mean an angular velocity while rotating LEFT
//  and lower values [503..0] while rotating RIGHT!)
#define ADCVAL_MINRATE					0		// Min. rate value
#define ADCVAL_ZERORATE_LEVEL			504		// Zero rate level
#define ADCVAL_MAXRATE					1023	// Max. rate value

/*****************************************************************************/
// Hot Swappable 2-Wire Bus Buffer (TCA4311A):

// Define USE_PD5, if you want to control the TCA4311A enable pin (EN) with
// the RP6 Sensor Board function "PD5". If USE_PD5 is NOT defined (default),
// the TCA4311A EN pin is controlled by the MAX7311 I/O 1.0 pin.
// ---------------
//#define USE_PD5
// ---------------

#define IO_SENSOR_TCA4311A_EN_IN		OC1B_PI6 // At I/O: 9 (RP6M256)
#define IO_SENSOR_TCA4311A_EN_DDR		DDRB
#define IO_SENSOR_TCA4311A_EN_PIN		PINB
#define IO_SENSOR_TCA4311A_EN_PORT		PORTB

/*****************************************************************************/
// Button:

// (Button is connected to I/O 1.7 of the MAX7311!)

/*****************************************************************************/
// Status LED:

// (Status LED is connected to I/O 1.6 of the MAX7311!)

/*****************************************************************************/
// Other ADC channel definitions:

// (Depending on connections on the RP6 Sensor Board!)
#define ADC_SENSOR_SHARP_F				ADC_0	// At USRBUS: 1 (RP6BASE)
#define ADC_SENSOR_SHARP_R				ADC_9	// At ADC: 3    (RP6M256)

/*****************************************************************************/

#endif

/******************************************************************************
 * Additional info
 * ****************************************************************************
 * Changelog:
 * 
 *  ---> changes are documented in the file "RP6M256_Sensor_BoardLib.c"
 *
 * ****************************************************************************
 */

/*****************************************************************************/
// EOF

Library Header

Datei: RP6M256_Sensor_BoardLib.h

/* ****************************************************************************
 *                           _______________________
 *                           \| RP6  ROBOT SYSTEM |/
 *                            \_-_-_-_-_-_-_-_-_-_/           >>> RP6 M256 WIFI
 * ----------------------------------------------------------------------------
 * ----------------------------- [c]2014 - Dirk -------------------------------
 * ****************************************************************************
 * File: RP6M256_Sensor_BoardLib.h
 * Version: 1.0
 * Target: RP6 M256 WIFI - ATMEGA2560 @16.00MHz
 * Author(s): Dirk
 * ****************************************************************************
 * Description:
 * Header file for new RP6 Sensor Board library.
 *
 * ****************************************************************************
 */

#ifndef RP6M256_SENSOR_BOARDLIB_H
#define RP6M256_SENSOR_BOARDLIB_H


/*****************************************************************************/
// RP6 Sensor Board hardwired components:
// - I2C I/O Port Expander (MAX7311)
// - I2C Real Time Clock (RTC DS1339)
// - I2C Temperature Sensor (LM75B)
// - I2C Servo Controller (PCA9685)
// - I2C LED Driver (TCA6507)
// - I2C 3D Magnetometer & Accelerometer (LSM303DLHC)
// - Analog Yaw-Rate Gyroscope (LY330ALH)
// - Hot Swappable 2-Wire Bus Buffer (TCA4311A)
// - Button
// - Status LED

/*****************************************************************************/
// Includes:

#include "RP6M256Lib.h"
#include "RP6I2CmasterTWI.h"
#include "RP6M256_Sensor_Board.h"
#include <math.h>

/*****************************************************************************/
// RP6M256_I2CMasterLib definitions:

#ifndef I2C_RP6_BASE_ADR
	#define I2C_RP6_BASE_ADR			10
#endif
#ifndef I2C_REG_ADC_ADC0_L
	#define I2C_REG_ADC_ADC0_L			23
#endif
#ifndef I2C_REG_ADC_ADC1_L
	#define I2C_REG_ADC_ADC1_L			25
#endif
#ifndef CMD_POWER_OFF
	#define CMD_POWER_OFF				0
#endif
#ifndef CMD_POWER_ON
	#define CMD_POWER_ON				1
#endif

/*****************************************************************************/
// I2C I/O Port Expander (MAX7311):

// Registers:
#define MAX7311_INPUT_PORT1				0
#define MAX7311_INPUT_PORT2				1
#define MAX7311_OUTPUT_PORT1			2
#define MAX7311_OUTPUT_PORT2			3
#define MAX7311_PORT1_POL_INV			4
#define MAX7311_PORT2_POL_INV			5
#define MAX7311_PORT1_CONFIG			6
#define MAX7311_PORT2_CONFIG			7
#define MAX7311_TIMEOUT					8

// Port 1 configuration register bitmasks:
#define ST_GYRO							(1 << PIN7)	// I/O 0.7 (Output)
#define PD_GYRO							(1 << PIN6)	// I/O 0.6 (Output)
#define PWR_CAM_IF						(1 << PIN5)	// I/O 0.5 (Output)
#define EN_LEDDRIV						(1 << PIN4)	// I/O 0.4 (Output)
#define RDY_BUSBUFF						(1 << PIN3)	// I/O 0.3 (Input)
#define PWR_SHARP_R						(1 << PIN2)	// I/O 0.2 (Output)
#define PWR_SHARP_F						(1 << PIN1)	// I/O 0.1 (Output)
#define PWR_I2C							(1 << PIN0)	// I/O 0.0 (Output)

// Port 2 configuration register bitmasks:
#define IO2_I2C							(1 << PIN7)	// I/O 1.7 (Input)
#define IO1_I2C							(1 << PIN6)	// I/O 1.6 (Output)
#define DRDY_MAGACC						(1 << PIN5)	// I/O 1.5 (Input)
#define PWR_SERVO						(1 << PIN4)	// I/O 1.4 (Output)
#define OEN_SERVO						(1 << PIN3)	// I/O 1.3 (Output)
#define IO_K19_1						(1 << PIN2)	// I/O 1.2 (Input)
#define IO_K19_3						(1 << PIN1)	// I/O 1.1 (Input)
#define EN_BUSBUFF						(1 << PIN0)	// I/O 1.0 (Output)

// Port 1 & 2 register 16-bit bitmasks:
// (CONFIG: 1 = Input, 0 = Output)
#define MAX7311_CONFIG_DEFAULT			0b1010011000001000
#define MAX7311_CONFIG_USE_PD5			0b1010011100001000
// (OUTPUT: 1 = High/True, 0 = Low/False)
#define MAX7311_OUTPUT_DEFAULT			0b0100000100010000
#define MAX7311_OUTPUT_FULLPOWER		0b0001000100110111
#define MAX7311_OUTPUT_SHUTDOWN			0b0100100011000000

// Safe configuration:
// (Ensures if activated, that I/Os 0.3, 1.5 and 1.7 cannot be configured
//  as output with the function MAX7311_write_cfg(). This may protect the
//  RP6 Sensor Board from hardware damage.)
// -------------------
#define SAFE_CONFIG
// -------------------

typedef union {
 	uint8_t byte;
	struct {
		unsigned int1   :1;				// INT1  XBUS Pin 8
		unsigned int2   :1;				// INT2  XBUS Pin 11
		unsigned int3   :1;				// INT3  XBUS Pin 9
		unsigned intu   :1;				// INTU  XBUS Pin 7
		unsigned unused :4;
	};
	struct {
		unsigned magacc1     :1;		// INT1: LSM303DLHC INT1
		unsigned portexp     :1;		// INT2: MAX7311 INT_N
		unsigned magacc2_rtc :1;		// INT3: LSM303DLHC INT2
		unsigned noname      :5;		//       OR DS1339 SQW/INT_N
	};
} interrupt_RP6SBstatus_t;
extern interrupt_RP6SBstatus_t interrupt_RP6SBstatus;

typedef union {
	uint16_t word;
	struct {
		unsigned pwr_i2c     :1;		// I/O 0.0 (Output)
		unsigned pwr_sharp_f :1;		// I/O 0.1 (Output)
		unsigned pwr_sharp_r :1;		// I/O 0.2 (Output)
		unsigned rdy_busbuff :1;		// I/O 0.3 (Input)
		unsigned en_leddriv  :1;		// I/O 0.4 (Output)
		unsigned pwr_cam_if  :1;		// I/O 0.5 (Output)
		unsigned pd_gyro     :1;		// I/O 0.6 (Output)
		unsigned st_gyro     :1;		// I/O 0.7 (Output)
		unsigned en_busbuff  :1;		// I/O 1.0 (Output)
		unsigned io_k19_3    :1;		// I/O 1.1 (Input)
		unsigned io_k19_1    :1;		// I/O 1.2 (Input)
		unsigned oen_servo   :1;		// I/O 1.3 (Output)
		unsigned pwr_servo   :1;		// I/O 1.4 (Output)
		unsigned drdy_magacc :1;		// I/O 1.5 (Input)
		unsigned io1_i2c     :1;		// I/O 1.6 (Output)
		unsigned io2_i2c     :1;		// I/O 1.7 (Input)
	};
} max7311IOs_t;
extern max7311IOs_t inport;
extern max7311IOs_t outport;
extern uint8_t readyBusbuff;
extern uint8_t pressedButton;

void MAX7311_write_cfg(uint16_t);
void task_checkINTs(void);
void task_RP6SensorBoard(void);
void MAX7311_update(void);
void MAX7311_write(uint16_t);
void MAX7311_init(void);

/*****************************************************************************/
// I2C Real Time Clock (RTC DS1339):

// Registers:
#define DS1339_SECONDS					0
#define DS1339_MINUTES					1
#define DS1339_HOURS					2
#define DS1339_DAY						3
#define DS1339_DATE						4
#define DS1339_MONTH_CENTURY			5
#define DS1339_YEAR						6
#define DS1339_ALARM1_SECONDS			7
#define DS1339_ALARM1_MINUTES			8
#define DS1339_ALARM1_HOURS				9
#define DS1339_ALARM1_DAY_DATE			10
#define DS1339_ALARM2_MINUTES			11
#define DS1339_ALARM2_HOURS				12
#define DS1339_ALARM2_DAY_DATE			13
#define DS1339_CONTROL					14
#define DS1339_STATUS					15
#define DS1339_TRICKLE_CHARGER			16

// Control register bitmasks:
#define DS1339_CONTROL_DEFAULT			0		// Power on default
#define DS1339_CONTROL_A1IE				1
#define DS1339_CONTROL_A2IE				2
#define DS1339_CONTROL_INTCN			4
#define DS1339_CONTROL_RS1				8
#define DS1339_CONTROL_RS2				16
#define DS1339_CONTROL_BBSQI			32
#define DS1339_CONTROL_EOSC				128

// Trickle Charger register bitmasks:
#define DS1339_TRICKLE_CHARGER_DEFAULT	0b10101010 // One diode, 2k resistor
#define DS1339_TRICKLE_CHARGER_ROUT0	1
#define DS1339_TRICKLE_CHARGER_ROUT1	2
#define DS1339_TRICKLE_CHARGER_DS0		4
#define DS1339_TRICKLE_CHARGER_DS1		8
#define DS1339_TRICKLE_CHARGER_TCS0		16
#define DS1339_TRICKLE_CHARGER_TCS1		32
#define DS1339_TRICKLE_CHARGER_TCS2		64
#define DS1339_TRICKLE_CHARGER_TCS3		128

enum RTCWEEKDAYS {
	R_MO = 1, R_TU, R_WE, R_TH, R_FR, R_SA, R_SU
};

typedef struct {
	uint16_t         year;				// Year
	uint8_t          month;				// Month   [1..12]
	enum RTCWEEKDAYS weekday;			// Weekday [1..7 = R_MO..R_SU]
	uint8_t          day;				// Day     [1..31]
} rtcdate_t;
rtcdate_t rtc_date;

typedef struct {
	uint8_t dst;						// Daylight-saving-time (time zone)
	uint8_t hour;						// Hour    [0..23]
	uint8_t minute;						// Minute  [0..59]
	uint8_t second;						// Second  [0..59]
} rtctime_t;
rtctime_t rtc_time;

uint8_t BCD2DEC(uint8_t);
uint8_t DEC2BCD(uint8_t);
void DS1339_write_cfg(uint8_t);
void DS1339_init(void);
#define CALC_DST						// Time zone will be calculated
void DS1339_read(void);
void DS1339_write(void);

/*****************************************************************************/
// I2C Temperature Sensor (LM75B):

// Registers:
#define LM75B_TEMP						0
#define LM75B_CONF						1
#define LM75B_THYST						2
#define LM75B_TOS						3

// Conf register bitmasks:
#define LM75B_CONF_DEFAULT				0		// Power on default
#define LM75B_CONF_SHUTDOWN				1
#define LM75B_CONF_OS_COMP_INT			2
#define LM75B_CONF_OS_POL				4
#define LM75B_CONF_OS_F_QUE0			8
#define LM75B_CONF_OS_F_QUE1			16

extern double temperature;

void LM75B_write_cfg(uint8_t);
#define LM75B_shutdown() {LM75B_write_cfg(LM75B_CONF_SHUTDOWN);}
#define LM75B_init() {LM75B_write_cfg(LM75B_CONF_DEFAULT);}
extern uint8_t temperature_low;
extern uint8_t temperature_high;
#define getTemperatureHigh() (temperature_high)
#define getTemperatureLow() (temperature_low)
void LM75B_read(void);
double LM75B_calculate(void);
double LM75B_measure(void);

/*****************************************************************************/
// I2C Servo Controller (PCA9685):

// Registers:
#define PCA9685_MODE1					0
#define PCA9685_MODE2					1
#define PCA9685_SUBADR1					2
#define PCA9685_SUBADR2					3
#define PCA9685_SUBADR3					4
#define PCA9685_ALLCALLADR				5
#define PCA9685_LED0_ON_L				6
#define PCA9685_LED0_ON_H				7
#define PCA9685_LED0_OFF_L				8
#define PCA9685_LED0_OFF_H				9
#define PCA9685_LED1_ON_L				10
#define PCA9685_LED1_ON_H				11
#define PCA9685_LED1_OFF_L				12
#define PCA9685_LED1_OFF_H				13
#define PCA9685_LED2_ON_L				14
#define PCA9685_LED2_ON_H				15
#define PCA9685_LED2_OFF_L				16
#define PCA9685_LED2_OFF_H				17
#define PCA9685_LED3_ON_L				18
#define PCA9685_LED3_ON_H				19
#define PCA9685_LED3_OFF_L				20
#define PCA9685_LED3_OFF_H				21
#define PCA9685_LED4_ON_L				22
#define PCA9685_LED4_ON_H				23
#define PCA9685_LED4_OFF_L				24
#define PCA9685_LED4_OFF_H				25
#define PCA9685_LED5_ON_L				26
#define PCA9685_LED5_ON_H				27
#define PCA9685_LED5_OFF_L				28
#define PCA9685_LED5_OFF_H				29
#define PCA9685_LED6_ON_L				30
#define PCA9685_LED6_ON_H				31
#define PCA9685_LED6_OFF_L				32
#define PCA9685_LED6_OFF_H				33
#define PCA9685_LED7_ON_L				34
#define PCA9685_LED7_ON_H				35
#define PCA9685_LED7_OFF_L				36
#define PCA9685_LED7_OFF_H				37
#define PCA9685_LED8_ON_L				38
#define PCA9685_LED8_ON_H				39
#define PCA9685_LED8_OFF_L				40
#define PCA9685_LED8_OFF_H				41
#define PCA9685_LED9_ON_L				42
#define PCA9685_LED9_ON_H				43
#define PCA9685_LED9_OFF_L				44
#define PCA9685_LED9_OFF_H				45
#define PCA9685_LED10_ON_L				46
#define PCA9685_LED10_ON_H				47
#define PCA9685_LED10_OFF_L				48
#define PCA9685_LED10_OFF_H				49
#define PCA9685_LED11_ON_L				50
#define PCA9685_LED11_ON_H				51
#define PCA9685_LED11_OFF_L				52
#define PCA9685_LED11_OFF_H				53
#define PCA9685_LED12_ON_L				54
#define PCA9685_LED12_ON_H				55
#define PCA9685_LED12_OFF_L				56
#define PCA9685_LED12_OFF_H				57
#define PCA9685_LED13_ON_L				58
#define PCA9685_LED13_ON_H				59
#define PCA9685_LED13_OFF_L				60
#define PCA9685_LED13_OFF_H				61
#define PCA9685_LED14_ON_L				62
#define PCA9685_LED14_ON_H				63
#define PCA9685_LED14_OFF_L				64
#define PCA9685_LED14_OFF_H				65
#define PCA9685_LED15_ON_L				66
#define PCA9685_LED15_ON_H				67
#define PCA9685_LED15_OFF_L				68
#define PCA9685_LED15_OFF_H				69
#define PCA9685_ALL_LED_ON_L			250
#define PCA9685_ALL_LED_ON_H			251
#define PCA9685_ALL_LED_OFF_L			252
#define PCA9685_ALL_LED_OFF_H			253
#define PCA9685_PRE_SCALE				254
#define PCA9685_TESTMODE				255

// Mode1 register bitmasks:
#define PCA9685_MODE1_ALLCALL			1
#define PCA9685_MODE1_SUB3				2
#define PCA9685_MODE1_SUB2				4
#define PCA9685_MODE1_SUB1				8
#define PCA9685_MODE1_SLEEP				16
#define PCA9685_MODE1_AI				32
#define PCA9685_MODE1_EXTCLK			64
#define PCA9685_MODE1_RESTART			128

// Mode2 register bitmasks:
#define PCA9685_MODE2_OUTNE01_DEFAULT	0
#define PCA9685_MODE2_OUTDRV			4
#define PCA9685_MODE2_OCH				8
#define PCA9685_MODE2_INVRT				16

#define F_PCA9685						25000000.0	// Int. Clock: 25 MHz

void PCA9685_init(uint16_t);
#define initServo(__FREQ__) {PCA9685_init(__FREQ__);}
uint8_t servo(uint8_t);
void PCA9685_set(uint8_t, uint16_t);
#define setServo(__SERVO__,__POS__) {PCA9685_set(servo(__SERVO__),__POS__);}
void PCA9685_shutdown(void);
void PCA9685_restart(void);
void setServoPower(uint8_t);

/*****************************************************************************/
// I2C LED Driver (TCA6507):

// Registers:
#define TCA6507_SELECT0				0
#define TCA6507_SELECT1				1
#define TCA6507_SELECT2				2
#define TCA6507_FADEON_TIME			3
#define TCA6507_FULLYON_TIME		4
#define TCA6507_FADEOFF_TIME		5
#define TCA6507_1ST_FULLYOFF_TIME	6
#define TCA6507_2ND_FULLYOFF_TIME	7
#define TCA6507_MAXIMUM_INTENSITY	8
#define TCA6507_MASTER_INTENSITY	9
#define TCA6507_INITIALIZATION		10

// Output states:
#define STATE_OFF					0
#define STATE_PWM0					2
#define STATE_PWM1					3
#define STATE_ON					4
#define STATE_MINT					5
#define STATE_BLINK0				6
#define STATE_BLINK1				7

extern uint8_t select[3];

void TCA6507_write_cfg(uint8_t, uint8_t);
void TCA6507_update(void);
void TCA6507_set(uint8_t, uint8_t);
#define setLED(__LED__,__STATE__) {TCA6507_set(__LED__,__STATE__);}
void TCA6507_shutdown(void);
void TCA6507_restart(void);
void TCA6507_init(void);

/*****************************************************************************/
// I2C 3D Magnetometer & Accelerometer (LSM303DLHC):

// LSM303DLHC accelerometer internal registers:
#define CTRL_REG1_A					0x20
#define CTRL_REG2_A					0x21
#define CTRL_REG3_A					0x22
#define CTRL_REG4_A					0x23
#define CTRL_REG5_A					0x24
#define CTRL_REG6_A					0x25
#define REFERENCE_A					0x26
#define STATUS_REG_A				0x27
#define OUT_X_L_A					0x28
#define OUT_X_H_A					0x29
#define OUT_Y_L_A					0x2a
#define OUT_Y_H_A					0x2b
#define OUT_Z_L_A					0x2c
#define OUT_Z_H_A					0x2d
#define FIFO_CTRL_REG_A				0x2e
#define FIFO_SRC_REG_A				0x2f
#define INT1_CFG_A					0x30
#define INT1_SOURCE_A				0x31
#define INT1_THS_A					0x32
#define INT1_DURATION_A				0x33
#define INT2_CFG_A					0x34
#define INT2_SOURCE_A				0x35
#define INT2_THS_A					0x36
#define INT2_DURATION_A				0x37
#define CLICK_CFG_A					0x38
#define CLICK_SRC_A					0x39
#define CLICK_THS_A					0x3a
#define TIME_LIMIT_A				0x3b
#define TIME_LATENCY_A				0x3c
#define TIME_WINDOW_A				0x3d

// LSM303DLHC accelerometer register settings:
#define CTRL_REG1_A_10HZ			0b00100111	// 10Hz & all axes enable
#define CTRL_REG1_A_50HZ			0b01000111	// 50Hz & all axes enable
#define CTRL_REG4_A_DEFAULT			0b00000000	// Default (Normal mode)
#define CTRL_REG4_A_HR				0b00001000	// HR output mode

// LSM303DLHC magnetometer internal registers:
#define CRA_REG_M					0x00
#define CRB_REG_M					0x01
#define MR_REG_M					0x02
#define OUT_X_H_M					0x03
#define OUT_X_L_M					0x04
#define OUT_Z_H_M					0x05
#define OUT_Z_L_M					0x06
#define OUT_Y_H_M					0x07
#define OUT_Y_L_M					0x08
#define SR_REG_MG					0x09
#define IRA_REG_M					0x0a
#define IRB_REG_M					0x0b
#define IRC_REG_M					0x0c
#define TEMP_OUT_H_M				0x31
#define TEMP_OUT_L_M				0x32

// LSM303DLHC magnetometer register settings:
#define CRA_REG_M_15HZ				0b00010000	// Default (Data rate 15Hz)
#define CRA_REG_M_15HZ_T_EN			0b10010000	// 15Hz & temperature enable
#define CRB_REG_M_13GAUSS			0b00100000	// Gain +-1.3Gauss
#define MR_REG_M_CCM				0b00000000	// Continuous-conversion mode
#define MR_REG_M_SCM				0b00000001	// Single-conversion mode
#define MR_REG_M_SLEEP				0b00000011	// Default (Sleep-mode)

extern int16_t x_axisa, y_axisa, z_axisa;
extern double pitch, roll;
extern double xa, ya, za;
extern double the, phi;

extern int16_t x_axism, y_axism, z_axism;
extern int16_t headingm;
extern double xm, ym, zm;
#ifdef GET_TEMP_M
extern int16_t temperaturem;
extern double temperature_mag;
#endif

extern int16_t headingtc;

void LSM303DLHC_A_init(void);
void readLSM303DLHC_A(void);
void normalizeLSM303DLHC_A(void);
void positionLSM303DLHC_A(void);

void LSM303DLHC_M_init(void);
void readLSM303DLHC_M(void);
void normalizeLSM303DLHC_M(void);
int16_t headingLSM303DLHC_M(void);

int16_t headingLSM303DLHC_TC(void);

void LSM303DLHC_init(void);

/*****************************************************************************/
// Analog Yaw-Rate Gyroscope (LY330ALH):

#define LY330ALH_MODE_NORMAL		0			// Power on default
#define LY330ALH_MODE_SLEEP			1
#define LY330ALH_MODE_SELFTEST		2
#define LY330ALH_MODE_POWERDOWN		3			// Shutdown mode

extern int16_t z_axisg;

void LY330ALH_set_mode(uint8_t);
uint16_t getRP6Base_ADC1(void);
int16_t LY330ALH_measure(void);

/*****************************************************************************/
// Hot Swappable 2-Wire Bus Buffer (TCA4311A):

//extern uint8_t readyBusbuff;			// See "I2C I/O Port Expander" section
extern uint8_t errorBusbuff;

#define getReadyBusbuff() (readyBusbuff)
uint8_t readReadyBusbuff(void);
void TCA4311A_shutdown(void);
void TCA4311A_reconnect(void);
void setRP6Base_PWR(uint8_t);
void setI2CModulePower(uint8_t);

/*****************************************************************************/
// Button:

//extern uint8_t pressedButton;			// See "I2C I/O Port Expander" section

#define getPressedButton() (pressedButton)
uint8_t readButton(void);

/*****************************************************************************/
// Status LED:

void setStatusLED(uint8_t);

/*****************************************************************************/
// RP6 Sensor Board initialisation and shutdown:

void sensor_board_init(void);
void sensor_board_shutdown(void);

/*****************************************************************************/

#endif

/******************************************************************************
 * Additional info
 * ****************************************************************************
 * Changelog:
 * 
 *  ---> changes are documented in the file "RP6M256_Sensor_BoardLib.c"
 *
 * ****************************************************************************
 */

/*****************************************************************************/
// EOF

Library Source

Datei: RP6M256_Sensor_BoardLib.c

/* ****************************************************************************
 *                           _______________________
 *                           \| RP6  ROBOT SYSTEM |/
 *                            \_-_-_-_-_-_-_-_-_-_/           >>> RP6 M256 WIFI
 * ----------------------------------------------------------------------------
 * ----------------------------- [c]2014 - Dirk -------------------------------
 * ****************************************************************************
 * File: RP6M256_Sensor_BoardLib.c
 * Version: 1.0
 * Target: RP6 M256 WIFI - ATMEGA2560 @16.00MHz
 *         and a RP6 Sensor Board (CONRAD 1082384)
 * Author(s): Dirk
 * ****************************************************************************
 * Description:
 * This is our new Library that contains basic routines and functions for
 * accessing the hardwired components of the RP6 Sensor Board (CONRAD 1082384).
 * This library assumes the RP6 Sensor Board Revision 2.0 (04.07.13).
 *
 * There are more sensors and actors, that may be connected to the RP6 Sensor
 * Board: Many I2C components including a GPS module (CONRAD 1082385), servo
 *        motors, SHARP IR distance sensors, LEDs, a camera ...
 * This library doesn't contain functions for these sensors/actors, because
 * they are not HARDWIRED to the RP6 Sensor Board and may be connected
 * OPTIONALLY.
 *
 * ****************************************************************************
 */

/*****************************************************************************/
// RP6 Sensor Board hardwired components:
// - I2C I/O Port Expander (MAX7311)
// - I2C Real Time Clock (RTC DS1339)
// - I2C Temperature Sensor (LM75B)
// - I2C Servo Controller (PCA9685)
// - I2C LED Driver (TCA6507)
// - I2C 3D Magnetometer & Accelerometer (LSM303DLHC)
// - Analog Yaw-Rate Gyroscope (LY330ALH)
// - Hot Swappable 2-Wire Bus Buffer (TCA4311A)
// - Button
// - Status LED

/*****************************************************************************/
// Includes:

#include "RP6M256_Sensor_BoardLib.h" 		

/*****************************************************************************/
// Variables:

uint8_t registerBuf[13]; 

/*****************************************************************************/
// I2C I/O Port Expander (MAX7311):

/**
 * Sends the configuration word to a MAX7311.
 *
 * Input: config -> Config word for control
 *                  registers
 *
 */
void MAX7311_write_cfg(uint16_t config)
{
#ifdef SAFE_CONFIG
	config |= 0b1010000000001000;
#endif
	I2CTWI_transmit3Bytes(I2C_SENSOR_PORTEXP_ADR, MAX7311_PORT1_CONFIG, config, (config>>8));
}

interrupt_RP6SBstatus_t interrupt_RP6SBstatus;

/** 
 * This function checks all interrupt pins on
 * the XBUS (INT1..3, INTU).
 * This function must be called VERY frequently
 * out of the main loop.
 * Bigger delays result in slower reaction to
 * interrupt requests of the RP6 Sensor Board.
 * Interrupt function table:
 *   Int. : Function 1      IRQ  Function 2       IRQ
 *   -------------------------------------------------
 *   INT1 : LSM303DLHC INT1  H
 *   INT2 : MAX7311   INT_N  L
 *   INT3 : LSM303DLHC INT2  H   DS1339 SQW/INT_N  L
 *   INTU : not used
 * You can see in the "IRQ" column, which level
 * (H/L) means an ACTIVE INTERRUPT signal.
 * The interrupt pin levels (H=true/L=false) are
 * read into the global interrupt_RP6SBstatus
 * variable.
 *
 * Hints: - This function checks the interrupt
 *          signals by polling. This is NOT the
 *          best way to to this job. You will
 *          have to write your own task, if you
 *          want to use the Pin Change Interrupt
 *          function of the ATmega2560
 *          microcontroller.
 *        - You cannot use both interrupt sources
 *          for INT3! So you should not use INT2
 *          of the LSM303DLHC together with the
 *          alarm function of the DS1339 (RTC)!
 *        - Every interrupt signal of the Sensor
 *          Board may be cut off from the XBUS
 *          by opening a jumper (Jx):
 *          INT1 -> J3 (LSM303DLHC INT1)
 *          INT2 -> J4 (MAX7311 INT_N)
 *          INT3 -> J2 (DS1339 SQW/INT_N)
 *          INT3 -> J5 (LSM303DLHC INT2)
 *
 * Example:
 *
 *   if (!interrupt_RP6SBstatus.portexp)
 *        writeString_P("==> MAX7311 INT_N!!!");
 *
 */
void task_checkINTs(void)
{
	if(IO_SENSOR_INT1_PIN & IO_SENSOR_INT1_IN)	// XBUS INT1 -> PJ3 (PCINT12)
		interrupt_RP6SBstatus.int1 = true;
	else
		interrupt_RP6SBstatus.int1 = false;
	if(IO_SENSOR_INT2_PIN & IO_SENSOR_INT2_IN)	// XBUS INT2 -> PJ6 (PCINT15)
		interrupt_RP6SBstatus.int2 = true;
	else
		interrupt_RP6SBstatus.int2 = false;
	if(IO_SENSOR_INT3_PIN & IO_SENSOR_INT3_IN)	// XBUS INT3 -> PJ5 (PCINT14)
		interrupt_RP6SBstatus.int3 = true;
	else
		interrupt_RP6SBstatus.int3 = false;
//	if(IO_SENSOR_INTU_PIN & IO_SENSOR_INTU_IN)	// XBUS INTU -> PJ4 (PCINT13)
//		interrupt_RP6SBstatus.intu = true;
//	else
//		interrupt_RP6SBstatus.intu = false;
}

max7311IOs_t inport;
uint8_t readyBusbuff;
uint8_t pressedButton;

/**
 * This function checks all I/O ports (I/O 0.0..1.7)
 * of the MAX7311 sequentially!
 * You must call this function frequently out of
 * the main loop.
 * The I/O port values (true/false) are read into
 * the global inport variable.
 * The global variables readyBusbuff (2-Wire Bus
 * Buffer: Enable) and pressedButton (Button S1:
 * Pressed) contain actual values (true/false)
 * after execution of this task.
 *
 * Examples:
 *
 *   if (inport.rdy_busbuff)
 *        writeString_P("I/O 0.3: High");
 *   else writeString_P("I/O 0.3: Low");
 *   if (!inport.drdy_magacc)
 *        writeString_P("I/O 1.5: Low");
 *   if (pressedButton)
 *        writeString_P("Button pressed!");
 *
 */
void task_RP6SensorBoard(void)
{
	I2CTWI_transmitByte(I2C_SENSOR_PORTEXP_ADR, MAX7311_INPUT_PORT1);
	I2CTWI_readBytes(I2C_SENSOR_PORTEXP_ADR, registerBuf, 2);
	inport.word = (registerBuf[1] << 8) + registerBuf[0];
	readyBusbuff = inport.rdy_busbuff;
	pressedButton = inport.io2_i2c;
	pressedButton = !pressedButton;
}

max7311IOs_t outport;

/**
 * Updates the I/O ports with current value from
 * the global outport variable!
 * If one or more port pins were configured as
 * output before, they will be set (high) or
 * cleared (low).
 *
 * Example:
 *
 *   outport.word = 0b0000000000001001;
 *   MAX7311_update();
 *   // This clears all I/O ports and sets
 *   // the ports I/O 0.0 and I/O 0.3!
 *
 *   // Other possibility:
 *   outport.pwr_sharp_f = true;
 *   MAX7311_update();
 *   // This sets I/O 0.1 and does not
 *   // affect any other I/O port!
 *
 */
void MAX7311_update(void)
{
	I2CTWI_transmit3Bytes(I2C_SENSOR_PORTEXP_ADR, MAX7311_OUTPUT_PORT1, outport.word, (outport.word>>8));
}

/**
 * Writes the 16-bit data to a MAX7311.
 * The data are output with those I/O ports that
 * were configured as output before.
 *
 * Input: data -> Data word
 *
 */
void MAX7311_write(uint16_t data)
{
	outport.word = data;
	MAX7311_update();
}

/**
 * Initializes the I/O Port Expander after power
 * on.
 *
 * Hint: If USE_PD5 is defined in the
 *       configuration header file, this
 *       function uses the RP6M256 portpin,
 *       that is connected to plug I/O: Pin 9
 *       instead of MAX7311 I/O 1.0!
 *
 */
void MAX7311_init(void)
{
#ifdef USE_PD5
	MAX7311_write_cfg(MAX7311_CONFIG_USE_PD5);
	IO_SENSOR_TCA4311A_EN_DDR |= IO_SENSOR_TCA4311A_EN_IN;		// Output
	IO_SENSOR_TCA4311A_EN_PORT |= IO_SENSOR_TCA4311A_EN_IN;		// High
#else
	MAX7311_write_cfg(MAX7311_CONFIG_DEFAULT);
#endif
	MAX7311_write(MAX7311_OUTPUT_DEFAULT);
}

/*****************************************************************************/
// I2C Real Time Clock (RTC DS1339):

/**
 * This function converts a BCD to a DEC value.
 *
 */
uint8_t BCD2DEC(uint8_t bcd)
{
	return ((bcd >> 4) * 10 + (bcd & 0x0f));
}

/**
 * This function converts a DEC to a BCD value.
 *
 */
uint8_t DEC2BCD(uint8_t dec)
{uint8_t units = dec % 10;
	if (dec /= 10) {
		return (units + (DEC2BCD(dec) << 4));
	}
	else {
		return units;
	}
}

/**
 * Sends the configuration byte to a DS1339.
 *
 * Input: Config byte for control register
 *
 */
void DS1339_write_cfg(uint8_t config)
{
	I2CTWI_transmit2Bytes(I2C_SENSOR_RTC_ADR, DS1339_CONTROL, config);
}

/**
 * Initializes the DS1339 Trickle Charger
 * function to the default for the RP6 Sensor
 * Board: One diode and 2kOhm resistor.
 *
 */
void DS1339_init(void)
{
	I2CTWI_transmit2Bytes(I2C_SENSOR_RTC_ADR, DS1339_TRICKLE_CHARGER, DS1339_TRICKLE_CHARGER_DEFAULT);
}

/**
 * Reads all data registers of the Real Time Clock (RTC).
 * They are stored in the time & date variables defined in
 * the library header.
 *
 */
void DS1339_read(void)
{
	I2CTWI_transmitByte(I2C_SENSOR_RTC_ADR, DS1339_SECONDS);
	I2CTWI_readBytes(I2C_SENSOR_RTC_ADR, registerBuf, 7);
	rtc_time.second = BCD2DEC(registerBuf[0]);
	rtc_time.minute = BCD2DEC(registerBuf[1]);
	rtc_time.hour = BCD2DEC(registerBuf[2] & 0x3f);
	rtc_date.weekday = (registerBuf[3] & 0x07);
	rtc_date.day = BCD2DEC(registerBuf[4]);
	rtc_date.month = BCD2DEC(registerBuf[5] & 0x7f);
	if(registerBuf[5] & 0x80) rtc_date.year = 100;
	else rtc_date.year = 0;
	rtc_date.year += BCD2DEC(registerBuf[6]);
	rtc_date.year += 2000;
	rtc_time.dst = 0;
#ifdef CALC_DST
	// Calculate MESZ (DST):
	uint8_t wday = rtc_date.weekday;			// Weekday [1..7 = R_MO..R_SU]
	if(wday == 7) wday = 0;
	if(rtc_date.month < 3 || rtc_date.month > 10) {
		return;
	}
	if((rtc_date.day - wday >= 25)
	 && (wday || rtc_time.hour >= 2)) {
		if(rtc_date.month == 10)
			return;
	}
	else {
		if(rtc_date.month == 3) {
			return;
		}
	}
	rtc_time.dst = 1;
#endif
}

/**
 * Writes the time & date infos in the variables defined in
 * the library header to the Real Time Clock (RTC).
 *
 */
void DS1339_write(void)
{
	registerBuf[0] = DS1339_SECONDS;
	registerBuf[1] = DEC2BCD(rtc_time.second);
	registerBuf[2] = DEC2BCD(rtc_time.minute);
	registerBuf[3] = DEC2BCD(rtc_time.hour);
	registerBuf[4] = rtc_date.weekday;
	registerBuf[5] = DEC2BCD(rtc_date.day);
	registerBuf[6] = DEC2BCD(rtc_date.month);
	registerBuf[7] = DEC2BCD(rtc_date.year - 2000);
	I2CTWI_transmitBytes(I2C_SENSOR_RTC_ADR, &registerBuf[0], 8);
}

/*****************************************************************************/
// I2C Temperature Sensor (LM75B):

/**
 * Sends the configuration byte to a LM75B.
 *
 * Input: Config byte for Conf register
 *
 */
void LM75B_write_cfg(uint8_t config)
{
	I2CTWI_transmit2Bytes(I2C_SENSOR_TEMP_ADR, LM75B_CONF, config);
}

uint8_t temperature_low;
uint8_t temperature_high;

/**
 * Reads the two data registers of the temperature
 * sensor's Temp register.
 * They are stored in the variables temperature_low
 * and _high.
 *
 */
void LM75B_read(void)
{
	I2CTWI_transmitByte(I2C_SENSOR_TEMP_ADR, LM75B_TEMP);
	I2CTWI_readBytes(I2C_SENSOR_TEMP_ADR, registerBuf, 2);
	temperature_high = registerBuf[0];
	temperature_low = registerBuf[1];
}

double temperature;						// Temperature [°C]

/**
 * Calculates and returns the temperature value
 * by using the data read from the LM75B with
 * the function LM75B_read(). The sensor is
 * designed for 11 bit measurement and 0.125°C
 * resolution.
 *
 */
double LM75B_calculate(void)
{
	int16_t tempraw = 0;
	double temp;

	tempraw = (getTemperatureHigh() << 8) | getTemperatureLow();
	tempraw >>= 5;
	if (tempraw & 1024)							// Calculate temperature
		tempraw = (tempraw & 1023) - 1024;
	else
		tempraw = tempraw & 1023;
	temp = tempraw * 0.125;
	return temp;
}

/**
 * Performs a 11 bit measurement and returns the
 * temperature [°C]. The sensor has a measuring
 * cycle of 100ms. So it makes no sense calling
 * this function at a frequency above 10Hz.
 *
 */
double LM75B_measure(void)
{
//	LM75B_init();
	LM75B_read();								// Read data
	return (LM75B_calculate());					// Calculate value
}

/*****************************************************************************/
// I2C Servo Controller (PCA9685):

/**
 * Call this once before using the servo function.
 *
 * Input: PWM frequency [40..1000 Hz]
 *
 * Hints: - Default servo frequency is 50 Hz!
 *        - The servo power is NOT switched on by
 *          this function!
 *
 * There is also a macro initServo(freq), which
 * does exactly the same as this function.
 *
 * Example:
 *   initServo(50);
 *
 */
void PCA9685_init(uint16_t freq)
{
	outport.oen_servo = false;					// I/O 1.3: Low
	MAX7311_update();
	if ((freq < 40) || (freq > 1000)) freq = 50;
	I2CTWI_transmitByte(I2C_SENSOR_SERVO_ADR, PCA9685_MODE2);
	uint8_t last_mode = I2CTWI_readByte(I2C_SENSOR_SERVO_ADR);
	last_mode &= ~PCA9685_MODE2_INVRT;			// Clear INVRT bit
	last_mode |= PCA9685_MODE2_OUTDRV;			// Set OUTDRV bit
	I2CTWI_transmit2Bytes(I2C_SENSOR_SERVO_ADR, PCA9685_MODE2, last_mode);
	I2CTWI_transmitByte(I2C_SENSOR_SERVO_ADR, PCA9685_MODE1);
	last_mode = I2CTWI_readByte(I2C_SENSOR_SERVO_ADR);
	last_mode |= PCA9685_MODE1_AI;				// Set AI bit
	uint8_t mode1 = last_mode;
	mode1 |= PCA9685_MODE1_SLEEP;				// Set SLEEP bit
	I2CTWI_transmit2Bytes(I2C_SENSOR_SERVO_ADR, PCA9685_MODE1, mode1);
	uint8_t prescale = (uint8_t) (F_PCA9685 / 4096 / freq - 0.5);
	I2CTWI_transmit2Bytes(I2C_SENSOR_SERVO_ADR, PCA9685_PRE_SCALE, prescale);
	last_mode &= ~PCA9685_MODE1_SLEEP;			// Clear SLEEP bit
	I2CTWI_transmit2Bytes(I2C_SENSOR_SERVO_ADR, PCA9685_MODE1, last_mode);
	mSleep(1);
	last_mode |= PCA9685_MODE1_RESTART;			// Clear RESTART bit
	I2CTWI_transmit2Bytes(I2C_SENSOR_SERVO_ADR, PCA9685_MODE1, last_mode);
	I2CTWI_transmit2Bytes(I2C_SENSOR_SERVO_ADR, PCA9685_LED8_ON_H, 0x10);
	return;
}

/** 
 * Returns the channel belonging to a servo number.
 *
 * Input: number -> Servo number [1..8]
 *
 */
uint8_t servo(uint8_t number)
{
	uint8_t channel;

	switch (number) {
		case 1 :
			channel = CH_SERVO_1; break;
		case 2 :
			channel = CH_SERVO_2; break;
		case 3 :
			channel = CH_SERVO_3; break;
		case 4 :
			channel = CH_SERVO_4; break;
		case 5 :
			channel = CH_SERVO_5; break;
		case 6 :
			channel = CH_SERVO_6; break;
		case 7 :
			channel = CH_SERVO_7; break;
		case 8 :
			channel = CH_SERVO_8; break;
		default : channel = CH_SERVO_1;
	}
	return channel;
}

/**
 * This is the servo position set function.
 *
 * Input: channel -> Channel number [1..16]
 *        pos     -> Servo position [SERVOx_LT..SERVOx_RT]
 *
 * Hints: - A servo position of 205 means 1 ms servo impulse,
 *          a position of 410 means a 2 ms servo impulse!
 *          You may calculate the servo impulse length by:
 *          ==> Impulse [ms] = servo position / 204.8 <==
 *          (Formula only valid for a PWM of 50 Hz!)
 *
 * There is also a macro setServo(servo, pos), which
 * does nearly the same as this function, but the first
 * parameter is the servo and NOT the channel number.
 *
 * Example:
 *   setServo(2,300);
 *
 */
void PCA9685_set(uint8_t channel, uint16_t pos)
{
	if ((channel == 0) || (channel > 16))
		return;
	uint8_t reg = channel * 4 + 4;				// Register LEDx_OFF_L
	I2CTWI_transmit3Bytes(I2C_SENSOR_SERVO_ADR, reg, (pos & 0x00ff), (pos >> 8));
}

/**
 * If the servos are not moving for a while, the
 * servo function can be stopped with this
 * function (PCA9685 set to sleep mode).
 *
 * Hint: The servo power is NOT switched off by
 *       this function!
 *
 */
void PCA9685_shutdown(void)
{
	I2CTWI_transmit2Bytes(I2C_SENSOR_SERVO_ADR, PCA9685_ALL_LED_OFF_H, 0x10);
	I2CTWI_transmitByte(I2C_SENSOR_SERVO_ADR, PCA9685_MODE1);
	uint8_t mode1 = I2CTWI_readByte(I2C_SENSOR_SERVO_ADR);
	mode1 |= PCA9685_MODE1_SLEEP;				// Set SLEEP bit
	I2CTWI_transmit2Bytes(I2C_SENSOR_SERVO_ADR, PCA9685_MODE1, mode1);
}

/**
 * If the servo function was stopped with the
 * function PCA9685_shutdown() before, it can be
 * (re)started again with this function.
 *
 * Hint: The servo power is NOT switched on by
 *       this function!
 *
 */
void PCA9685_restart(void)
{
	I2CTWI_transmitByte(I2C_SENSOR_SERVO_ADR, PCA9685_MODE1);
	uint8_t mode1 = I2CTWI_readByte(I2C_SENSOR_SERVO_ADR);
	if (mode1 & PCA9685_MODE1_RESTART) {		// RESTART bit set?
		mode1 &= ~PCA9685_MODE1_SLEEP;			// Clear SLEEP bit
		I2CTWI_transmit2Bytes(I2C_SENSOR_SERVO_ADR, PCA9685_MODE1, mode1);
		mSleep(1);
		mode1 |= PCA9685_MODE1_RESTART;			// Clear RESTART bit
		I2CTWI_transmit2Bytes(I2C_SENSOR_SERVO_ADR, PCA9685_MODE1, mode1);
		I2CTWI_transmit2Bytes(I2C_SENSOR_SERVO_ADR, PCA9685_ALL_LED_OFF_H, 0);
	}
}

/** 
 * With this function you can switch the servo
 * power on or off.
 *
 * Input: pwr -> 0 (false) = servo power off
 *               >0 (true) = servo power on
 *
 * Hints: - If connected servos are not used, you
 *          should always switch the servo power off
 *          (default) to save energy!
 *        - The PCA9685 is NOT restarted or put into
 *          shutdown mode by this function!
 *
 */
void setServoPower(uint8_t pwr)
{
	if(pwr > 0)
		outport.pwr_servo = true;				// I/O 1.4: High
	else
		outport.pwr_servo = false;				// I/O 1.4: Low
	MAX7311_update();
}	

/*****************************************************************************/
// I2C LED Driver (TCA6507):

/**
 * Sends the configuration byte to TCA6507's
 * internal registers.
 *
 * Input: reg    -> Register address [0..10]
 *        config -> Config byte for internal
 *                  registers
 *
 * Hints: - With this function you can write to
 *          all internal registers of the LED
 *          driver, see "Registers:" in the
 *          library header file!
 *        - Mainly you will use this function
 *          for setting time (regs 3..7) and
 *          intensity (regs 8, 9) values.
 *
 * Example:
 *   TCA6507_write_cfg(TCA6507_MAXIMUM_INTENSITY,0x77);
 *
 */
void TCA6507_write_cfg(uint8_t reg, uint8_t config)
{
	if(reg > 10) reg = 0;
	I2CTWI_transmit2Bytes(I2C_SENSOR_LEDDRIVER_ADR, reg, config);
}

uint8_t select[3];

/**
 * Updates the LED driver select registers (0..2)
 * with the global select array!
 *
 * Hint: - Also have a look at Table 4 in the
 *         TCA6507 datasheet!
 *
 */
void TCA6507_update(void)
{
	I2CTWI_transmit4Bytes(I2C_SENSOR_LEDDRIVER_ADR, (TCA6507_SELECT0 | 0x10), select[0], select[1], select[2]);
}

/**
 * This is the LED state set function.
 *
 * Input: led   -> LED number [0..6]
 *        state -> LED state  [0..7]
 *
 * Hints: - Possible driver states [0..7] see
 *          "Output states:" in the library
 *          header file!
 *        - Also have a look at Table 4 in the
 *          TCA6507 datasheet!
 *
 * There is also a macro setLED(led, state), which
 * does exactly the same as this function.
 *
 * Example:
 *   setLED(1,4);
 *   // Switches LED1 ON
 *   // ... OR ...
 *   setLED(LED1,STATE_ON);
 *   // does the same.
 *
 */
void TCA6507_set(uint8_t led, uint8_t state)
{
	uint8_t i, mask;

	if(led > 6) led = 0;
	led = 1 << led;
	mask = 1;
	for(i = 0; i < 3; i++) {
		select[i] &= ~led;
		if(state & mask)
			select[i] |= led;
		mask <<= 1;
	}
	TCA6507_update();
}

/**
 * If the LEDs are not needed for a while, the
 * LED driver can be stopped with this function
 * (TCA6507 set to shutdown mode).
 *
 */
void TCA6507_shutdown(void)
{
	outport.en_leddriv = false;					// I/O 0.4: Low
	MAX7311_update();
}

/**
 * If the LED driver was stopped with the
 * function TCA6507_shutdown() before, it can be
 * (re)started again with this function.
 *
 */
void TCA6507_restart(void)
{
	outport.en_leddriv = true;					// I/O 0.4: High
	MAX7311_update();
}

/**
 * Initializes the LED driver after power on by
 * switching all LEDs off.
 *
 */
void TCA6507_init(void)
{
	select[0] = 0;
	select[1] = 0;
	select[2] = 0;
	TCA6507_update();
}

/*****************************************************************************/
// I2C 3D Magnetometer & Accelerometer (LSM303DLHC):

// --------------------------------------------
// LSM303DLHC accelerometer functions:
int16_t x_axisa, y_axisa, z_axisa;
double pitch, roll;
double xa, ya, za;
double the, phi;

/**
 * Call this once before using the 3D-Accelerometer.
 *
 */
void LSM303DLHC_A_init(void)
{
	// Set normal power mode, 10 Hz data rate, all axes enabled:
	I2CTWI_transmit2Bytes(I2C_SENSOR_LSM303DLHC_A_ADR, CTRL_REG1_A, CTRL_REG1_A_10HZ);
	mSleep(10);
	// Set HD mode:
	I2CTWI_transmit2Bytes(I2C_SENSOR_LSM303DLHC_A_ADR, CTRL_REG4_A, CTRL_REG4_A_HR);
	mSleep(10);
}

/**
 * This function reads the X-axis, Y-axis and Z-axis values
 * from the LSM303DLHC accelerometer and stores them in the
 * global variables x_axisa, y_axisa and z_axisa.
 *
 */
void readLSM303DLHC_A(void)
{
	uint8_t readBuf[6];

	I2CTWI_transmitByte(I2C_SENSOR_LSM303DLHC_A_ADR, (OUT_X_L_A | 0x80));
	I2CTWI_readBytes(I2C_SENSOR_LSM303DLHC_A_ADR, readBuf, 6); // Read  X-/Y-/Z-axis
	// xb = y:
	x_axisa = ((readBuf[OUT_Y_H_A - OUT_X_L_A] << 8) + readBuf[OUT_Y_L_A - OUT_X_L_A]) / 16;
	// yb = x:
	y_axisa = ((readBuf[OUT_X_H_A - OUT_X_L_A] << 8) + readBuf[OUT_X_L_A - OUT_X_L_A]) / 16;
	// zb = z:
	z_axisa = ((readBuf[OUT_Z_H_A - OUT_X_L_A] << 8) + readBuf[OUT_Z_L_A - OUT_X_L_A]) / 16;
	mSleep(10);
}

/**
 * This function limits and normalizes the x-axisa,
 * y-axisa and z-axisa values read from the LSM303DLHC
 * accelerometer with the function readLSM303DLHC_A() and
 * stores them in the global double variables xa, ya and
 * za.
 * It also calculates the position in space in form of the
 * tilt angles (the = pitch, phi = roll) and stores them
 * in the global double variables the and phi.
 *
 */
void normalizeLSM303DLHC_A(void)
{
	// Limit raw values:
	if (x_axisa < MIN_X_A) x_axisa = MIN_X_A;
	if (x_axisa > MAX_X_A) x_axisa = MAX_X_A;
	if (y_axisa < MIN_Y_A) y_axisa = MIN_Y_A;
	if (y_axisa > MAX_Y_A) y_axisa = MAX_Y_A;
	if (z_axisa < MIN_Z_A) z_axisa = MIN_Z_A;
	if (z_axisa > MAX_Z_A) z_axisa = MAX_Z_A;
	xa = (double) x_axisa / 1000.0;
	ya = (double) y_axisa / 1000.0;
	za = (double) z_axisa / 1000.0;
	// Calculate Pitch and Roll:
	phi = asin(xa);								// Calculate the and phi
	the = asin(ya / cos(the));					//  from the raw values
}

/**
 * This function calculates the position in space by
 * using the the and phi values (see function
 * normalizeLSM303DLHC_A()!). It stores the position
 * in the global double variables pitch and roll. 
 *
 */
void positionLSM303DLHC_A(void)
{
	pitch = the * 180 / M_PI + OFFSET_PITCH_A;
	roll = phi * 180 / M_PI + OFFSET_ROLL_A;
}

// LSM303DLHC magnetometer functions:
int16_t x_axism, y_axism, z_axism;
int16_t headingm;
double xm, ym, zm;
#ifdef GET_TEMP_M
int16_t temperaturem;
double temperature_mag;
#endif

int16_t headingtc;

/**
 * Call this once before using the 3D-Compass.
 *
 */
void LSM303DLHC_M_init(void)
{
#ifdef GET_TEMP_M
	// Set Data output rate to 15Hz, enable temperature sensor:
	I2CTWI_transmit2Bytes(I2C_SENSOR_LSM303DLHC_M_ADR, CRA_REG_M, CRA_REG_M_15HZ_T_EN);
#else
	// Set Data output rate to 15Hz:
	I2CTWI_transmit2Bytes(I2C_SENSOR_LSM303DLHC_M_ADR, CRA_REG_M, CRA_REG_M_15HZ);
#endif
	mSleep(10);
	// Set Gain to +-1.3Gauss:
	I2CTWI_transmit2Bytes(I2C_SENSOR_LSM303DLHC_M_ADR, CRB_REG_M, CRB_REG_M_13GAUSS);
	mSleep(10);
	// Set Continuous-conversion mode:
	I2CTWI_transmit2Bytes(I2C_SENSOR_LSM303DLHC_M_ADR, MR_REG_M, MR_REG_M_CCM);
	mSleep(10);
}

/**
 * This function reads the X-axis, Y-axis, Z-axis and
 * temperature values from the LSM303DLHC magnetometer
 * and stores them in the global variables x_axism,
 * y_axism, z_axism and temperaturem.
 *
 */
void readLSM303DLHC_M(void)
{
	uint8_t readBuf[6];

	I2CTWI_transmitByte(I2C_SENSOR_LSM303DLHC_M_ADR, OUT_X_H_M);
	I2CTWI_readBytes(I2C_SENSOR_LSM303DLHC_M_ADR, readBuf, 6); // Read  X-/Y-/Z-axis
	// xb = x:
	x_axism = (readBuf[OUT_X_H_M - OUT_X_H_M] << 8) + readBuf[OUT_X_L_M - OUT_X_H_M];
	// yb = y:
	y_axism = (readBuf[OUT_Y_H_M - OUT_X_H_M] << 8) + readBuf[OUT_Y_L_M - OUT_X_H_M];
	// zb = z:
	z_axism = (readBuf[OUT_Z_H_M - OUT_X_H_M] << 8) + readBuf[OUT_Z_L_M - OUT_X_H_M];
	mSleep(10);
#ifdef GET_TEMP_M
	// Read temperature raw value:
	I2CTWI_transmitByte(I2C_SENSOR_LSM303DLHC_M_ADR, TEMP_OUT_H_M);
	readBuf[0] = I2CTWI_readByte(I2C_SENSOR_LSM303DLHC_M_ADR);
	I2CTWI_transmitByte(I2C_SENSOR_LSM303DLHC_M_ADR, TEMP_OUT_L_M);
	readBuf[1] = I2CTWI_readByte(I2C_SENSOR_LSM303DLHC_M_ADR);
	temperaturem = (readBuf[0] << 8) | readBuf[1];
	temperaturem >>= 4;
	if (temperaturem & 2048)					// Calculate temperature
		temperaturem = (temperaturem & 2047) - 2048;
	else
		temperaturem = temperaturem & 2047;
	mSleep(10);
#endif
}

/**
 * This function compensates hard iron effects and
 * scales the x-axism, y-axism and z-axism values read
 * from the LSM303DLHC magnetometer with the function
 * readLSM303DLHC_M() and stores them in the global
 * double variables xm, ym and zm.
 *
 */
void normalizeLSM303DLHC_M(void)
{
	// Add hard iron offsets:
	x_axism += OFFSET_X_M;
	y_axism += OFFSET_Y_M;
	z_axism += OFFSET_Z_M;
	// Scale raw values:
	xm = (x_axism - MIN_X_M) / (MAX_X_M - MIN_X_M) * 2 - 1;
	ym = (y_axism - MIN_Y_M) / (MAX_Y_M - MIN_Y_M) * 2 - 1;
	zm = (z_axism - MIN_Z_M) / (MAX_Z_M - MIN_Z_M) * 2 - 1;
}

/**
 * This function calculates the heading by using the
 * normalized sensor values xm and ym (see function
 * normalizeLSM303DLHC_M()!). The heading is a value
 * from 0° to 359°. If the robot's front (and the
 * RP6 Sensor Board's front edge with the two
 * headlights) points to ...
 *   north -> heading is 0°,
 *   east  -> heading is 90°,
 *   south -> heading is 180°,
 *   west  -> heading is 270°.
 *
 */
int16_t headingLSM303DLHC_M(void)
{
	double hdg = atan2(ym, xm) * 180 / M_PI + DECLINATION_M;
	if(hdg < 0.0) hdg += 360.0;
	return ((int16_t) hdg);
}

// --------------------------------------------
/**
 * This function calculates the TILT COMPENSATED
 * heading by using accelerometer (the, phi) AND
 * magnetometer (xm, ym, zm) data. The heading is a
 * value from 0° to 359°. If the robot's front (and
 * the RP6 Sensor Board's front edge with the two
 * headlights) points to ...
 *   north -> heading is 0°,
 *   east  -> heading is 90°,
 *   south -> heading is 180°,
 *   west  -> heading is 270°.
 *
 */
int16_t headingLSM303DLHC_TC(void)
{
	double hdgtc, xh, yh;

	xh = xm * cos(the) + zm * sin(the);
	yh = xm * sin(phi) * sin(the) + ym * cos(phi) - zm * sin(phi) * cos(the);
//	zh = -xm * cos(phi) * sin(the) + ym * sin(phi) + zm * cos(phi) * cos(the);
	hdgtc = atan2(yh, xh) * 180 / M_PI + DECLINATION_M;
	if(hdgtc < 0.0) hdgtc += 360.0;
	return ((int16_t) hdgtc);
}

// --------------------------------------------
/**
 * Call this once before using the LSM303DLHC.
 *
 */
void LSM303DLHC_init(void)
{
	LSM303DLHC_A_init();						// Accelerometer init
	LSM303DLHC_M_init();						// Magnetometer init
}

/*****************************************************************************/
// Analog Yaw-Rate Gyroscope (LY330ALH):

/**
 * Sets the LY330ALH mode of operation.
 *
 * Input: mode -> Mode [0..3]
 *
 */
void LY330ALH_set_mode(uint8_t mode)
{
	outport.pd_gyro = false;					// I/O 0.6: Low
	outport.st_gyro = false;					// I/O 0.7: Low
	if(mode & 1) outport.pd_gyro = true;
	if(mode & 2) outport.st_gyro = true;
	MAX7311_update();
}

/**
 * Reads the ADC1 value from the RP6Base.
 *
 * Hints: - The RP6Base input ADC1 has to be
 *          connected to the USRBUS: Pin 2 on
 *          the RP6 mainboard!
 *        - The RP6Base_I2CSlave program has to
 *          be loaded into the RP6Base
 *          microcontroller!
 *
 */
uint16_t getRP6Base_ADC1(void)
{
	I2CTWI_readRegisters(I2C_RP6_BASE_ADR, I2C_REG_ADC_ADC1_L, registerBuf, 2);
	return (registerBuf[0] + (registerBuf[1] << 8));
}

int16_t z_axisg;

/**
 * Performs a LY330ALH gyroscope measurement and
 * returns the angular rate (velocity).
 * Positive values mean an angular rate while
 * rotating LEFT and negative values while
 * rotating RIGHT!
 * The global integer variable z_axisg also
 * contains the measurement result.
 *
 */
int16_t LY330ALH_measure(void)
{
	z_axisg = getRP6Base_ADC1();				// Read raw data
	z_axisg -= ADCVAL_ZERORATE_LEVEL;			// Calculate value
	return (z_axisg);
}

/*****************************************************************************/
// Hot Swappable 2-Wire Bus Buffer (TCA4311A):

/**
 * Reads the TCA4311A Ready output. 
 * The function returns the state (true/false).
 * The global variable readyBusbuff is also
 * filled with the Ready output state.
 *
 * Hint: You don't have to use this function,
 *       if the task task_RP6SensorBoard() is
 *       called frequently out of the main
 *       loop, because readyBusbuff will be
 *       updated by this task!
 *
 */
uint8_t readReadyBusbuff(void)
{
	I2CTWI_transmitByte(I2C_SENSOR_PORTEXP_ADR, MAX7311_INPUT_PORT1);
	readyBusbuff = I2CTWI_readByte(I2C_SENSOR_PORTEXP_ADR);
	readyBusbuff &= RDY_BUSBUFF;
	return readyBusbuff;
}

uint8_t errorBusbuff;

/**
 * If the I2C bus for the I2C-MODULE (SCLOUT,
 * SDAOUT) is not used or shall be isolated from
 * the RP6 I2C bus, this can be done with this
 * function.
 *
 * Hints: - The I2C-MODULE power is NOT switched
 *          off by this function!
 *        - If USE_PD5 is defined in the
 *          configuration header file, this
 *          function uses the RP6M256 portpin,
 *          that is connected to plug I/O: Pin 9
 *          instead of MAX7311 I/O 1.0!
 *        - If the global variable errorBusbuff
 *          returns true, the bus buffer could
 *          not be switched to shutdown mode!
 *
 */
void TCA4311A_shutdown(void)
{
	// Wait until the I2C bus is idle:
	while(I2CTWI_isBusy() || TWI_operation != I2CTWI_NO_OPERATION) task_I2CTWI();
#ifdef USE_PD5
	IO_SENSOR_TCA4311A_EN_PORT &= ~IO_SENSOR_TCA4311A_EN_IN; // Low
#else
	outport.en_busbuff = false;					// I/O 1.0: Low
	MAX7311_update();
#endif
	errorBusbuff = readReadyBusbuff();
}

/**
 * If the I2C bus for the I2C-MODULE (SCLOUT,
 * SDAOUT) was isolated from the RP6 I2C bus with
 * the function TCA4311A_shutdown() before, it
 * can be connected again with this function.
 *
 * Hints: - The I2C-MODULE power is NOT switched
 *          on by this function!
 *        - If USE_PD5 is defined in the
 *          configuration header file, this
 *          function uses the RP6M256 portpin,
 *          that is connected to plug I/O: Pin 9
 *          instead of MAX7311 I/O 1.0!
 *        - If the global variable errorBusbuff
 *          returns true, the bus buffer could
 *          not be switched to normal operation!
 *
 */
void TCA4311A_reconnect(void)
{
	// Wait until the I2C bus is idle:
	while(I2CTWI_isBusy() || TWI_operation != I2CTWI_NO_OPERATION) task_I2CTWI();
#ifdef USE_PD5
	IO_SENSOR_TCA4311A_EN_PORT |= IO_SENSOR_TCA4311A_EN_IN; // High
#else
	outport.en_busbuff = true;					// I/O 1.0: High
	MAX7311_update();
#endif
	errorBusbuff = !readReadyBusbuff();
}

/**
 * Sets/clears the RP6Base signal PWR.
 *
 * Input: pwr -> 0 (false) = PWR off
 *               >0 (true) = PWR on
 *
 * Hints: - The RP6Base signal PWR has to be
 *          connected to the USRBUS: Pin 3 on
 *          the RP6 mainboard!
 *        - The RP6Base_I2CSlave program has to
 *          be loaded into the RP6Base
 *          microcontroller!
 *
 */
void setRP6Base_PWR(uint8_t pwr)
{
	if(pwr > 0)
		I2CTWI_transmit2Bytes(I2C_RP6_BASE_ADR, 0, CMD_POWER_ON);
	else
		I2CTWI_transmit2Bytes(I2C_RP6_BASE_ADR, 0, CMD_POWER_OFF);
	while(I2CTWI_isBusy() || TWI_operation != I2CTWI_NO_OPERATION) task_I2CTWI();
}

/** 
 * With this function you can switch the I2C-
 * MODULE (plug M1) power on or off.
 *
 * Input: pwr -> 0 (false) = I2C-MODULE power off
 *               >0 (true) = I2C-MODULE power on
 *
 * Hints: - If the I2C-MODULE plug is not used,
 *          you should always switch the
 *          I2C-MODULE power off (default)!
 *        - The TCA4311A is NOT switched to
 *          normal operation (default) or
 *          shutdown mode by this function!
 *        - The RP6Base signal PWR has to be
 *          connected to the USRBUS: Pin 3 on
 *          the RP6 mainboard!
 *        - The RP6Base_I2CSlave program has to
 *          be loaded into the RP6Base
 *          microcontroller!
 *
 */
void setI2CModulePower(uint8_t pwr)
{
	if(pwr > 0) {
		setRP6Base_PWR(1);						// RP6Base PWR on
		outport.pwr_i2c = true;					// I/O 0.0: High
	}
	else
		outport.pwr_i2c = false;				// I/O 0.0: Low
	MAX7311_update();
}	

/*****************************************************************************/
// Button:

/**
 * Reads the MAX7311 I/O port 1.7. 
 * The function returns true, if  button S1 is
 * pressed (I/O 1.7: Low!) and false, if it is
 * not pressed (I/O 1.7: High!).
 * The global variable pressedButton is also
 * filled with the I/O 1.7 output state.
 *
 * Hint: You don't have to use this function,
 *       if the task task_RP6SensorBoard() is
 *       called frequently out of the main
 *       loop, because pressedButton will be
 *       updated by this task!
 *
 */
uint8_t readButton(void)
{
	I2CTWI_transmitByte(I2C_SENSOR_PORTEXP_ADR, MAX7311_INPUT_PORT2);
	pressedButton = I2CTWI_readByte(I2C_SENSOR_PORTEXP_ADR);
	pressedButton &= IO2_I2C;
	pressedButton = !pressedButton;
	return pressedButton;
}

/*****************************************************************************/
// Status LED:

/** 
 * Sets the status LED (D2) of the RP6 Sensor
 * Board.
 *
 * Example:
 *   setStatusLED(1);
 *   // This switches the status LED on!
 *
 */
void setStatusLED(uint8_t on)
{
	if(on > 0)
		outport.io1_i2c = false;				// I/O 1.6: Low
	else
		outport.io1_i2c = true;					// I/O 1.6: High
	MAX7311_update();
}

/*****************************************************************************/
/*****************************************************************************/
// RP6 Sensor Board initialisation and shutdown:

/**
 * You MUST call this function at the beginning of a
 * main program, that uses the RP6 Sensor Board. 
 *
 */
void sensor_board_init(void)
{
	// I/O Port Expander:
	MAX7311_init();
	// RTC:
	DS1339_init();
	// Temperature Sensor:
	LM75B_init();
	// Servo Controller:
	PCA9685_init(50);							// Init PWM 50 Hz
	// LED Driver:
	TCA6507_init();
	// 3D Magnetometer & Accelerometer:
	LSM303DLHC_init();
}

/**
 * If you don't use any function of the RP6
 * Sensor Board, you can put the board into
 * "SHUTDOWN MODE". In this mode the electric
 * power consumption is very low to save energy.
 *
 */
void sensor_board_shutdown(void)
{
	// Servo Controller:
	PCA9685_shutdown();							// Shutdown PCA9685
	// Temperature Sensor:
	LM75B_shutdown();							// Shutdown LM75B
	// I/O Port Expander:
	MAX7311_write(MAX7311_OUTPUT_SHUTDOWN);
}

/*****************************************************************************/
/*****************************************************************************/

/******************************************************************************
 * Additional info
 * ****************************************************************************
 * Changelog:
 * - v. 1.0 TCA4311A reconnect problem fixed 10.04.2014 by Dirk
 * - v. 0.9 Interrupt status functions added 09.04.2014 by Dirk
 * - v. 0.8 (initial release) 17.03.2014 by Dirk
 *
 * ****************************************************************************
 */

/*****************************************************************************/
// EOF

Erklärung

Demo

makefile:

...
TARGET = RP6M256_Sensor_Board_01
...
SRC += $(RP6_LIB_PATH)/RP6common/RP6I2CmasterTWI.c

SRC += $(RP6_LIB_PATH)/RP6control_M256_WIFI/RP6M256_Sensor_BoardLib.c
...

Datei: RP6M256_Sensor_Board_01.c

/* 
 * ****************************************************************************
 * RP6 ROBOT SYSTEM - RP6 CONTROL M256 Examples
 * ****************************************************************************
 * Example: RP6M256 Sensor Board
 * Author(s): Dirk
 * ****************************************************************************
 * Description:
 * In this example we show a first test for the RP6 Sensor Board.
 * 
 * ############################################################################
 * The Robot does NOT move in this example! You can simply put it on a table
 * next to your PC and you should connect it to the PC via the USB Interface!
 * You should also connect to it via WIFI.
 * ############################################################################
 * ****************************************************************************
 */

/*****************************************************************************/
// Includes:

#include "RP6M256Lib.h" 				// The RP6 M256 Library. 
										// Always needs to be included!
#include "RP6I2CmasterTWI.h"			// Include the I2C-Bus Master Library

/*****************************************************************************/
/*****************************************************************************/
// Include our new "RP6M256 Sensor Board library":
// (This is the library for accessing the RP6 Sensor Board!)

#include "RP6M256_Sensor_BoardLib.h"

/*****************************************************************************/
// Variables:

char dir[3];

/*****************************************************************************/

/**
 * Returns a 2 character string for the eighth
 * parts of the direction calculated from the
 * heading value.
 *
 * Input: heading -> Heading value [0..359]
 *
 */
void calculateDir(char *dir, uint16_t heading)
{
	dir[1] = ' ';
	dir[2] = '\0';
	if ((heading <= 22) || (heading >=338)) dir[0] = 'N';
	if ((heading >= 23) && (heading <= 67)) {dir[0] = 'N'; dir[1] = 'E';} 
	if ((heading >= 68) && (heading <= 112)) dir[0] = 'E';
	if ((heading >= 113) && (heading <= 157)) {dir[0] = 'S'; dir[1] = 'E';}
	if ((heading >= 158) && (heading <= 202)) dir[0] = 'S';
	if ((heading >= 203) && (heading <= 247)) {dir[0] = 'S'; dir[1] = 'W';}
	if ((heading >= 248) && (heading <= 292)) dir[0] = 'W';
	if ((heading >= 293) && (heading <= 337)) {dir[0] = 'N'; dir[1] = 'W';}
}

/**
 * Write a floating point number to the WIFI.
 *
 * Example:
 *
 *			// Write a floating point number to the WIFI (no exponent):
 *			writeDouble_WIFI(1234567.890, 11, 3);
 *
 * The value of prec (precision) defines the number of decimal places.
 * For 32 bit floating point variables (float, double ...) 6 is
 * the max. value for prec (7 relevant digits).
 * The value of width defines the overall number of characters in the
 * floating point number including the decimal point. The number of
 * pre-decimal positions is: (width - prec - 1).
 */
void writeDouble_WIFI(double number, uint8_t width, uint8_t prec)
{char buffer[width + 1];
	dtostrf(number, width, prec, &buffer[0]);
	writeString_WIFI(&buffer[0]);
}

/**
 * Write a floating point number to the LCD.
 *
 * Example:
 *
 *			// Write a floating point number to the LCD (no exponent):
 *			writeDoubleLCD(1234567.890, 11, 3);
 *
 * The value of prec (precision) defines the number of decimal places.
 * For 32 bit floating point variables (float, double ...) 6 is
 * the max. value for prec (7 relevant digits).
 * The value of width defines the overall number of characters in the
 * floating point number including the decimal point. The number of
 * pre-decimal positions is: (width - prec - 1).
 */
void writeDoubleLCD(double number, uint8_t width, uint8_t prec)
{char buffer[width + 1];
	dtostrf(number, width, prec, &buffer[0]);
	writeStringLCD(&buffer[0]);
}

/*****************************************************************************/
// I2C Error handler

/**
 * This function gets called automatically if there was an I2C Error like
 * the slave sent a "not acknowledge" (NACK, error codes e.g. 0x20 or 0x30).
 *
 */
void I2C_transmissionError(uint8_t errorState)
{
	writeString_P_WIFI("\nI2C ERROR - TWI STATE: 0x");
	writeInteger_WIFI(errorState, HEX);
	writeChar_WIFI('\n');
}

/*****************************************************************************/
// Main function - The program starts here:

int main(void)
{
	initRP6M256();    // Always call this first! The Processor will not work
					  // correctly otherwise. 

	initLCD(); // Initialize the LC-Display (LCD)
			   // Always call this before using the LCD!

	setLEDs(0b1111);
	mSleep(500);
	setLEDs(0b0000);

	writeString_P_WIFI("\n\nRP6 Sensor Board Selftest 1!\n"); 

	// IMPORTANT:
	I2CTWI_initMaster(100); // Initialize the TWI Module for Master operation
							// with 100kHz SCL Frequency


	// Register the event handler:
	I2CTWI_setTransmissionErrorHandler(I2C_transmissionError);

	setLEDs(0b1111);

	// Write a text message to the LCD:
	showScreenLCD("################", "################");
	mSleep(1500);
	showScreenLCD("RP6v2-M256-WIFI ", "Example Program");
	mSleep(2500); 
	showScreenLCD("RP6 Sensor Board", "   Selftest 1");
	mSleep(2500);
	clearLCD();

	setLEDs(0b0000);

	// ---------------------------------------

	uint8_t onoff = 0;
	uint16_t servopos = SERVO1_LT;
	int16_t rate;

	startStopwatch1();
	startStopwatch2();

	// IMPORTANT:
	sensor_board_init();						// RP6 Sensor Board init!!!
	setServoPower(1);							// Servo power ON!
/*
	// ----------------------------------------------
	// Set RTC once (battery empty or not existing:
	rtc_time.second = 0;
	rtc_time.minute = 15;
	rtc_time.hour = 20;				// 20:15
	rtc_date.weekday = R_SU;
	rtc_date.day = 20;
	rtc_date.month = 4;
	rtc_date.year = 2014;			// Su, 20.4.2014
	DS1339_write();
	// Remove this, if RTC is set and running!!!
	// ----------------------------------------------
*/
	while(true) 
	{
		if(getStopwatch1() > 1000) // 1s
		{
			if (onoff) onoff = 0;
			else onoff = 1;

			// Interrupt status:
			writeString_P_WIFI("\nInterrupt Status:      ");
			writeIntegerLength_WIFI(interrupt_RP6SBstatus.byte, BIN, 8);
			writeString_P_WIFI("\n");

			// Inport status:
			writeString_P_WIFI("Inport Status: ");
			writeIntegerLength_WIFI(inport.word, BIN, 16);
			writeString_P_WIFI("\n");

			// Temperature sensor test:
			temperature = LM75B_measure();		// Measure
			writeString_P_WIFI("Temperature: ");
			writeDouble_WIFI(temperature, 7, 3);
			writeString_P_WIFI("°\n");

			// LED driver test:
			if (onoff) {
				setLED(HEADLIGHT_R, STATE_ON);
				setLED(HEADLIGHT_L, STATE_OFF);
			}
			else {
				setLED(HEADLIGHT_L, STATE_ON);
				setLED(HEADLIGHT_R, STATE_OFF);
			}

			// Servo controller test (SERVO_1):
			setServo(1, servopos);
			servopos += 10;
			if (servopos > SERVO1_RT) servopos = SERVO1_LT;

			// RTC test:
			DS1339_read();
			writeString_P_WIFI("RTC: ");
			writeIntegerLength_WIFI(rtc_time.hour, DEC, 2);
			writeString_P_WIFI(":");
			writeIntegerLength_WIFI(rtc_time.minute, DEC, 2);
			writeString_P_WIFI(":");
			writeIntegerLength_WIFI(rtc_time.second, DEC, 2);
			writeString_P_WIFI("  ");
			writeIntegerLength_WIFI(rtc_date.day, DEC, 2);
			writeString_P_WIFI(".");
			writeIntegerLength_WIFI(rtc_date.month, DEC, 2);
			writeString_P_WIFI(".");
			writeIntegerLength_WIFI(rtc_date.year, DEC, 4);
			writeString_P_WIFI("\n");

			// 3D Magnetometer & Accelerometer test:
			//  LSM303DLHC accelerometer:
			task_I2CTWI();
			readLSM303DLHC_A();					// Get sensor values
			task_I2CTWI();
			writeString_P_WIFI("\n3D-Acceleration SENSOR ->\n");
			writeString_P_WIFI("X-axis: ");
			writeInteger_WIFI(x_axisa, DEC);
			writeChar_WIFI('\n');
			writeString_P_WIFI("Y-axis: ");
			writeInteger_WIFI(y_axisa, DEC);
			writeChar_WIFI('\n');
			writeString_P_WIFI("Z-axis: ");
			writeInteger_WIFI(z_axisa, DEC);
			writeChar_WIFI('\n');
			normalizeLSM303DLHC_A();			// Normalize data
			positionLSM303DLHC_A();				// Calculate position
			writeString_P_WIFI("POSITION: \n");
			writeString_P_WIFI("  Pitch [°]: ");
			writeDouble_WIFI(pitch, 6, 1);
			writeChar_WIFI('\n');
			writeString_P_WIFI("  Roll [°]:  ");
			writeDouble_WIFI(roll, 6, 1);
			writeChar_WIFI('\n');

			//  LSM303DLHC magnetometer:
			task_I2CTWI();
			readLSM303DLHC_M();					// Get sensor values
			task_I2CTWI();
			writeString_P_WIFI("\n3D-Compass SENSOR ->\n");
			writeString_P_WIFI("X-axis: ");
			writeInteger_WIFI(x_axism, DEC);
			writeChar_WIFI('\n');
			writeString_P_WIFI("Y-axis: ");
			writeInteger_WIFI(y_axism, DEC);
			writeChar_WIFI('\n');
			writeString_P_WIFI("Z-axis: ");
			writeInteger_WIFI(z_axism, DEC);
			writeChar_WIFI('\n');
			normalizeLSM303DLHC_M();			// Normalize data
			headingm = headingLSM303DLHC_M();	// Calculate heading
			writeString_P_WIFI("                 Heading [°]: ");
			writeInteger_WIFI(headingm, DEC);
			calculateDir(dir, headingm);
			writeString_P_WIFI("  Direction: ");
			writeString_WIFI(dir);
			writeChar_WIFI('\n');
			headingtc = headingLSM303DLHC_TC();	// Calculate TILT COMPENSATED
			writeString_P_WIFI("TILT COMPENSATED heading [°]: "); // heading
			writeInteger_WIFI(headingtc, DEC);
			calculateDir(dir, headingtc);
			writeString_P_WIFI("  Direction: ");
			writeString_WIFI(dir);
			writeChar_WIFI('\n');
#ifdef GET_TEMP_M
			temperature_mag = (double) temperaturem / 8.0 + OFFSET_TEMP_M;
			writeString_P_WIFI("Temperature [°]: ");
			writeDouble_WIFI(temperature_mag, 7, 2);
			writeChar_WIFI('\n');
#endif
			// Yaw-Rate Gyroscope test:
			rate = LY330ALH_measure();
			writeString_P_WIFI("\n1D-Gyro SENSOR ->\n");
			writeString_P_WIFI("Z-axis: ");
			writeInteger_WIFI(rate, DEC);
			writeString_P_WIFI("\n");

			// RP6 Sensor Board shutdown (button & status LED test):
			if (pressedButton) {
				setStatusLED(1);
				writeString_P_WIFI("\nPress button again for RP6 Sensor Board SHUTDOWN!\n");
				mSleep(2000);
				do {
					if(getStopwatch2() > 200) { // 0.2s
						if (onoff) onoff = 0;
						else onoff = 1;
						if (onoff) setStatusLED(1);
						else setStatusLED(0);
						setStopwatch2(0);
					}
					mSleep(1);
					task_I2CTWI();
				} while (!readButton());
				writeString_P_WIFI("\n\n\n\n\nPlease wait for RP6 Sensor Board SHUTDOWN...\n");
				sensor_board_shutdown();
				mSleep(3000);
				writeString_P_WIFI("\n\n\nThe RP6 Sensor Board now is in SHUTDOWN MODE!!!\n");
				mSleep(1000);
				writeString_P_WIFI("\nRESET the M256 microcontroller now...\n\n");
				while(true) {};
			}

			setStopwatch1(0);
		}

		task_checkINTs();						// Read interrupt signals
		task_RP6SensorBoard();					// RP6 Sensor Board task

		task_I2CTWI();
	}

	return 0;
}

Erklärung

Projekte

RP6 Xtra Module

Die Dokumentation zu den RP6 Xtra Modulen befindet sich auf der mitgelieferten "RP6 Robot System Xtra Modules CD-ROM". Auf der CD-ROM sind auch Software-Beispiele zu finden.

Die hier veröffentlichte Software zu den Xtra Modulen ist eine Eigenentwicklung in GCC:

• Arexx I2C GPS Empfänger JM3-GPS
• Arexx Gyro Modul JM3-GYRO
• Arexx 3D Accelerometer Modul JM3-3DA

I2C GPS Empfänger JM3-GPS

RP6M256 WiFi: I2C_GPS Library

Die nachfolgende RP6M256_I2C_GPSLib (GCC) dient zur Ansteuerung des I2C GPS Empfängers durch die RP6M256 WiFi.

Libary Header

Datei: RP6M256_I2C_GPSLib.h

/* ****************************************************************************
 *                           _______________________
 *                           \| RP6  ROBOT SYSTEM |/
 *                            \_-_-_-_-_-_-_-_-_-_/           >>> RP6 M256 WIFI
 * ----------------------------------------------------------------------------
 * ----------------------------- [c]2014 - Dirk -------------------------------
 * ****************************************************************************
 * File: RP6M256_I2C_GPSLib.h
 * Version: 1.0
 * Target: RP6 M256 WIFI - ATMEGA2560 @16.00MHz
 * Author(s): Dirk
 * ****************************************************************************
 * Description:
 * Header file for new RP6 I2C GPS Receiver library.
 *
 * ****************************************************************************
 */

#ifndef RP6M256_I2C_GPSLIB_H
#define RP6M256_I2C_GPSLIB_H


/*****************************************************************************/
// RP6 Sensor Board:

// If you want to use the RP6 I2C GPS Receiver on the RP6 Sensor Board, you
// have to define SENSOR_BOARD (default)!
// If SENSOR_BOARD is NOT defined, this library assumes that the RP6 I2C GPS
// Receiver is used stand-alone.
// ------------------
#define SENSOR_BOARD
// ------------------

/*****************************************************************************/
// Includes:

#include "RP6M256Lib.h"
#include "RP6I2CmasterTWI.h"
#ifdef SENSOR_BOARD
	#include "RP6M256_Sensor_BoardLib.h"
#endif

/*****************************************************************************/
// RP6 I2C GPS Receiver:
#define I2C_GPS_RECEIVER_ADR			0xc8	// Default
//#define I2C_GPS_RECEIVER_ADR			0xca
//#define I2C_GPS_RECEIVER_ADR			0xcc
//#define I2C_GPS_RECEIVER_ADR			0xce
//#define I2C_GPS_RECEIVER_ADR			0xd0	// DO NOT USE on Sensor Board!
//#define I2C_GPS_RECEIVER_ADR			0xd2
//#define I2C_GPS_RECEIVER_ADR			0xd4
//#define I2C_GPS_RECEIVER_ADR			0xd6
//#define I2C_GPS_RECEIVER_ADR			0xd8
//#define I2C_GPS_RECEIVER_ADR			0xda
//#define I2C_GPS_RECEIVER_ADR			0xdc
//#define I2C_GPS_RECEIVER_ADR			0xde

// Read registers:
#define WHO_AM_I						0x20	// I2C address [0xc8..0xde]
#define LAT_DEG							0x21	// Degrees
#define LAT_MIN							0x22	// Minutes
#define LAT_MIN_D_L						0x23	// Minutes decimal part low
#define LAT_MIN_D_H						0x24	// Minutes decimal part high
#define LON_DEG							0x25	// Degrees
#define LON_MIN							0x26	// Minutes
#define LON_MIN_D_L						0x27	// Minutes decimal part low
#define LON_MIN_D_H						0x28	// Minutes decimal part high
#define STATUS							0x29	// Status flags
#define FIX_IND							0x2a	// FIX_IND
#define HDOP							0x2b	// HDOP
#define HDOP_D							0x2c	// HDOP_D
#define SAT_N							0x2d	// Number of satellites
#define COURSE_T_L						0x2e	// True heading low byte
#define COURSE_T_H						0x2f	// True heading high byte
#define COURSE_T_D						0x30	// Decimal part of course
#define SPEED_L							0x31	// Speed low byte
#define SPEED_H							0x32	// Speed high byte
#define SPEED_D							0x33	// Speed decimal part
#define ALT_L							0x34	// Altitude low byte
#define ALT_H							0x35	// Altitude high byte
#define ALT_DIF_L						0x36	// Differ. WGS-84 MSL low byte
#define ALT_DIF_H						0x37	// Differ. WGS-84 MSL high byte
#define UTC_H							0x38	// UTC hour
#define UTC_MIN							0x39	// UTC minute
#define UTC_S							0x3a	// UTC second
#define DAY								0x3b	// Day
#define YEAR_L							0x3c	// Year low byte
#define YEAR_H							0x3d	// Year high byte
#define MONTH							0x3e	// Month
#define PDOP							0x3f	// PDOP
#define PDOP_D							0x40	// PDOP_D
#define VDOP							0x41	// VDOP
#define VDOP_D							0x42	// VDOP_D
#define SAT_1							0x43	// SAT # 1
#define SAT_2							0x44	// SAT # 2
#define SAT_3							0x45	// SAT # 3
#define SAT_4							0x46	// SAT # 4
#define SAT_5							0x47	// SAT # 5
#define SAT_6							0x48	// SAT # 6
#define SAT_7							0x49	// SAT # 7
#define SAT_8							0x4a	// SAT # 8
#define SAT_9							0x4b	// SAT # 9
#define SAT_A							0x4c	// SAT # 10
#define SAT_B							0x4d	// SAT # 11
#define SAT_C							0x4e	// SAT # 12
#define SW_VERSION						0x4f	// SW_Version
#define HEARTBEAT						0x50	// Heartbeat

// Status and Fix_Ind register bitmasks:
#define STATUS_NS						0b00000001
#define STATUS_EW						0b00000010
#define STATUS_SPU						0b00000100
#define STATUS_AUTON					0b00001000
#define FIX_IND_UI_FIX					0b10000000

// Write registers:
#define INIT_CFG						0x90	// INIT CFG
#define SET_FIX_TIME					0x91	// Pos_Fix_Time
#define STBY							0x92	// STBY
#define FAC_RES							0x93	// Factory Reset
#define EN_JAM							0x94	// Jamming Remover
#define TEST_MODE						0x96	// Test Mode

// Write register values (commands):
#define INIT_CFG_HOT_START				1		// Hot Start
#define INIT_CFG_WARM_START				2		// Warm Start init data loaded
#define INIT_CFG_WARM_START_EPH_CLR		3		// Warm Start Ephemeris cleared
#define INIT_CFG_COLD_START				4		// Cold Start
#define SET_FIX_TIME_1S					1		// Pos Fix Time 1 second
#define SET_FIX_TIME_2S					2		// Pos Fix Time 2 seconds
#define STBY_ENTER						1		// Standby (cleared after write)
#define FAC_RES_ENTER					1		// Factory Reset
#define EN_JAM_DISABLE					0		// Jamming Remover disable
#define EN_JAM_ENABLE					1		// Jamming Remover enable
#define TEST_MODE_INACTIVE				0		// Test Mode inactive
#define TEST_MODE_ACTIVE				0x15	// Test Mode active

// Data structure (starts at LAT_DEG):
#define DATA_STRUCT						LAT_DEG	// Start
// All data from LAT_DEG to HEARTBEAT (48 bytes):
#define DATA_LENGTH						(HEARTBEAT - LAT_DEG + 1)
// Basic data from LAT_DEG to SAT_N (13 bytes):
#define DATA_BASIC						(SAT_N - LAT_DEG + 1)

typedef union {
 	uint8_t byte;
	struct {
		unsigned ns    :1;				// 1: North 0: South
		unsigned ew    :1;				// 1: East 0: West
		unsigned spu   :1;				// 1: km/h 0: kn
		unsigned auton :1;				// 1: Data not valid 0: Autonomous
		unsigned fix0  :1;
		unsigned fix1  :1;
		unsigned stat0 :1;				// Invalid
		unsigned stat1 :1;				// GPS Com Error
	};
	struct {
		unsigned ns    :1;				// 1: North 0: South
		unsigned ew    :1;				// 1: East 0: West
		unsigned spu   :1;				// 1: km/h 0: kn
		unsigned auton :1;				// 1: Data not valid 0: Autonomous
		unsigned fix   :2;
		unsigned stat  :2;				// 00 -> Valid
										// 01 -> Invalid
										// 10 -> GPS Com Error
										// 11 -> n. def.
	};
} status_t;

typedef union {
 	uint8_t byte;
	struct {							// Fix indicator:
		unsigned fix_0  :1;				//  1: GPS fix          2: DGPS fix
		unsigned fix_1  :1;				//  3: PPS fix          4: RTK
		unsigned fix_2  :1;				//  5: Float RTK        6: Dead reck.
		unsigned fix_3  :1;				//  7: Manual inp. mode 8: Simul. mode
		unsigned unused :3;
		unsigned ui_fix :1;				// HW signal from UC530
	};
	struct {
		unsigned fix    :4;				// See above "Fix indicator"!
		unsigned unused :3;
		unsigned ui_fix :1;				// HW signal from UC530
	};
} fix_ind_t;

typedef union {
	struct {
		uint8_t   latDeg;
		uint8_t   latMin;
		uint16_t  latMinDec;
		uint8_t   lonDeg;
		uint8_t   lonMin;
		uint16_t  lonMinDec;
		status_t  status;
		fix_ind_t fixInd;
		uint8_t   hdop;
		uint8_t   hdopDec;
		uint8_t   satCount;
		uint16_t  course;
		uint8_t   courseDec;
		uint16_t  speed;
		uint8_t   speedDec;
		uint16_t  altitude;
		uint16_t  altDif;
		uint8_t   utcHour;
		uint8_t   utcMinute;
		uint8_t   utcSec;
		uint8_t   day;
		uint16_t  year;
		uint8_t   month;
		uint8_t   pdop;
		uint8_t   pdopDec;
		uint8_t   vdop;
		uint8_t   vdopDec;
		uint8_t   sat[12];
		uint8_t   swVersion;
		uint8_t   heartbeat;
	};
} gpsdata_t;
extern gpsdata_t gpsdata;

void I2C_GPS_reset(void);
void I2C_GPS_write_cmd(uint8_t, uint8_t);
uint8_t I2C_GPS_read_reg(uint8_t);
void I2C_GPS_read(uint8_t);
void I2C_GPS_init(void);
void I2C_GPS_new_addr(uint8_t);

/*****************************************************************************/

#endif

/******************************************************************************
 * Additional info
 * ****************************************************************************
 * Changelog:
 * 
 *  ---> changes are documented in the file "RP6M256_I2C_GPSLib.c"
 *
 * ****************************************************************************
 */

/*****************************************************************************/
// EOF

Libary Source

Datei: RP6M256_I2C_GPSLib.c

/* ****************************************************************************
 *                           _______________________
 *                           \| RP6  ROBOT SYSTEM |/
 *                            \_-_-_-_-_-_-_-_-_-_/           >>> RP6 M256 WIFI
 * ----------------------------------------------------------------------------
 * ----------------------------- [c]2014 - Dirk -------------------------------
 * ****************************************************************************
 * File: RP6M256_I2C_GPSLib.c
 * Version: 1.0
 * Target: RP6 M256 WIFI - ATMEGA2560 @16.00MHz
 *         and a RP6 I2C GPS Receiver (CONRAD 1082385)
 *         optionally on a RP6 Sensor Board (CONRAD 1082384)
 * Author(s): Dirk
 * ****************************************************************************
 * Description:
 * 
 * This is our new Library that contains basic routines and functions for
 * accessing the RP6 I2C GPS Receiver JM3-GPS (CONRAD 1082385).
 * The library assumes the I2C GPS Receiver software (firmware) v1.2.
 * If the I2C GPS Receiver is located on the RP6 Sensor Board (CONRAD 1082384)
 * and connected to the I2C-MODULE plug M1, the RP6M256_Sensor_BoardLib has to
 * be included!
 * 
 * ****************************************************************************
 */

/*****************************************************************************/
// Includes:

#include "RP6M256_I2C_GPSLib.h" 		

/*****************************************************************************/
// Variables:

uint8_t gpsdataBuf[DATA_LENGTH + 1]; 

/*****************************************************************************/

/**
 * Software reset for the I2C GPS Receiver.
 *
 */
void I2C_GPS_reset(void)
{
	I2CTWI_transmit2Bytes(I2C_GPS_RECEIVER_ADR, 0, 0x06);
}

/**
 * Writes a command cmd to a register reg of the
 * I2C GPS Receiver.
 *
 * Input: reg -> Write register [0x90..0x96]
 *        cmd -> Command
 *
 */
void I2C_GPS_write_cmd(uint8_t reg, uint8_t cmd)
{
	if((reg < INIT_CFG) || (reg > TEST_MODE)) return;
	I2CTWI_transmit2Bytes(I2C_GPS_RECEIVER_ADR, reg, cmd);
}

/**
 * Reads a single register reg from the I2C GPS
 * Receiver.
 *
 * Input: reg -> Read register [0x20..0x50]
 *
 */
uint8_t I2C_GPS_read_reg(uint8_t reg)
{
	if((reg < WHO_AM_I) || (reg > HEARTBEAT)) reg = WHO_AM_I;
	I2CTWI_transmitByte(I2C_GPS_RECEIVER_ADR, reg);
	return (I2CTWI_readByte(I2C_GPS_RECEIVER_ADR));
}

gpsdata_t gpsdata;

/**
 * Reads the GPS data from the I2C GPS Receiver.
 *
 * Input: length -> Number of data bytes [1..48]
 *
 * Hint: If length = DATA_BASIC, only basic GPS
 *       data are read. If length = DATA_LENGTH,
 *       ALL GPS data are read.
 *
 */
void I2C_GPS_read(uint8_t length)
{
	uint8_t i;

	if(length > DATA_LENGTH) length = DATA_LENGTH;
	I2CTWI_readRegisters(I2C_GPS_RECEIVER_ADR, DATA_STRUCT, gpsdataBuf, length);
	for(i = length; i < DATA_LENGTH; i++) {
		gpsdataBuf[i] = 0;
	}
	gpsdata.latDeg = gpsdataBuf[0];
	gpsdata.latMin = gpsdataBuf[1];
	gpsdata.latMinDec = gpsdataBuf[2] + (gpsdataBuf[3] << 8);
	gpsdata.lonDeg = gpsdataBuf[4];
	gpsdata.lonMin = gpsdataBuf[5];
	gpsdata.lonMinDec = gpsdataBuf[6] + (gpsdataBuf[7] << 8);
	gpsdata.status.byte = gpsdataBuf[8];
	gpsdata.fixInd.byte = gpsdataBuf[9];
	gpsdata.hdop = gpsdataBuf[10];
	gpsdata.hdopDec = gpsdataBuf[11];
	gpsdata.satCount = gpsdataBuf[12];
	if(length == DATA_BASIC) return;
	gpsdata.course = gpsdataBuf[13] + (gpsdataBuf[14] << 8);
	gpsdata.courseDec = gpsdataBuf[15];
	gpsdata.speed = gpsdataBuf[16] + (gpsdataBuf[17] << 8);
	gpsdata.speedDec = gpsdataBuf[18];
	gpsdata.altitude = gpsdataBuf[19] + (gpsdataBuf[20] << 8);
	gpsdata.altDif = gpsdataBuf[21] + (gpsdataBuf[22] << 8);
	gpsdata.utcHour = gpsdataBuf[23];
	gpsdata.utcMinute = gpsdataBuf[24];
	gpsdata.utcSec = gpsdataBuf[25];
	gpsdata.day = gpsdataBuf[26];
	gpsdata.year = gpsdataBuf[27] + (gpsdataBuf[28] * 100);
	gpsdata.month = gpsdataBuf[29];
	gpsdata.pdop = gpsdataBuf[30];
	gpsdata.pdopDec = gpsdataBuf[31];
	gpsdata.vdop = gpsdataBuf[32];
	gpsdata.vdopDec = gpsdataBuf[33];
	for(i = 34; i < 46; i++) {
		gpsdata.sat[i - 34] = gpsdataBuf[i];
	}
	gpsdata.swVersion = gpsdataBuf[46];
	gpsdata.heartbeat = gpsdataBuf[47];
}

/**
 * You MUST call this function at the beginning of a
 * main program, that uses the RP6 I2C GPS Receiver. 
 * If SENSOR_BOARD is defined, the I2C-MODULE power
 * will be switched on with the function
 * setI2CModulePower() defined in the
 * RP6M256_Sensor_BoardLib, which will be included
 * then.
 *
 */
void I2C_GPS_init(void)
{
#ifdef SENSOR_BOARD
	// I2C-MODULE (plug M1) power on: 
	setI2CModulePower(1);
#endif
	// Other init procedures here!
}

/**
 * Writes a new I2C address to the I2C GPS Receiver.
 *
 * Input: addr -> New I2C address [0xc8..0xde]
 *
 * Hints: - Default I2C address is 0xc8.
 *        - You may choose a new I2C address in
 *          the range 0xc8..0xde.
 *        - After writing a new address you have
 *          to switch the I2C GPS Receiver off
 *          and on again.
 *        - Definition I2C_GPS_RECEIVER_ADR must
 *          be adapted to the new I2C address!
 *
 */
void I2C_GPS_new_addr(uint8_t addr)
{
	if((addr < 0xc8) || (addr > 0xde)) return;
	addr &= 0xfe;								// Clear R/W bit
	I2CTWI_transmit3Bytes(I2C_GPS_RECEIVER_ADR, 0, 0x04, addr);
}

/*****************************************************************************/
/*****************************************************************************/

/******************************************************************************
 * Additional info
 * ****************************************************************************
 * Changelog:
 * - v. 1.0 (initial release) 21.04.2014 by Dirk
 *
 * ****************************************************************************
 */

/*****************************************************************************/
// EOF

Erklärung

Demo

makefile:

...
TARGET = RP6M256_Sensor_Board_02
...
SRC += $(RP6_LIB_PATH)/RP6common/RP6I2CmasterTWI.c

SRC += $(RP6_LIB_PATH)/RP6control_M256_WIFI/RP6M256_Sensor_BoardLib.c
SRC += $(RP6_LIB_PATH)/RP6control_M256_WIFI/RP6M256_I2C_GPSLib.c
...

Datei: RP6M256_Sensor_Board_02.c

/* 
 * ****************************************************************************
 * RP6 ROBOT SYSTEM - RP6 CONTROL M256 Examples
 * ****************************************************************************
 * Example: RP6M256 Sensor Board
 * Author(s): Dirk
 * ****************************************************************************
 * Description:
 * In this example we show a second test for the RP6 Sensor Board.
 * 
 * ############################################################################
 * The Robot does NOT move in this example! You can simply put it on a table
 * next to your PC and you should connect it to the PC via the USB Interface!
 * You should also connect to it via WIFI.
 * ############################################################################
 * ****************************************************************************
 */

/*****************************************************************************/
// Includes:

#include "RP6M256Lib.h" 				// The RP6 M256 Library. 
										// Always needs to be included!
#include "RP6I2CmasterTWI.h"			// Include the I2C-Bus Master Library

/*****************************************************************************/
/*****************************************************************************/
// Include our new "RP6M256 I2C GPS Receiver library":
// (This is the library for accessing the RP6 I2C GPS Receiver!)

#include "RP6M256_I2C_GPSLib.h"

/*****************************************************************************/

/**
 * Write a floating point number to the WIFI.
 *
 * Example:
 *
 *			// Write a floating point number to the WIFI (no exponent):
 *			writeDouble_WIFI(1234567.890, 11, 3);
 *
 * The value of prec (precision) defines the number of decimal places.
 * For 32 bit floating point variables (float, double ...) 6 is
 * the max. value for prec (7 relevant digits).
 * The value of width defines the overall number of characters in the
 * floating point number including the decimal point. The number of
 * pre-decimal positions is: (width - prec - 1).
 */
void writeDouble_WIFI(double number, uint8_t width, uint8_t prec)
{char buffer[width + 1];
	dtostrf(number, width, prec, &buffer[0]);
	writeString_WIFI(&buffer[0]);
}

/**
 * Write a floating point number to the LCD.
 *
 * Example:
 *
 *			// Write a floating point number to the LCD (no exponent):
 *			writeDoubleLCD(1234567.890, 11, 3);
 *
 * The value of prec (precision) defines the number of decimal places.
 * For 32 bit floating point variables (float, double ...) 6 is
 * the max. value for prec (7 relevant digits).
 * The value of width defines the overall number of characters in the
 * floating point number including the decimal point. The number of
 * pre-decimal positions is: (width - prec - 1).
 */
void writeDoubleLCD(double number, uint8_t width, uint8_t prec)
{char buffer[width + 1];
	dtostrf(number, width, prec, &buffer[0]);
	writeStringLCD(&buffer[0]);
}

/*****************************************************************************/
// I2C Error handler

/**
 * This function gets called automatically if there was an I2C Error like
 * the slave sent a "not acknowledge" (NACK, error codes e.g. 0x20 or 0x30).
 *
 */
void I2C_transmissionError(uint8_t errorState)
{
	writeString_P_WIFI("\nI2C ERROR - TWI STATE: 0x");
	writeInteger_WIFI(errorState, HEX);
	writeChar_WIFI('\n');
}

/*****************************************************************************/
// Main function - The program starts here:

int main(void)
{
	initRP6M256();    // Always call this first! The Processor will not work
					  // correctly otherwise. 

	initLCD(); // Initialize the LC-Display (LCD)
			   // Always call this before using the LCD!

	setLEDs(0b1111);
	mSleep(500);
	setLEDs(0b0000);

	writeString_P_WIFI("\n\nRP6 Sensor Board Selftest 2!\n"); 

	// IMPORTANT:
	I2CTWI_initMaster(100); // Initialize the TWI Module for Master operation
							// with 100kHz SCL Frequency


	// Register the event handler:
	I2CTWI_setTransmissionErrorHandler(I2C_transmissionError);

	setLEDs(0b1111);

	// Write a text message to the LCD:
	showScreenLCD("################", "################");
	mSleep(1500);
	showScreenLCD("RP6v2-M256-WIFI ", "Example Program");
	mSleep(2500); 
	showScreenLCD("RP6 Sensor Board", "   Selftest 2");
	mSleep(2500);
	clearLCD();

	setLEDs(0b0000);

	// ---------------------------------------

	uint8_t onoff = 0;
	uint8_t i;

	startStopwatch1();
	startStopwatch2();

	// IMPORTANT:
	sensor_board_init();						// RP6 Sensor Board init!!!
	I2C_GPS_init();								// RP6 I2C GPS Receiver init

	while(true) 
	{
		if(getStopwatch1() > 1000) // 1s
		{
			if (onoff) onoff = 0;
			else onoff = 1;

			// Show GPS data:
			I2C_GPS_read(DATA_LENGTH);			// Read data
			writeString_P_WIFI("\nLAT_DEG:     ");
			writeIntegerLength_WIFI(gpsdata.latDeg, DEC, 2);
			writeString_P_WIFI("° ");
			writeIntegerLength_WIFI(gpsdata.latMin, DEC, 2);
			writeString_P_WIFI(".");
			writeIntegerLength_WIFI(gpsdata.latMinDec, DEC, 4);
			writeString_P_WIFI("' ");
			if(gpsdata.status.ns)
				writeString_P_WIFI("N");
			else
				writeString_P_WIFI("S");

			writeString_P_WIFI("\nLON_DEG:    ");
			writeIntegerLength_WIFI(gpsdata.lonDeg, DEC, 3);
			writeString_P_WIFI("° ");
			writeIntegerLength_WIFI(gpsdata.lonMin, DEC, 2);
			writeString_P_WIFI(".");
			writeIntegerLength_WIFI(gpsdata.lonMinDec, DEC, 4);
			writeString_P_WIFI("' ");
			if(gpsdata.status.ew)
				writeString_P_WIFI("E");
			else
				writeString_P_WIFI("W");

			writeString_P_WIFI("\nSTATUS:     ");
			writeIntegerLength_WIFI(gpsdata.status.byte, BIN, 8);

			writeString_P_WIFI("\nFIX_IND:    ");
			writeIntegerLength_WIFI(gpsdata.fixInd.byte, BIN, 8);

			writeString_P_WIFI("\nHDOP:       ");
			writeInteger_WIFI(gpsdata.hdop, DEC);
			writeString_P_WIFI(".");
			writeIntegerLength_WIFI(gpsdata.hdopDec, DEC, 2);

			writeString_P_WIFI("\nSAT_N:      ");
			writeInteger_WIFI(gpsdata.satCount, DEC);

			writeString_P_WIFI("\nCOURSE_T:   ");
			writeInteger_WIFI(gpsdata.course, DEC);
			writeString_P_WIFI(".");
			writeIntegerLength_WIFI(gpsdata.courseDec, DEC, 2);
			writeString_P_WIFI("°");

			writeString_P_WIFI("\nSPEED:      ");
			writeInteger_WIFI(gpsdata.speed, DEC);
			writeString_P_WIFI(".");
			writeIntegerLength_WIFI(gpsdata.speedDec, DEC, 2);
			if(gpsdata.status.spu)
				writeString_P_WIFI("km/h");
			else
				writeString_P_WIFI("kn");

			writeString_P_WIFI("\nALT:        ");
			writeInteger_WIFI(gpsdata.altitude, DEC);

			writeString_P_WIFI("m\nALT_DIF:    ");
			writeInteger_WIFI(gpsdata.altDif, DEC);

			writeString_P_WIFI("m\nUTC:        ");
			writeIntegerLength_WIFI(gpsdata.utcHour, DEC, 2);
			writeString_P_WIFI(":");
			writeIntegerLength_WIFI(gpsdata.utcMinute, DEC, 2);
			writeString_P_WIFI(":");
			writeIntegerLength_WIFI(gpsdata.utcSec, DEC, 2);

			writeString_P_WIFI("\nDATE:       ");
			writeIntegerLength_WIFI(gpsdata.day, DEC, 2);
			writeString_P_WIFI(".");
			writeIntegerLength_WIFI(gpsdata.month, DEC, 2);
			writeString_P_WIFI(".");
			writeIntegerLength_WIFI(gpsdata.year, DEC, 4);

			writeString_P_WIFI("\nPDOP:       ");
			writeInteger_WIFI(gpsdata.pdop, DEC);
			writeString_P_WIFI(".");
			writeIntegerLength_WIFI(gpsdata.pdopDec, DEC, 2);

			writeString_P_WIFI("\nVDOP:       ");
			writeInteger_WIFI(gpsdata.vdop, DEC);
			writeString_P_WIFI(".");
			writeIntegerLength_WIFI(gpsdata.vdopDec, DEC, 2);
			writeString_P_WIFI("\n");

			for(i = 0; i < gpsdata.satCount; i++) {
				if(i == 8) writeString_P_WIFI("\n");
				writeString_P_WIFI("  SAT_");
				writeIntegerLength_WIFI(i + 1, HEX, 1);
				writeString_P_WIFI(": ");
				writeInteger_WIFI(gpsdata.sat[i], DEC);
			}

			writeString_P_WIFI("\nSW_VERSION: ");
			writeInteger_WIFI(gpsdata.swVersion, DEC);

			writeString_P_WIFI("\nHEARTBEAT:  ");
			writeInteger_WIFI(gpsdata.heartbeat, DEC);
			writeString_P_WIFI("\n");

			// RP6 Sensor Board shutdown (button & status LED test):
			if (pressedButton) {
				setStatusLED(1);
				writeString_P_WIFI("\nPress button again for RP6 Sensor Board SHUTDOWN!\n");
				mSleep(2000);
				do {
					if(getStopwatch2() > 200) { // 0.2s
						if (onoff) onoff = 0;
						else onoff = 1;
						if (onoff) setStatusLED(1);
						else setStatusLED(0);
						setStopwatch2(0);
					}
					mSleep(1);
					task_I2CTWI();
				} while (!readButton());
				writeString_P_WIFI("\n\n\n\n\nPlease wait for RP6 Sensor Board SHUTDOWN...\n");
				sensor_board_shutdown();
				mSleep(3000);
				writeString_P_WIFI("\n\n\nThe RP6 Sensor Board now is in SHUTDOWN MODE!!!\n");
				mSleep(1000);
				writeString_P_WIFI("\nRESET the M256 microcontroller now...\n\n");
				while(true) {};
			}

			setStopwatch1(0);
		}

		task_RP6SensorBoard();					// RP6 Sensor Board task

		task_I2CTWI();
	}

	return 0;
}

Erklärung

Gyro Modul JM3-GYRO

Analoge Auswertung

Am Pin OUT des Gyro Moduls (siehe Anschluss-Schema!) liegt die Ausgangsspannung des Sensors (0..5V) an. Sie kann mit einem ADC-Eingang eines Microcontrollers gemessen werden. Syntax mit den Libraries der RP6Base, M32 oder M256 WiFi (ADC Kanal 0):

uint16_t result = readADC(0);

In der C-Control-Pro Umgebung der M128 sieht das z.B. in CompactC so aus (ADC Kanal 0):

word result;
ADC_Set(ADC_VREF_VCC, 0);
...
result = ADC_Read();

Das Ergebnis result kann noch zur Weiterverarbeitung angepasst werden:

#define ADCVAL_ZERORATE_LEVEL			511		// Zero rate Level
...
int16_t z_axisg = result - ADCVAL_ZERORATE_LEVEL;

Dadurch zeigt z_axisg Null an, wenn keine Rotation anliegt und wird positiv, wenn nach LINKS gedreht wird und negativ bei RECHTS-Drehung. Die Definition ADCVAL_ZERORATE_LEVEL muss natürlich an den eigenen Sensor-Nullwert angepasst werden.

Aus dem Absolutwert von z_axisg kann man die Drehrate (Drehgeschwindigkeit) auch bestimmen:

In den Technischen Daten ist die Empfindlichkeit des Sensors mit 10mV/dps (0,01V pro Grad und Sekunde) angegeben. Damit errechnet sich die Drehrate als:

Drehrate [dps] = abs(z_axisg) * (5 / 1024 / 0.01) = abs(z_axisg) * 0.48828125

Optionale Ansteuerung

Im Anschluss-Schema ist erkennbar, dass es noch 2 Steuerleitungen SLEEP/PD und ST gibt. Mit diesen Anschlüssen kann die Betriebsart des Gyro Moduls festgelegt werden, siehe nachfolgende Tabelle:

Will man zwischen den Betriebsarten per Software umschalten, muss man die beiden Steuerleitungen mit I/O Portpins des Microcontrollers verbinden, die als Ausgänge konfiguriert sind. Braucht man diese Funktion nicht, reicht es, ST und SLEEP/PD mit GND zu verbinden (Betriebsart "Normal").

3D Accelerometer Modul JM3-3DA

Das 3D Accelerometer Modul kann über den I2C- oder den SPI-Bus angesteuert werden. Wird das Modul auf der RP6 CONTROL M32 Platine betrieben (es sitzt also auf dem Sockel IC5 für das 2. SPI-EEPROM), dann erfolgt die Ansteuerung über den SPI-Bus. Die SPI-Grundfunktionen befinden sich in der RP6ControlLib, die daher eingebunden werden muss.

RP6 CONTROL M32: ACCSENS Library

Die nachfolgende RP6ControlACCSENSLib (GCC) dient zur Ansteuerung des 3D Accelerometer Moduls durch die RP6 CONTROL M32.

Configuration Header

Vor dem Kompilieren der Library muss eine Anpassung des Configuration Headers an die eigenen Hardware-Voraussetzungen erfolgen. Dazu gibt es im Configuration Header eine Definition:

• EXP_STACK_1 -> Die RP6 CONTROL M32 sitzt auf dem vorderen Platinenstapel 1 des RP6

Im nachfolgenden Listing ist die Definition EXP_STACK_1 NICHT aktiv (d.h. die M32 sitzt auf dem hinteren Platinenstapel 2!). Dies muss an die eigene Hardware angepaßt werden. Wenn die RP6 CONTROL M32 auf dem vorderen Platinenstapel 1 des RP6 sitzt, muss die Zeile mit "EXP_STACK_1" "aktiviert" werden. Dazu entfernt man "//" am Zeilenanfang:

#define EXP_STACK_1

Bevor man den Beschleunigungssensor nutzen kann, muss man ihn kalibrieren:

Accelerometer:

• Die X-Achse wird kalibriert, indem man den RP6 mit der rechten Seite nach unten hält und den Maximalwert in MAX_X_A einträgt. Dann neigt man ihn auf seine linke Seite und trägt den negativen Maximalwert in MIN_X_A ein.
• Die Y-Achse wird kalibriert, indem man den RP6 mit der Vorderseite nach oben hält und den Maximalwert in MAX_Y_A einträgt. Dann kippt man die Vorderseite nach unten und trägt den negativen Maximalwert in MIN_Y_A ein.
• Die Z-Achse wird kalibriert, indem man den RP6 normal waagerecht hält und den Maximalwert in MAX_Z_A einträgt. Dann dreht man den RP6 "auf den Kopf" und trägt den negativen Maximalwert in MIN_Z_A ein.
• Sollten die angezeigten Pitch- und Roll-Werte nach der Kalibrierung der Achsen und genau waagerecht stehendem Sensor bzw. RP6 vom Nullwert deutlich abweichen, kann man sie mithilfe der Definitionen OFFSET_PITCH_A und OFFSET_ROLL_A "nullen". Die Einheit dieser Definitionen ist °.

Nach dem Eintragen der Kalibrierungswerte muss das Programm neu kompiliert werden!

Datei: RP6ControlACCSENS.h

/* ****************************************************************************
 *                           _______________________
 *                           \| RP6  ROBOT SYSTEM |/
 *                            \_-_-_-_-_-_-_-_-_-_/             >>> RP6 CONTROL
 * ----------------------------------------------------------------------------
 * ----------------------------- [c]2014 - Dirk -------------------------------
 * ****************************************************************************
 * File: RP6ControlACCSENS.h
 * Version: 1.0
 * Target: RP6 CONTROL - ATMEGA32 @16.00MHz
 * Author(s): Dirk
 * ****************************************************************************
 * Description:
 * Configuration header file for new ACCSENS library.
 *
 * ****************************************************************************
 * THE CHANGELOG CAN BE FOUND AT THE END OF THIS FILE!
 * ****************************************************************************
 */

#ifndef RP6CONTROLACCSENS_H
#define RP6CONTROLACCSENS_H


/*****************************************************************************/
// Sensor position on the RP6 robot:
// (If EXP_STACK_1 is defined, the RP6 CONTROL M32 is located on the front
//  stack for addon boards on the RP6 robot. If EXP_STACK_1 is NOT defined
//  (default), the RP6 CONTROL M32 is mounted on the rear stack.)
// -------------------
//#define EXP_STACK_1
// -------------------

/*****************************************************************************/
// The following sensor is used:
// - 3D Accelerometer Modul (CONRAD 1082387-62) with LIS302DLH 3-axis
//   accelerometer

/*****************************************************************************/
// LIS302DLH accelerometer calibration data:
#define MAX_X_A						32767		// Max. X-axis value
#define MIN_X_A						-32768		// Min. X-axis value
#define MAX_Y_A						32767		// Max. Y-axis value
#define MIN_Y_A						-32768		// Min. Y-axis value
#define MAX_Z_A						32767		// Max. Z-axis value
#define MIN_Z_A						-32768		// Min. Z-axis value
#define OFFSET_PITCH_A				0.0			// Offset Pitch [°]
#define OFFSET_ROLL_A				0.0			// Offset Roll [°]

/*****************************************************************************/

#endif

/******************************************************************************
 * Additional info
 * ****************************************************************************
 * Changelog:
 * 
 *  ---> changes are documented in the file "RP6ControlACCSENSLib.c"
 *
 * ****************************************************************************
 */

/*****************************************************************************/
// EOF

Library Header

Datei: RP6ControlACCSENSLib.h

/* ****************************************************************************
 *                           _______________________
 *                           \| RP6  ROBOT SYSTEM |/
 *                            \_-_-_-_-_-_-_-_-_-_/             >>> RP6 CONTROL
 * ----------------------------------------------------------------------------
 * ----------------------------- [c]2014 - Dirk -------------------------------
 * ****************************************************************************
 * File: RP6ControlACCSENSLib.h
 * Version: 1.0
 * Target: RP6 CONTROL - ATMEGA32 @16.00MHz
 * Author(s): Dirk
 * ****************************************************************************
 * Description:
 * This is the RP6ControlACCSENSLib header file.
 * You have to include this file, if you want to use the library
 * RP6ControlACCSENSLib.c in your own projects.
 *
 * ****************************************************************************
 * THE CHANGELOG CAN BE FOUND AT THE END OF THIS FILE!
 * ****************************************************************************
 */

#ifndef RP6CONTROLACCSENSLIB_H
#define RP6CONTROLACCSENSLIB_H


/*****************************************************************************/
// Includes:

#include "RP6ControlLib.h" 		// The RP6 Control Library (v. 1.1 or higher). 
#include "RP6ControlACCSENS.h"
#include <math.h>

/*****************************************************************************/

#define ACC_CS							MEM_CS2
#define DEVICE_IDENTIFIER				0x32

// Registers:
#define WHO_AM_I						0x0f
#define CTRL_REG1						0x20
#define CTRL_REG2						0x21
#define CTRL_REG3						0x22
#define CTRL_REG4						0x23
#define CTRL_REG5						0x24
#define HP_FILTER_RESET					0x25
#define REFERENCE						0x26
#define STATUS_REG						0x27
#define OUT_X_L							0x28
#define OUT_X_H							0x29
#define OUT_Y_L							0x2a
#define OUT_Y_H							0x2b
#define OUT_Z_L							0x2c
#define OUT_Z_H							0x2d
#define INT1_CFG						0x30
#define INT1_SOURCE						0x31
#define INT1_THS						0x32
#define INT1_DURATION					0x33
#define INT2_CFG						0x34
#define INT2_SOURCE						0x35
#define INT2_THS						0x36
#define INT2_DURATION					0x37

// Control Register bitmasks:
#define CTRL_REG1_POWERDOWN				0b00000000
#define CTRL_REG1_3D_DEFAULT			0b00000111
#define CTRL_REG1_3D_LOPWR_10HZ			0b11000111
#define CTRL_REG2_DEFAULT				0b00000000
#define CTRL_REG3_DEFAULT				0b00000000
#define CTRL_REG4_DEFAULT				0b00000000
#define CTRL_REG4_BDU_2G				0b10000000
#define CTRL_REG4_BDU_2G_STSIGN			0b10001000
#define CTRL_REG4_BDU_BLE_2G			0b11000000
#define CTRL_REG4_BDU_BLE_2G_STSIGN		0b11001000
#define CTRL_REG5_DEFAULT				0b00000000

// Read/write address bitmasks:
#define READ_MASK						0b10000000
#define READ_AUTOINC_MASK				0b11000000
#define WRITE_MASK						0b01111111

extern int16_t x_axisa, y_axisa, z_axisa;
extern double pitch, roll;
extern double xa, ya, za;
extern double the, phi;

uint8_t SPI_ACCSENS_readByte(uint8_t regAddr);
void SPI_ACCSENS_readBytes(uint8_t startAddr, uint8_t *buffer, uint8_t length);
void SPI_ACCSENS_writeByte(uint8_t regAddr, uint8_t data);
uint8_t SPI_ACCSENS_getStatus(void);

void SPI_ACCSENS_init(void);
void SPI_ACCSENS_shutdown(void);
void SPI_ACCSENS_read(void);
void SPI_ACCSENS_normalize(void);
void SPI_ACCSENS_position(void);

/*****************************************************************************/

#endif

/******************************************************************************
 * Additional info
 * ****************************************************************************
 * Changelog:
 * 
 *  ---> changes are documented in the file "RP6ControlACCSENSLib.c"
 *
 * ****************************************************************************
 */

/*****************************************************************************/
// EOF

Library Source

Datei: RP6ControlACCSENSLib.c

/* ****************************************************************************
 *                           _______________________
 *                           \| RP6  ROBOT SYSTEM |/
 *                            \_-_-_-_-_-_-_-_-_-_/             >>> RP6 CONTROL
 * ----------------------------------------------------------------------------
 * ----------------------------- [c]2014 - Dirk -------------------------------
 * ****************************************************************************
 * File: RP6ControlACCSENSLib.c
 * Version: 1.0
 * Target: RP6 CONTROL - ATMEGA32 @16.00MHz
 *         with optional LC-Display 16x2 chars (CONRAD 190911-62)
 *         and a 3D Accelerometer Modul (CONRAD 1082387-62) (IC5)
 * Author(s): Dirk
 * ****************************************************************************
 * Description:
 * This is my RP6 Control ACCSENS Library. This library may be used to access
 * the basic functions of the 3D Accelerometer Modul. This RP6 Xtra module may
 * be placed on the IC5 socket on the RP6 CONTROL M32 instead of a second
 * EEPROM.
 *
 * ****************************************************************************
 * THE CHANGELOG CAN BE FOUND AT THE END OF THIS FILE!
 * ****************************************************************************
 */
 
/*****************************************************************************/
// Includes:

#include "RP6ControlACCSENSLib.h"

/*****************************************************************************/
// SPI functions:

/**
 * Reads a single register from the 3D Accelerometer
 * Modul (ACCSENS).
 *
 */
uint8_t SPI_ACCSENS_readByte(uint8_t regAddr)
{
	uint8_t data;
	regAddr |= READ_MASK;						// Read byte
	PORTB &= ~ACC_CS;
	writeSPI(regAddr);
	data = readSPI();
	PORTB |= ACC_CS;
	return data;
}

/**
 * Reads "length" bytes into the buffer "buffer"
 * from startAddr on. 
 *
 */
void SPI_ACCSENS_readBytes(uint8_t startAddr, uint8_t *buffer, uint8_t length)
{
	startAddr |= READ_AUTOINC_MASK;				// Read byte, auto increment
	PORTB &= ~ACC_CS;
	writeSPI(startAddr);
	readBufferSPI(&buffer[0], length);
	PORTB |= ACC_CS;
}

/**
 * Write a single data byte to the specified sensor
 * address.
 *
 */
void SPI_ACCSENS_writeByte(uint8_t regAddr, uint8_t data)
{
	PORTB &= ~ACC_CS;
	writeSPI(regAddr);
	writeSPI(data);
	PORTB |= ACC_CS;
}

/**
 * Returns sensor status register - for checking if
 * sensor is busy. 
 *
 */
uint8_t SPI_ACCSENS_getStatus(void)
{
	uint8_t status;
	PORTB &= ~ACC_CS;
	writeSPI(STATUS_REG);
	status = readSPI();
	PORTB |= ACC_CS;
	return status;
}

/*****************************************************************************/
// 3D accelerometer functions:

int16_t x_axisa, y_axisa, z_axisa;
double pitch, roll;
double xa, ya, za;
double the, phi;

/**
 * Initializes the sensor. You must call this
 * function at the beginning of a program, that
 * uses the 3D Accelerometer Modul.
 *
 */
void SPI_ACCSENS_init(void)
{
	// All axes enable, low power, 10Hz output data rate:
	SPI_ACCSENS_writeByte(CTRL_REG1, CTRL_REG1_3D_LOPWR_10HZ);
	// No update while reading, +- 2g, self-test minus:
	SPI_ACCSENS_writeByte(CTRL_REG4, CTRL_REG4_BDU_2G_STSIGN);
}

/**
 * Sets the LIS302DLH power-down mode. If you
 * do not need the sensor, you can save energy
 * by using the sensor power-down mode.
 *
 */
void SPI_ACCSENS_shutdown(void)
{
	// LIS302DLH in power-down mode:
	SPI_ACCSENS_writeByte(CTRL_REG1, CTRL_REG1_POWERDOWN);
}

/**
 * This function reads the X-axis, Y-axis and Z-axis
 * values from the LIS302DLH accelerometer and stores
 * them in the global variables x_axisa, y_axisa and
 * z_axisa.
 *
 */
void SPI_ACCSENS_read(void)
{
#ifndef EXP_STACK_1
	// Default sensor position (M32 located on the RP6 rear stack):
	// xb = -y:
	x_axisa = SPI_ACCSENS_readByte(OUT_Y_L) | (SPI_ACCSENS_readByte(OUT_Y_H) << 8);
	x_axisa *= -1;
	// yb = -x:
	y_axisa = SPI_ACCSENS_readByte(OUT_X_L) | (SPI_ACCSENS_readByte(OUT_X_H) << 8);
	y_axisa *= -1;
	// zb = z:
	z_axisa = SPI_ACCSENS_readByte(OUT_Z_L) | (SPI_ACCSENS_readByte(OUT_Z_H) << 8);
#else
	// Other sensor position (M32 located on the RP6 front stack):
	// xb = y:
	x_axisa = SPI_ACCSENS_readByte(OUT_Y_L) | (SPI_ACCSENS_readByte(OUT_Y_H) << 8);
	// yb = x:
	y_axisa = SPI_ACCSENS_readByte(OUT_X_L) | (SPI_ACCSENS_readByte(OUT_X_H) << 8);
	// zb = z:
	z_axisa = SPI_ACCSENS_readByte(OUT_Z_L) | (SPI_ACCSENS_readByte(OUT_Z_H) << 8);
#endif
}

/**
 * This function limits and normalizes the x-axisa,
 * y-axisa and z-axisa values read from the LIS302DLH
 * accelerometer with the function SPI_ACCSENS_read() and
 * stores them in the global double variables xa, ya and
 * za.
 * It also calculates the position in space in form of the
 * tilt angles (the = pitch, phi = roll) and stores them
 * in the global double variables the and phi.
 *
 */
void SPI_ACCSENS_normalize(void)
{
	// Limit raw values:
	if (x_axisa < MIN_X_A) x_axisa = MIN_X_A;
	if (x_axisa > MAX_X_A) x_axisa = MAX_X_A;
	if (y_axisa < MIN_Y_A) y_axisa = MIN_Y_A;
	if (y_axisa > MAX_Y_A) y_axisa = MAX_Y_A;
	if (z_axisa < MIN_Z_A) z_axisa = MIN_Z_A;
	if (z_axisa > MAX_Z_A) z_axisa = MAX_Z_A;
	xa = (double) x_axisa / 10000.0;
	ya = (double) y_axisa / 10000.0;
	za = (double) z_axisa / 10000.0;
	// Calculate Pitch and Roll:
	phi = asin(xa);								// Calculate the and phi
	the = asin(ya / cos(the));					//  from the raw values
}

/**
 * This function calculates the position in space by
 * using the the and phi values (see function
 * SPI_ACCSENS_normalize()!). It stores the position
 * in the global double variables pitch and roll. 
 *
 */
void SPI_ACCSENS_position(void)
{
	pitch = the * 180 / M_PI + OFFSET_PITCH_A;
	roll = phi * 180 / M_PI + OFFSET_ROLL_A;
}

/******************************************************************************
 * Additional info
 * ****************************************************************************
 * Changelog:
 * - v. 1.0 (initial release) 04.04.2014 by Dirk
 *
 * ****************************************************************************
 */

/*****************************************************************************/
// EOF

Erklärung

Demo

makefile:

...
TARGET = RP6Control_01_ACCSENS
...
SRC += $(RP6_LIB_PATH)/RP6control/RP6ControlLib.c
SRC += $(RP6_LIB_PATH)/RP6common/RP6uart.c
SRC += $(RP6_LIB_PATH)/RP6control/RP6ControlACCSENSLib.c
...

Datei: RP6Control_01_ACCSENS.c

// Uncommented Version of RP6Control_01_ACCSENS.c
// written by Dirk
// ------------------------------------------------------------------------------------------

#include "RP6ControlACCSENSLib.h"


void writeDouble(double number, uint8_t width, uint8_t prec)
{char buffer[width + 1];
	dtostrf(number, width, prec, &buffer[0]);
	writeString(&buffer[0]);
}

void writeDoubleLCD(double number, uint8_t width, uint8_t prec)
{char buffer[width + 1];
	dtostrf(number, width, prec, &buffer[0]);
	writeStringLCD(&buffer[0]);
}


int main(void)
{
	initRP6Control();

	writeString_P("\n\nRP6Control SPI ACCSENS Test Program!\n"); 
	setLEDs(0b1111);
	mSleep(50);
	initLCD(); 
	showScreenLCD("################", "################");

	showScreenLCD("  SPI ACCSENS", "  Test Program");
	mSleep(1000);
	setLEDs(0b0000);

	SPI_ACCSENS_init();

	uint8_t whoami = SPI_ACCSENS_readByte(WHO_AM_I);
	writeString_P("\nWHO AM I: ");
	writeInteger(whoami, HEX);
	writeChar('\n');
	mSleep(2000);
	
	startStopwatch1();

	while(true) 
	{
		if(getStopwatch1() > 1000)
		{
			SPI_ACCSENS_read();
			writeString_P("\n3D Accelerometer SENSOR ->\n");
			writeString_P("X-axis: ");
			writeInteger(x_axisa, DEC);
			writeChar('\n');
			writeString_P("Y-axis: ");
			writeInteger(y_axisa, DEC);
			writeChar('\n');
			writeString_P("Z-axis: ");
			writeInteger(z_axisa, DEC);
			writeChar('\n');
			SPI_ACCSENS_normalize();
			SPI_ACCSENS_position();
			writeString_P("POSITION: \n");
			writeString_P("  Pitch [°]: ");
			writeDouble(pitch, 6, 1);
			writeChar('\n');
			writeString_P("  Roll [°]:  ");
			writeDouble(roll, 6, 1);
			writeChar('\n');
			setCursorPosLCD(0, 0);
			writeStringLCD_P("P");
			writeDoubleLCD(pitch, 6, 1);
			writeStringLCD_P(" ");
			setCursorPosLCD(0, 8);
			writeStringLCD_P("R");
			writeDoubleLCD(roll, 6, 1);
			writeStringLCD_P(" ");
			setCursorPosLCD(1, 0);
			writeStringLCD_P("X");
			writeIntegerLCD(x_axisa, DEC);
			writeStringLCD_P("   ");
			setCursorPosLCD(1, 5);
			writeStringLCD_P("Y");
			writeIntegerLCD(y_axisa, DEC);
			writeStringLCD_P("   ");
			setCursorPosLCD(1, 10);
			writeStringLCD_P("Z");
			writeIntegerLCD(z_axisa, DEC);
			writeStringLCD_P("    ");
			mSleep(2000);
		}
	}
	return 0;
}

Erklärung

Erfahrungsberichte

...in Arbeit...(kann aber gerne ergänzt werden)

Siehe auch

• RP6v2
• RP6
• RP6 - Programmierung
• RP6 Kamera - Mitmach-Projekt
• RP6v2 I2C-Portexpander
• RP6v2 USB-RS232-Adapter
• RP6v2 Orientierung
• RP6 Multi IO Projekt
• RP6 Multi IO Projekt - Software
• CCRP5
• Yeti
• Asuro
• C't-Bot

Weblinks

Siehe auch die Weblinks im RP6v2 und im RP6 Artikel!

• RP6 Sensor Board bei CONRAD
• JM3 Engeneering: RP6 Roboter Erweiterung
• Ankündigung einer Sensorerweiterungsplatine für die RP6v2 M256 WiFi im Roboternetz RP6 Forum
• Erfahrungsbericht zum Sensor Board im AREXX Support Forum und im Roboternetz RP6 Forum
• Erfahrungsbericht zum I2C GPS Empfänger im AREXX Support Forum

Autoren

--Dirk 09:06, 29. Apr 2014 (CET)