#include "fba.h"
/* PWM-Generierung (im wesentlichen in „isr.S“) sowie
Kommunikation mit der „Controller“ genannten Funkfernbedienung.
Diese ist bidirektional und kann Messwerte des Fahrzeugs anzeigen.
Kommunikation über UART mit 115200 Baud.
(Diese Baudrate erzwingt entweder einen Baudratenquarz
oder eine PLL mit 32-KHz-Uhrenquarz.)
*/
byte rxRingBuf[40] NOINIT; // kann nicht ganz voll werden
byte rxWrIdx,rxRdIdx; // Indizes für rxRingBuf
byte rxItIdx; // Index der letzten Empfangslücke bzw. Paketende
static byte wrapIdx(byte i) {if (i>=sizeof rxRingBuf) i-=sizeof rxRingBuf; return i;}
static byte getRxByte(byte i){return rxRingBuf[wrapIdx(i+rxRdIdx)];}
static byte getRxFill() {return wrapIdx(rxWrIdx+sizeof rxRingBuf-rxRdIdx);}
static unsigned getRxWord(byte i) {return getRxByte(i)|getRxByte(i+1)<<8;}
byte txBuf[8] NOINIT;
byte txIdx; // Leseindex für txBuf
L2 l2; // niemals cNaN! cNaN wird I²C bei pwmok≡0 vorgespielt.
L3 l3 NOINIT; // Wird mit cNaN initialisiert
unsigned i1ts NOINIT; // Timerwert bei INT1 zur genaueren Frequenzmessung
L3::L3() {memset(values,cNaN,sizeof values);}
byte L2::autopuls[6];
byte L3::fbaus; // !=0: Fernbedienung unwirksam, zählt mit 150 Hz runter
// 16-Bit-Prüfsumme aus den Bytes eines Puffers,
// beginnend mit Gesamtlängenbyte, berechnen.
// Das Gesamtlängenbyte muss >=4 sein!
static unsigned checksum(const byte*buf) {
byte len=buf[0]-2;
unsigned ret=0;
for (byte i=0; i<len; i++) ret+=buf[i];
return ret;
}
static bool set_checksum(byte*buf) {
byte l=buf[0];
if (l<4 || l>8) return false; // sicherheitshalber!! Bug in Programmlogik!
auto&cs=*reinterpret_cast<unsigned*>(buf+l-2);
cs=~checksum(buf);
return true;
}
static bool check_checksum(byte l) {
unsigned sum=l;
for (byte i=1; i<l-2; i++) sum+=getRxByte(i);
return ~sum==getRxWord(l-2);
}
void L3::onSensorQuery() {
// Bei Hochlauf gibt es noch den Query 04-F0 (Antwort unbekannt)
// Nur 3 Zeilen passen auf die Anzeige, so muss man vertikal scrollen
// Blöd dass Anzeige-Einheiten und Kommastellen nicht konfigurierbar sind.
// Eine Null bei Bordspannung wird ignoriert, sonst Unterspannungspieps.
byte b1=getRxByte(1);
byte k=(b1&0x0F)-1; // Sensor-Kanal (Lo-Nibble), nullbasiert
if (k<6) { // 6 Sensoren = 6 (scrollbare) Zeilen auf dem Display
txBuf[0] = 6; // Standard-Antwort: 6 Bytes
txBuf[1] = b1; // Standard-Antwort: Echo
switch (b1&0xF0) { // Hi-Nibble = Aktion
case 0x80: { // “discover sensor”
txBuf[0] = 4; // Kurze Antwort: Sensor vorhanden
}break;
case 0x90: { // “get sensor type”
/*https://github.com/adis1313/iBUSTelemetry-Arduino/blob/master/iBUSTelemetry.cpp
Die Kanalzuordnung auf dem Display ist rätselhaft!!
Außer man belässt alles bei „Spannung“, dann bleibt die Reihenfolge bestehen.
x-mal probiert: Mit kanalweisen Typzuweisungen kommt nur Müll 'raus!
Scheint ein Firmware-Bug des Controllers zu sein.
Daher wird hier nur „Spannung“ ausgegeben.
0 Interne Spannung Ext.V| | -0.01V
1 (Temperatur/°C - 40) * 10 Temp.| | 15.0C
2 Drehzahl/(U/min) * 100, auch negativ Mot. | |-856RPM
3 Externe Spannung/V * 100 Ext.V| |-20.00V
4 // Avg cell voltage * 100/V wird nicht angezeigt
5 // Battery current * 100/A "
6 // Fuel / Remaining battery percentage "
7 // RPM / Throttle value / battery capacity "
8 // CMP-Heading
9 // Climb rate * 100/m? Fuß?
10 // Course over ground
11 // GPS status (2 values)
12 // Acc X * 100/m/s²
13 // Acc Y * 100/m/s²
14 // Acc Z * 100/m/s²
15 // Roll * 100/°/s "
16 // Pitch * 100/°/s
17 // Yaw * 100/°/s
18 // Vertical speed * 100/m?/s
19 // Ground Speed * 100/m/s
20 // GPS Distance from home /m?
21 // Armed / unarmed
22 // Flight mode "
0x41 // Pressure * 100/bar?
0x7e // Speed *10/km/h
// 4 byte sensors
GPS_LAT 0x80 // WGS84 * 1E7/°
GPS_LON 0x81 // WGS84 in degrees * 1E7/°
GPS_ALT 0x82 // GPS altitude * 100/m?
0x83 // Altitude * 100/m?
0x84 // Max altitude * 100/m?
*/
txBuf[2]=0; // 0 = Spannung
txBuf[3]=2; // muss 2 sein (Bytes Messwert?)
}break;
case 0xA0: { // “query value”
int v = 0;
if (!k) v = l4.lin[4]; // Auf Kanal 0 immer Bordspannung
else{
--k; // wiederum nullbasiert
char sw = l3.values[9]; // Kanal 10 = Schalter SWB
if (sw<-16) { // SWB auf Normalstellung (oben) oder cNaN
v = (k==4) ? (l4.kmz*168>>8)*eedata.fz.onestep>>16 // in cm, mit 2 Kommastellen in Meter
// eigentlich kmz*onestep/100000, 168 ≈ 2**24/100000, Division vermeiden
: l4.lin[k]; // 0: Lenkpoti in °
// 1: Geschwindigkeit in cm/s
// 2: Hubarm/Mulde in °
// 3: Löffel/Verwindung in °
}else if (sw>16) { // SWB zum Bediener: Raw-Werte ADC
v = l4.ad[k]; // 0: Lenkpoti
// 1: Hubarm/Mulde
// 2: Löffel/Verwindung
// 3: Bordspannung
// 4: Temperatur
}else switch (k) { // SWB in Mittelstellung: Sonstige Werte
case 0: v = l4.lin[5]; break; // Temperatur (°C)
case 1: v = l4.lin[6]; break; // Ultraschall-Entfernungsmesser
case 2: v = *(int*)&l4.flags; break; // flags + lamps
case 3: v = l4.kmz; break; // für Kreisfahrt (lo+hi getrennt)
default: v = l4.kmz>>16; break; // und Ermittlung von eedata.fz.q[]
}
}
*(int*)(txBuf+2) = v;
}break;
default: return;
}
set_checksum(txBuf);
UCSR0B= 0b00101000;
// Empfänger abschalten, Sender einschalten und ersten Interrupt auslösen lassen
}
// An der Fernbedienung muss man für Kanal 1 (Batterieanzeige)
// Setup aktivieren und dann auf die Batterien (rechts oben) touchen.
// Danach ist für externe Batterie ein Häkchen (kein Kreuz) zu setzen.
// Dort kann man auch die Alarmgrenzen einstellen.
}
enum{
k7hold_trigger=70, // reichlich 1 s
};
byte L2::k7hold; // Zeitmesser für PWM!=0 an Kanal 7 (Lichtsteuerung)
void L2::k7hold_handler() {
byte h = k7hold;
char c = l2.pwms[6];
if (c) { // aktiv?
if (!++h) return; // Auf 255 begrenzen
if (h==k7hold_trigger) { // 1 Sekunde vergangen?
if (c<0) {
byte lamps = l4.lamps+8;
if ((lamps&0x18)==0x18) lamps&=0xE7; // zählt 0-1-2
eedata.lamps = l4.lamps = lamps;
}else l4.flags^=0x10; // Nur Sound Ein oder Sound Aus (wozu auch immer getrennt; nicht im EEPROM)
}
}else h=0;
k7hold = h;
}
// TODO: Setzfunktion und Completion-Funktion auftrennen!
void L2::set(byte i, char c) {
if (c==cNaN) return; // Nicht setzen sondern (diesen Kanal) übergehen
byte k = i&0x0F;
if (k>=9) return; // Kanalnummer falsch
char b = pwms[k]; // Vorheriger Wert
if (i&fAdd) c = char(limit<int>(b+c,125));
byte slow = i>>bSlow;
if (slow) {
char Δmax = 5 << slow-1;
// 0b01 => 5: Von 0 bis 125 in 25 Stufen = 500 ms
// 0b10 => 10: Von 0 bis 125 in 13 Stufen = 260 ms
// 0b11 => 20: Von 0 bis 125 in 7 Stufen = 140 ms
c = limit<int>(c-b,Δmax)+b; // Wichtig: Über <int> gehen, Überläufe drohen
}
if (!(i&fRegler) && k<4) {
regler[k].reset(0xFF);
L7::purge(k); // Controllereingriff oder I²C-Direktsteuerung kanalspezifisch
}
// TODO: Keine Mehrfachabfrage des (mglw. prellenden) PIND.
if (!notaus::released()) return;
if (k==4) return; // Kanal 5 (Hydraulikpumpe) nicht setzbar
// Bei Mittelstellung von SWB, ohne Offset, und ausgeschalteter Zündung
bool directpwm = !l3.values[9] && !(l4.flags&1<<3);
if (directpwm) {
if (k==1) k=8; // Nicht den Motor sondern Soundgenerator (und damit Setup-Funktion) steuern
}else{
if (k==8) return; // Kanal 9 (Geräuschgenerator) nicht setzbar
// Hintertür: Setup des Geräuschgenerators statt Licht
// Die Unterdrückung der Sprachausgabe erfordert,
// dass der Ein/Aus-Zustand des Motorgeräuschgenerators bekannt ist.
// DAS IST NICHT SICHER BEKANNT!
// Dazu werden hier alle analogen Geber digitalisiert, die nur digital genutzt werden.
if (k>4) {
if (c<=-25) c=-125;
else if (c>=25) c=125;
else c=0; // ternär machen
}
}
pwmok = 10+60*3; // Laufzeit bis zum Toten Mann (3 Minuten)
if (b==c) return; // Raus wenn nichts zu tun ist
if (!directpwm) switch (k) {
// Über Kanal 6 (Index 5) wird der Status der Zündung mitgeschnitten.
// Dazu muss l4.flags bei PowerOn/BrownOut mit 0 initialisiert sein.
case 5: if (c<0 && b>=0) { // Zündung ein/aus verfolgen: Kurzer Puls schaltet
l4.flags^=1<<3; // Bit 3 kippen
}break;
// Über Kanal 7 (Index 6) der Zustand der vom Magic-Controller gesteuerten Lampen
// Dazu muss l4.lamps bei PowerOn/BrownOut vom EEPROM initialisiert sein.
// Der Fall b≡c ist bereits ausgefiltert, b und c sind ternär
case 6: if (b) { // Lampen-Schaltzustand verfolgen
if (k7hold<k7hold_trigger) { // Konstanter Pegel ±125 „kurze“ Zeit?
byte lamps = l4.lamps;
if (b<0) {
lamps++;
if ((lamps&7)>=6) lamps&=0xF8; // 6 Schaltzustände 0..5
}else{
lamps+=0x20;
if (lamps>=5<<5) lamps&=0x1F; // 5 Schaltzustände 0..4
}
eedata.lamps = l4.lamps = lamps;
}
k7hold = 0; // Zeitmessung beenden (c≡0) oder neu starten (c≡-b)
}break;
// Über Kanal 8 (Index 7) der Zustand der Rundumleuchte.
// Dazu müssen die oberen 3 Bits von l4.flags bei PowerOn/BrownOut mit 001
// initialisiert sein.
case 7: if (b<=0) { // Zustand der Rundumleuchte verfolgen
byte lamp = l4.flags + 0x20;
if (lamp>=5<<5) lamp&=0x1F; // 5 Schaltzustände 0..4
l4.flags = lamp; // sonstige Flags unverändert
}break;
}//switch
// Der Magic Controller scheint sich 2 aufeinanderfolgende Flanken zu merken,
// auch wenn diese zügig während des „Herunterfahrens“ gegeben werden.
pwms[k]=c; // hier auch ±127 zulassen, nicht schlimm
if ((k<4 || k==5) && !directpwm) {
// Hydraulikpumpenberechnung gesondert: Hier als Summe: Sinnvoll weil
// bei Parallelarbeit mehrerer Verbraucher die Pumpe mehr leisten muss.
// 230825: Für geringe Lenkgeschwindigkeiten trotzdem Pumpe schneller laufen lassen
// Das dürfte die Lenkeinsatzschwellen deutlich verändern (= Einfluss auf Regelung)
// 230825: Zurück, das ist Mist.
unsigned hy=cabs(pwms[0])+cabs(pwms[2])+cabs(pwms[3])<<0;
pwms[4] = hy>125 ? 125 : (char)hy; // Summe begrenzen und Hydraulikpumpe (nur positiv) setzen
// Motorgeräusch-Berechnung: Beide Motoren fließen ein!
// Sonst klingt die Hydraulik „elektrisch“.
if (l4.flags&1<<3) { // Zündung ein?
int v = pwms[1];
if (v<0) v-=hy; else v+=hy; // Hydraulik in der gleichen Richtung wie Fahren dem Sound hinzufügen
pwms[8] = limit(v,-125,+125);// Kanal 9 setzen
}else pwms[8]=0; // Nicht ins 汉语 Setup purzeln!
}
}
// Aufruf nach erledigter PWM-Ausgabe, alle 20 ms, generiert 8 gleiche Pulse
// Der Rundumleuchte genügen Einzelpulse, der Magic Controller will mehr
void L2::autopuls_handler() {
// Indizes 1..4 dürfen nicht gleichzeitig abgearbeitet werden,
// da sie gemeinsam auf PWM-Kanal 7 (Index 6) wirken!
bool K7busy = false;
for (byte i=0; i<elemof(autopuls); i++) {
static byte longwait;
byte p=autopuls[i];
if (!p) continue; // nichts zu tun
if (1<=i && i<5) {
if (K7busy) continue;// Diesen Autopuls-Generator aufschieben
K7busy=true;
if (longwait && --longwait) continue; // Aktion verzögern
}
autopuls[i]=--p; // Nächster Schritt
if ((p&7)==7) switch (i) { // 8 Telegramme gleich (für Zündung >4 erforderlich) : 160 ms (TODO: Stimmt nicht!!)
case 0: l2.set(5,p&8?-125:0); break;
case 3: longwait=70; nobreak;
case 1: l2.set(6,p&8?-125:0); break;
case 4: longwait=70; nobreak;
case 2: l2.set(6,p&8?+125:0); break;
case 5: l2.set(7,p&8?+125:-125); break;
}
}
}
void L2::countdown() {
// Vor dem „kontrollierten Ableben“ des PWM-Generators
// 5× 1,5-ms-Impluse (Nullstellung) ausgeben
byte ok=l2.pwmok;
if (ok>6 && !notaus::released()) ok=6; // als nächstes memset()
if (ok) {
if (ok>=10) {
static byte pwmokl; // Sub-Countdown solange pwmok>=10
byte l=pwmokl;
if (!l) l=50;
if (!--l) --ok; // Langsamer Countdown mit Periode 50
pwmokl=l;
}else if (--ok==5) // Schneller Countdown; bei Zählerstand 5 …
memset(l2.pwms,0,sizeof l2.pwms); // 1,5-ms-Pulse ausgeben lassen
l2.pwmok=ok;
}
}
/* Aufbau IBUS-Paket „Servo“
0 1 2 4 6 8 10 12 14 16 18 20 22 24 26 28 30 32
20 40 {K1}{K2}{K3}{K4}{K5}{K6}{K7}{K8}{K9}{K10}{1500}{1500}{1500}{1500}{cksum}
Damit beim Einschalten/Reset des ATmega328 nicht mitten in einem
laufenden Telegramm mit dem Empfang bekonnen wird,
legt die Empfangs-ISR erst nach einem Timeout los,
und bedient sich dabei Timer2, OCR2A und GPIOR0.0.
Das vermeidet verwirrende Fehlermeldungen.
(eedata.debug sollte möglichst komplett Null bleiben.)
*/
void L3::checkIbusRx() {
byte f=getRxFill(); // liefert 0..39, nie 40
if (!f) return;
byte b0 = getRxByte(0), lücke = rxItIdx;
if (b0<4 || b0>32) { // Falsche Länge?
eedata.debug[7]++; // Falsches Längenbyte
cli();
rxRdIdx = rxWrIdx = rxItIdx = 0; // Puffer leeren
GPIOR0&=~(1<<0); // erneut auf Lücke warten
sei();
return; // Beim nächsten Poll mit neuem rxRdIdx
}
if (f<b0) return; // Daten nicht ausreichend
// Lücke am Anfang? An das Paketende setzen, um Test unten passieren zu lassen
if (lücke==rxRdIdx) rxItIdx = lücke = wrapIdx(lücke+b0);
// IBUS-Paket „Sensor“ oder „Servo“ vollständig angekommen
// Der Ringpuffer wird/wurde von der ISR weiter gefüllt …
if (check_checksum(b0)) switch (getRxByte(1)) {
case 0x40: if (b0>=24) { // Mindestens 10 Kanal-Worte, dazu Header und Prüfsumme
for (byte i=0; i<10; i++) {
int µs = getRxWord(1+i<<1); // Werte zwischen 1000 (-125) und 2000 (+125)
char c = limit(µs-1500,-500,500) >> 2; // ±125 bietet glatte Umrechnung
// Es kann sein, dass nach RESET (values[i]==cNaN) der Fernbedienungsempfänger
// noch munter seine letzten Daten sendet, und man beim Abschalten der
// Controllerbatterie einen Hebel betätigt hatte bzw. von Null verschiedene Daten kommen.
// Daher für diesen Fall noch kein l2.set aufrufen. (230406)
char b = values[i];
if (b==c) continue; // Ohne kanalweise Änderung nichts tun
values[i] = c;
if (i==4 && !c && b) // Fernbedienung geht aus?
l2.pwmok = 10+3; // In 3 s PWM abschalten (I²C-Slave kann eingreifen)
if (!fbaus && b!=cNaN) // Änderung?
l2.set(i,c); // setzen; Regler werden ggf. dort abgeschaltet
}
// Bei ausgefallenem Controller (Batterie 'raus) sendet der Empfänger
// munter weiter, immer die _gleichen_ Reports
// 230905: Falsch! Das kann man einstellen, unter Failsafe.
// Dort habe ich für Kanal 5 (Taste links) das Senden von 0% festgelegt.
// (Das geht via temporären Mixbetrieb auf Kanal 5.)
// Beim Abschalten des Senders kommt so die 0 als Kennung für „aus“.
// Sonst -125 für nicht gedrückt und +125 für gedrückt.
byte aus = fbaus;
if (!values[4]) aus=0;
else if (aus) --aus; // mit ca. 150 Hz
fbaus = aus;
}break;
default: onSensorQuery();
}else{
eedata.debug[4]++;
}
rxRdIdx=wrapIdx(rxRdIdx+b0); // Lesezeiger vorrücken = Lesepuffer „leeren“
if (lücke!=rxRdIdx) { // Nicht am Paketende?
eedata.debug[5]++; // melden
}
}
| Detected encoding: UTF-8 | 0
|