#include "fba.h"
#include <util/delay_basic.h>
#include <avr/interrupt.h>// cli, sei
/***************
* Ultraschall *
***************/
// Die „störenden“ Tore mit dem Arduino senden alle 60 ms
// (ca. 3 50-Hz-Frames) an fünf Ultraschall-Entfernungsmesser SR-04.
// Macht eine Periodendauer und damit Abfrageintervall von 300 ms.
// In der Hoffnung, dass mindestens einer der SR-04 nicht in
// „Hörreichweite“ ist, ergibt sich eine Lücke von 80 ms,
// ab der der fahrzeugeigene SR-04 sendet.
// Dafür ist <count_csma> zuständig (carrier sense multiple access).
// Danach ist eine Sendepause von 10 Frames dafür zuständig,
// nicht in die Tore zu funken.
// (Die Tore haben kein CSMA sondern ein festes, asynchrones Zeitregime.)
namespace us{ // Ultraschall betreffend
inline int distance(unsigned ticks) {
if (ticks>=(unsigned)iInf) return ticks; // Inf und NaN durchreichen
// innerhalb von 20 ms kann der Reflektor max. 3,4 m weit sein.
// Ganze Millimeter reichen dicke.
return (unsigned long)ticks*12196U>>16;
}
static unsigned starttime;// Start
unsigned savecap; // erstes Echo (für Assembler non-static)
static byte csma_20ms, to_20ms;// zum Warten bis „Äther frei“ und Pause danach
// Gesamtzeit, nach 1 s erfolgloser Lückensuche
// („Äther voll“) wird der Messwert auf NaN gesetzt.
inline void startpuls() {
cli();
unsigned t = TCNT1; // Zählerstand retten und für Timeout verwenden
t0cnt = 8; // 8 Impulse per ISR abzählen lassen
DDRD|=1<<6; // Triggerschwelle nach unten ziehen (PORTD.6 = 0)
TCCR0B= 0b00001001; // Fast-PWM mit TOP==OCR0A, Vorteiler 1, starten: 8 Pulse senden
starttime = t;
sei();
}
// Möglicherweise muss man nach dem Puls warten, bis das Filter sich beruhigt hat!
inline void on20ms() {
// 1. Ist das Capture-Interruptflag gesetzt, ist entweder der Äther voll,
// oder die eigene Senderoutine hatte ein erfolgreiches Echo
// (Unklar was bei 2 Echos passiert! Ggf. ISR zum einmaligen Fangen von ICP)
byte csma = csma_20ms, to = to_20ms;
switch (csma) {
case 14: csma=0; break;
case 4: if (TIFR1&1<<5) csma=0; else startpuls(); break;
case 5: if (GPIOR0&1<<5) { // Puls kam?
GPIOR0&=~(1<<5);
l4.ustime = savecap-starttime; // Zeitbasis 1,085 µs, nach 20 ms max. 20000
to = 50; // 1 s lang gültig
}else{
TIMSK1 = 0b00000011; // kein Capture-Interrupt
l4.ustime = iInf; // Kode: Zu weit weg
}break;
default: if (csma<4 && TIFR1&1<<5) {
csma=0; // Fremdes Signal
DDRB|=1<<0; // Debug-Anzeige (kurze Lo-Nadel)
}
}
TIFR1|= 1<<5; // löschen (SBI geht für im Low-I/O-Bereich liegende Interruptflags)
DDRB&=~(1<<0); // Oszi-Anzeige löschen (generiert Lo-Hi-Flanke die nicht ausgewertet wird)
if (to && !--to) l4.ustime=iNaN;
to_20ms = to;
csma_20ms = csma+1;
}
}//namespace
// Der A/D-Wandler wird mit Vorteiler 64 betrieben
// und läuft dadurch (zufällig) mit der seriellen Schnittstelle synchron.
// Das ergibt bei kontinuierlicher Abtastung (13 ADC-Takte pro Umsetzung)
// 8,86 kSa/s = 177 Sa/20ms.
// Das mit der Referenzspannung 1,1 V funktioniert nicht,
// das Entladen des Kondensators an AREF dauert einige ms und damit zu lange.
// Bei 32 Abtastungen pro Kanal (32×5 = 160) bleiben 17 Abtastungen frei.
// Der Noise Canceler kann nicht eingesetzt werden, da CPU und Peripherie
// voll zu tun haben. Daher die Mittelung.
// Daten zur Kommunikation mit Assemblerroutine
// (Strukturname muss sein sonst ist die Struktur `static`
// und für den Assembler unsichtbar)
struct AdcCollect{
byte chan; // 0..4, Zielindex für A/D-Wert in ISR
char count; // 0..32, 32 = fertig
int a[5]; // ADC0..ADC3 und ADC8 = Chiptemperatur, garantiert 0..32736
}adcCollect NOINIT;
L4 l4 NOINIT;
void L4::adcstart() {
memset(&adcCollect,0,sizeof adcCollect);
ADMUX =0b01000000; // ratiometrisch (3,3 V), Kanal 0
ADCSRA=0b11111110; // ein, Start, kontinuierlich, Vorteiler 64, Interrupt
_delay_loop_1(64/3); // 1 ADC-Takt (9 µs) warten
ADMUX =0b01000001; // nächsten Kanal für nächste Wandlung setzen
// Die Kanalnummer ist dem aktuellen Wandlungsvorgang um 1 vorauseilend!
// In der ISR muss der Multiplexer um 2 vorauseilend gesetzt sein.
}
#if 0
// Um eine einigermaßen stabile Geschwindigkeitsanzeige zu bekommen,
// werden hier die letzten 10 Werte aufgehoben und summiert.
// Das ergibt ein einfaches Tiefpassfilter mit endlicher Impulsantwort;
// die Antwort versiegt nach 200 ms.
template<class T,byte N> class FIR{ // FIR = Finite Impulse Response
T a[N]; // Ringpuffer
byte idx; // Schreibindex, 0..N-1
public:
void push(T v) {
byte i = idx;
a[i] = v; // Ältesten Wert überschreiben
if (++i == N) i=0;
idx = i; // Umlaufender Index
}
int sum() const{
int ret=0;
for (byte i=0; i<N; i++) ret+=a[i];
return ret;
}
};
static FIR<char,10> speed_fir;
#endif
char L4::save() {
if (adcCollect.count==32) memcpy(ad,adcCollect.a,sizeof adcCollect.a);
else eedata.debug[8]++; // Ergebnisse verwerfen, da ging 'was schief
// Bei der Temperaturmessung ist der Gradient hingegen klitzeklein,
// daher wird unter „raw“ ein gesondert verrechneter Wert abgelegt.
/*Temp. Spanng Teil ADC ×16 >>2 -256
-45°C 242 mV 0,073 75 1201 300 44
0°C 288 mV 0,087 89 1429 357 101 => t0-Default
+25°C 314 mV 0,095 97 1558 389 133
+85°C 380 mV 0,115 118 1886 471 215
Steilheit (215-44)/(85°C - -45°C) = 1,315/K, ×64 = 84 => ts-Default
Zeitbetrachtung: Vorteiler 64 lt. Datenblatt erforderlich
Einzelkonvertierung (25 Takte) Kontinuierliche Konvert. (13 Takte)
mit Temperatur ohne mit Temperatur ohne Einheit
Summe 4,6 4,6 8,86 8,86 kSa/s
Summe 92 92 177 177 Sa/20ms
einzeln 18 23 35 44 Sa/20ms/K
prakt. 16 16 32 32
fertig 17,3 13,8 18,1 14,5 ms
*/
// ustime = iNaN; // (erst mal) kein Wert vom Entfernungsmesser
cli(); // Keinen Speed-Zählimpuls verpassen!
static unsigned i1ts0 NOINIT;
char t=spdCnt; spdCnt=0; // maximal 8 kann's (bei 400 Hz) werden
unsigned time = i1ts-i1ts0;
i1ts0 = i1ts;
sei();
byte to = i1to;
if (t) {
if (!to) time=0; // Uhr war abgelaufen: Zeit ungültig (TODO: Ändern auf „lange Zeit“)
to=3; // Uhr starten
}else if (to) --to; // Kein Puls? Uhr vorrücken, ggf. auf 0
i1time = time;
i1num = (byte)t;
i1to = to;
kmz+=(byte)t; // Impulse zählen
// Das Vorzeichen wird nur mit der Servostellung des Fahrservos erraten!
if (l2.pwms[1]<0) t = -t; // maximal 80 kann's (bei 400 Hz) werden
return t;
}
// Glücklicherweise liegt <adcval> garantiert im Bereich 0..32726 (1023×32)
// Liefert Hundertstelgrad für die 3 Potenziometer.
// Das ist vorteilhaft für den Regler und für die Controller-Anzeige
template<> int CalibData<11>::toLin(int v) {
if (!n) return v; // Tabelle fehlerhaft
if (n==1) return a[0].per125*100; // Tabelle fraglich
if (n>11) return iNaN; // Tabelle fehlerhaft
A*iaz = a; // Intervall-Anfangs-Zeiger
for (byte i=0; i<n-2; i++, iaz++) {// bei n≡2 nicht durchlaufen
if (v<iaz[1].adcvalue) break; // bei n≡3 an Mitte prüfen: i=0 wenn kleiner sonst 1
}
int a0 = iaz[0].adcvalue, aΔ = iaz[1].adcvalue-a0; // immer positiv
int p0 = iaz[0].per125*100, pΔ = iaz[1].per125*100-p0; // kann negativ sein
return add_sat(p0,MulDiv(pΔ,v-a0,aΔ)); // Division (32/16) unvermeidlich
}
// Für alle <N> gleich, nur die /maximale/ Arraygröße ist anders.
template<> int CalibData<3>::toLin(int v) {
if (n>3) return iNaN;
return (*reinterpret_cast<CalibData<11>*>(this)).toLin(v);
}
void L4::on20ms(char ticks) {
// Damit der Regler Nachkommastellen zum Rechnen hat erfolgt die Linearisierung
// auf Hundertstelgrad.
// Sicherlich übertrieben und bei exakter Reglerauslegung vermeidbar.
// Aber es ist genug Rechenleistung da.
lin[0]=eedata.lenkung.toLin(ad[0]);
int v=0;
if (ticks && i1time) {
// Unsigned-MulDiv spart Rechenpower?
v=MulDivU(unsigned(F_CPU/8/1000+0.5)*i1num,eedata.fz.onestep,i1time); // in 0,01 cm/s
// Divisor 8 für Vorteiler Timer1, 20 für 50 Hz, 1000 für 0,1 µm => 0,1 mm, ergibt Faktor 46,08 ≈ 46
// Die 20 entfällt, weil iltime die Zeit für die Ticks in 1/F_CPU*8 s enthält, ergibt 922
if (ticks<0) v=-v;
}
lin[1]=v;
v=ad[1]; // Bei <256 ist der Rückfahrtaster aktiv
if (v>>8) lin[2]=eedata.hubarm.toLin(v);
else if (l2.pwms[1]<0) l2.set(1,0); // killt Regler und Fahraufträge
lin[3]=eedata.löffel.toLin(ad[2]);
// Spannungsteiler 4:1 (30 kΩ und 10 kΩ) wirksam,
// dadurch wie Referenzspannung @FS (32768) = 4×3,3 V = 13,2 V
// ×100 = 2 Nachkommastellen, ×2 = Rechtsshift 16 (nicht 15) erlauben
lin[4]=ad[3]*long(13.2*100*2)>>16; // immer positv
// Temperatur in Hundertstelgrad (230222: Immer noch falsch!)
lin[5]=limit<int>(ad[4]-eedata.t0,-5500,+8500);
us::on20ms();
lin[6]=us::distance(ustime); // kein Wert vom Entfernungsmesser
}
| Detected encoding: UTF-8 | 0
|