Source file: /~heha/mb-iwp/Bergwerk/fba-fw-230825.zip/fba.h

#pragma once
#include <avr/io.h>
#include <avr/interrupt.h>// cli, sei
#include <string.h>	// memset

typedef unsigned char byte;
#define nobreak [[fallthrough]]
#define elemof(x) (sizeof(x)/sizeof(*(x)))
#define NOINIT __attribute__((section(".noinit")))

// 4 Register für den C-Compiler sperren
register unsigned saveR24 asm("r2");
register unsigned saveR30 asm("r4");

#define t0cnt GPIOR1	// zum Zählen der Pulse an OC0B
#define spdCnt GPIOR2	// Impulszähler an INT1

/**************************
 * Schicht 0: Echtzeituhr *
 **************************/

extern struct Uhr{	// sizeof = 6
 byte	  ms10;	// 10-ms-Schritte (0..99)
 uint32_t losec;// Sekunden (typisch seit 1.1.1970 0:00)
 char	  hisec;// Platz für Überlauf sowie für Prähistorie
 void operator++();	// 50 Hz zählen (Prä-Inkrement)
 operator byte*() {return (byte*)this;}
 void reset() {memset(this,0,sizeof*this);}
}uhr;
static_assert(sizeof uhr==6);
// Die 5 Bytes für ganze Sekunden genügt für die Kalenderjahre -15462 .. 19402
// 4 Bytes (vzb.) hingegen reichen nur bis zum 19. Januar 2038.

/*********************
 * Schicht 1: EEPROM *
 *********************/

template<byte N>struct CalibData{
 byte n;		// mindestens 2, maximal N
 struct A{		// muss Array Of Struct sein für variable N
  int16_t adcvalue;	// streng monoton steigend!
  int8_t per125;	// monoton steigend oder fallend
 }a[N];			// Muss letztes Element von CalibData sein
 int16_t toLin(int16_t);// Wandelt A/D-Wert in Gradangabe (Hundertstelgrad)
 inline A const&first() const {return a[0];}
 inline A const&last() const {return a[n-1];}
 inline char min() const {char a=first().per125, b=last().per125; return a<b?a:b;}
 inline char max() const {char a=first().per125, b=last().per125; return a>b?a:b;}
 static_assert(N>=2);
};

extern struct Eedata{	// Gesamtlänge 0x70 = 112
 byte lamps;		// [1]  @ 0
 byte i2caddr;		// [1]  @ 1
 CalibData<11>lenkung;	// [34] @ 2
 CalibData<3> hubarm,	// [10] @24
	      löffel;	// [10] @2E
 int t0,rsv[3];		// [8]  @38 0-°C-Wert für Temperatursensor; frei
 struct PID{		// [32] @40	// Alle Werte doppelt, für Minus- und Plus-Abweichung
  char	Kp[2],		// Proportional-Faktor	(minus-plus)
	Ki[2],		// Integrier-Faktor	(minus-plus)
	Kd[2],		// Differenzier-Faktor	(minus-plus)
	Z[4],		// Spreizung der Hydraulik (minus-plus Losreißen, minus-plus InFahrt)
	L[4];		// Limit für Integrier-Word (min-max-minus, min-max-plus)
  byte	T,		// Maximalzeit in 1/10 s, 0 = unendlich (für Fahrtregler)
	M;		// Schwellwert zum Erkennen des Losreißens
 }pid[4];		// Reglerdaten für Geschwindigkeit und Hydrauliken
 struct Fz{		//	@80
  char sp[2];		// [2]	@80 Maximalgeschwindigkeit rückwärts/vorwärts in cm/s
  unsigned onestep;	// [2]  @82 Fahrweite in 0,1 µm für 1 Motordrehimpuls
  unsigned q[4];	// [8]  @84 Winkeländerung in π*2^-23 für 10°, 20°, 30° und maximalen Lenkwinkel
  int rsv[2];		// [4]	@8C
 }fz;			// Fahrzeugkonstanten
 byte debug[32];	// [32] @90
	// Debug-Zähler
	// 0	Power-On-Reset (Einschalten)
	// 1	Externes Reset (vom Programmiergerät)
	// 2	Brown-Out-Reset (Spannung nicht ganz weg)
	// 3	Watchdog-Reset (nie)
	// 4	RxD: Falsche Prüfsumme
	// 5	RxD: Zeitlücke detektiert (erstmal kein Fehler)
	// 6	RxD: Pufferüberlauf
	// 7	RxD: Falsches Längenbyte (<4 oder >32)
	// 8	ADC: Nicht früh genug fertig (nie)
	// 9	PWM: Zeitproblem in ISR (Murks! Tritt auf! Interrupts >400µs gesperrt!?)
	// 10	PWM: Letztes pwmidx
	// 11	Regler: Notbremse vom Lenkregler
	// 12	I²C: Unverarbeiteter Status
	// 13	I²C: TWSR beim letzten unerwarteten Status
	// 14	I²C: „SCL Low Timeout“: I²C-Neustart (Tritt auf bei Taktfehlern vom RPi auf.)
	// 15	I²C: Status == 0xF8 (Dummy)
	// 16	I²C: Lesedaten unvollständig gelesen (ungenutzt)
	// 17	I²C: Antwortpuffer leer oder EEPROM nicht bereit: Sende 0xFF
	// 18	I²C: Schreibpuffer voll
 inline void read();	// Statische Konstruktoren fressen mehr Flash
 void update();
 operator byte*() {return (byte*)this;}
}eedata;
static_assert(sizeof eedata==176);	// 230418: Umstrukturierung!
#define eedebug (eedata+0x90)

/* Ungefähre Fahrzeugkennwerte (Radlader):
 * Zählimpulse pro Motorumdrehung: 4? (max. 400 Hz)
 * Erste Getriebeübersetzung: 4:1
 * Differenzialgetriebeübersetzung: 10:1
 * Raddurchmesser: 140 mm, Umfang 440 mm
*/

/****************************
 * Schicht 2: PWM-Generator *
 ****************************/
// Mit dem '4017 kommt man (bei gleich bleibender Wiederholrate von 50 Hz)
// nur auf 9 Kanäle; der zehnte Ausgang „schluckt“ den Rest der Zeit.
// Alle (auch die negativen) Flanken werden per Mikrocontroller-Hardware generiert
// und in der ISR von Flanke zu Flanke vorberechnet.
// Dafür muss die maximale Interruptsperrzeit unter 500 µs liegen.

template<class T> static constexpr T limit(T c, T minv, T maxv) {
 return c<minv ? minv : c>maxv ? maxv : c;
}
template<class T> static constexpr T limit(T c, T maxv) {
 return limit<T>(c,-maxv,maxv);
}

extern struct L2{
 char pwms[9];
 byte pwmok;	// PWM generieren solange !=0, dekrementiert mit 1 Hz
		// … von 190 bis 10, dann mit 50 Hz
 static void countdown();
 enum{		// Flags (High-Nibble) für Kanalbyte (Low-Nibble)
  fAdd  =0x10,	// Addieren statt setzen (absichtlich Bit 4!)
  fRegler=0x20,	// Vom Regler: Regler nicht abschalten (Nur für Kanalindex 0..3)
  bSlow =6,	// Bit 7~6: Stellgeschwindigkeit begrenzen (Servomotoren schonen)
 };		// 0b00 = unbegrenzt, 0b01 = 5/20ms, 0b10 = 10/20ms, 0b11 = 20/20ms
 void set(byte,char);	// setzt Kanal auf Wert, cNaN übergehend,
// und setzt pwmok auf 10+60×3 = 190 (3 Minuten; Totmann-Funktion)
// Wird aufgerufen von:
// * Original-Controller: L3::onIbusRx() -> L3::toServo() (nur bei Änderung)
// * Regler
// * I²C-Bus
// * Pampelmuse
// * A/D-Wandler (nur auf Kanal 1 = Fahrmotor wirkend):
//	- Bei Rückwärtsfahrt und Rückfahrtaster-Kontakt: Stopp
//	- Bei Vorwärtsfahrt und Ultraschall-Entfernung < 5 cm: Stopp
// Bei aktivem Notaus passiert nichts!
 static void t1init();	// in isr.S

// Nachträglich eingebaut wurde der Impulsgenerator und die Pulslängenerfassung
// zum Tracking des Magic Controllers, d.h. der Schaltzustände von Licht und Zündung
// Während der Rundumleuchte ein Einzelpuls zum Weiterschalten genügt,
// benötigt der Magic Controller eine „Mindestlänge“ = einige kurze (1 ms)
// bzw. lange (2 ms) Pulse für den Schaltvorgang.
// Dazu, am Kanal 7, eine „lange Mindestlänge“ für die Blinker-Schaltebene.
// Gesteuert wird das (via SPI oder I²C) über eine Hintertür im Level 4.
 static byte k7hold;
 static void k7hold_handler();
 static byte autopuls[6]; // Automatischer Pulsgenerator für alles was „zählt“
	// 0 Kanal 6- (Zündung)
	// 1 Kanal 7- (Licht Fahrzeug)
	// 2 Kanal 7+ (Licht Fahrerhaus (am Muldenkipper nichts zu sehen))
	// 3 Kanal 7-- (lang) (Blinkmodus)
	// 4 Kanal 7++ (lang) (Blinken ein/aus (am Radlader nur Geräusch))
	// 5 Kanal 8+ (Rundumleuchte)
 static void autopuls_handler();	// Aufruf alle 20 ms von Hauptschleife
}l2;

static_assert(sizeof l2==10);

namespace notaus{
inline void force() {	// Aufruf bei Notaus von Pampelmuse oder I²C
 PORTD&=~(1<<2);
 DDRD |=  1<<2;	// Wired-And am Not-Aus-Eingang
}
inline bool forced() {
 return DDRD&1<<2;
}
inline void release() {// Aufruf bei … Signal von Fernbedienung?? Nee!
 DDRD &=~(1<<2);// Wired-And „enteisen“
 PORTD|=  1<<2;
}
inline bool released() {
 return PIND&1<<2;
}
}//namespace

/****************************
 * Schicht 3: Fernbedienung *
 ****************************/
// Der Controller unterstützt (sendet) 10 Kanäle und hat:
// - 6 Analog-Geber (2 Sticks und 2 Drehräder),
// - 2 Tasten (Unterseite)
// - 2 Schalter mit 2 Stellungen
// - 2 Schalter mit 3 Stellungen
extern struct L3{
 char values[10];
 L3();
// static inline unsigned checksum(const byte*);
// static inline bool set_checksum(byte*);
 static inline void onSensorQuery();
 void checkIbusRx();
 void toServo() const;	// „kopiert“ l3 nach l2 einzelbyteweise
 static byte fbaus;	// Fernbedienung unwirksam („geraubt“) wenn !=0
}l3;

static_assert(sizeof l3==10);

/********************************************************
 * Schicht 4: A/D-Wandler, Impulszähler, Linearisierung *
 ********************************************************/

// Futter für I²C-Adresse 0xB6/0xB7: I²C-Daten Level 3
extern struct L4{
 int16_t lin[7];// Lenkung in 1/100°,
		// Fahrgeschwindigkeit in 0,1 mm/s (Anzeige cm/s),
		// Hubarm/Ladefläche in 1/100°,
		// Löffel/Verwindung in 1/100°,
		// Traktionsakkuspannung in 1/100 V,
		// Chiptemperatur in 1/100 °C,
		// Ultraschallsensor in 0,1mm (Anzeige cm)
 byte flags;	// 0	Anstoß-Schalter [Nur Muldenkipper]
		// 2:1	0 Kein Not-Aus
		//	1 Not-Aus vom Schalter
		//	2 Not-Aus von Pampelmuse
		//	3 Not-Aus vom I²C-Controller
		// 3	Zündung ein/aus (Kanal 6-) 0..1
		// 4	Blinken (Kanal 7+ lang) 0..1
		//	[Radlader: Nur Geräusch. Muldenkipper: Blinklichter links+rechts]
		// 7:5	Status der Rundumleuchte (Kanal 8+)
		//	0 aus
		//	1 normale Rundum-Funktion (beim Einschalten)
		//	2 langsam rundum
		//	3 blinkend
		//	4 blitzend
		//	5..7 sollte nicht vorkommen
 byte lamps;	// Bit 0..2: Lampenstatus Fahrzeug (Kanal 7-) 0..5
		// Bit 3..4: Blinkstatus der Fahrzeuglampen (Kanal 7- lang) 0..2
		// Bit 5..7: Lampenstatus Führerhaus (Kanal 7+) 0..4
// Der Magic Controller merkt sich (leider) den Schaltzustand,
// daher Mitführung im EEPROM
 uint16_t ad[5],// ADC0: Poti
		// ADC1: Poti
		// ADC2: Poti
		// ADC3: Traktionsakkuspannung
		// ADC8: Temperatur
	ustime,	// Zeit vom Ultraschallsensor
	i1time;	// Differenzzeit der Capturewerte vom INT1
 byte	i1num,	// Anzahl INT1
	i1to;	// Timeout (3..0 à 20 ms) für brauchbaren Capturewert
 uint32_t kmz;	// Kilometerzähler in Impulsen vom Motor
 static void adcstart();
 char save();	// liefert vzb. Tachoticks
 void on20ms(char);
}l4;

static_assert(sizeof l4==36);

// Im Gegensatz zu #define ohne Seiteneffekte (Mehrfachberechnung von x),
// so nicht ganz korrekt für Gleitkommazahlen (-0 wird nicht zu +0) => fabs()
template<class T> static constexpr T abs(T x) {
 return x<0 ? -x : x;
}

inline byte cabs(char x) {asm volatile(
"	sbrc	%0,7	\n"
"	 neg	%0	\n"
:"+r"(x));
 return x;
}
/*
inline char cadd_sat(char a, char b) {
 a+=b;
 asm("	brvc	1f	\n"	// ohne Überlauf nichts tun
"	ldi	%0,0x80	\n"	// lade mit iNaN, Begrenzung später
"	brcs	1f	\n"
"	com	%0	\n"	// lade mit iInf
"1:			\n"
:"+d"(a));
 return a;
}
*/
inline int add_sat(int a, int b) {
 a+=b;
 asm(	// Solche Tricksereien wirklich nur nach „+=“-Operator!
"	brvc	2f	\n"	// ohne Überlauf nichts tun
"	ldi	%A0,0	\n"
"	ldi	%B0,0x80\n"	// lade mit iNaN, Begrenzung später
"	brcs	1f	\n"
"	com	%A0	\n"
"	com	%B0	\n"	// lade mit iInf
"1:	lds	r0,%1	\n"
"	inc	r0	\n"
"	sts	%1,r0	\n"
"2:			\n"
:"+d"(a):"i"(eedebug+19));
 return a;
}

inline long add_sat(long a, long b) {
 a+=b;
 asm("	brvc	2f	\n"	// ohne Überlauf nichts tun
"	ldi	%A0,0	\n"
"	ldi	%B0,0	\n"
"	ldi	%C0,0	\n"
"	ldi	%D0,0x80\n"	// lade mit lNaN, Begrenzung später
"	brcs	1f	\n"
"	com	%A0	\n"
"	com	%B0	\n"
"	com	%C0	\n"
"	com	%D0	\n"	// lade mit +lInf
"1:	lds	r0,%1	\n"
"	inc	r0	\n"
"	sts	%1,r0	\n"
"2:			\n"
:"+d"(a):"i"(eedebug+19));
 return a;
}

// Kleine Divisionsroutine ÷100 für <a> im Bereich ±12700 mit Rundung
// 90 Takte
inline char idiv100r(int a) {
 byte r=8;
 asm(
"	tst	%B0	\n"
"	clt		\n"
"	brpl	1f	\n"
"	com	%B0	\n"	// Komplement statt Negation führt zu Rundung gegen +∞
"	com	%A0	\n"	// 50 => 1, -50 => 49 => 0
"	set		\n"
"1:	subi	%A0,lo8(-50)	\n"
"	sbci	%B0,hi8(-50)	\n"	// Mit halbem Divisor aufrunden
"0:	lsl	%A0	\n"	// Null einschieben
"	rol	%B0	\n"
"	brcs	1f	\n"	// Hier niemals Carry
"	cpi	%B0,100	\n"
"	brcs	2f	\n"
"1:	subi	%B0,100	\n"
"	inc	%A0	\n"	// Low-Teil = Divisionsergebnis, High-Teil = Rest
"2:	dec	%1	\n"
"	brne	0b	\n"
"	brtc	1f	\n"
"	neg	%A0	\n"	// Ergebnisbyte negieren
"1:			\n"
:"+d"(a):"r"(r));
 return a;	// Rest verwerfen
}

inline int MulDiv(int a, int b, int c) {
 return (long)a*b/c;	// Achtung! Division rundet Richtung 0!
}
inline unsigned MulDivU(unsigned a, unsigned b, unsigned c) {
 return a*(unsigned long)b/c;	// Auch diese Division rundet Richtung 0!
}
template<byte N>inline int MulShr(int a, int b) {
 return (long)a*b>>N;	// Achtung! Rundet Richtung -∞, also wie floor()!
}
template<byte N>inline int MulShrR(int a, int b) {
 return (long)a*b+(1<<N-1)>>N;
}	// halben Shiftwert vorher addieren, das rundet „richtig“.
// Beim Barrelshifter in ASM wäre das ein "adc 0" danach.

// 6x rechtsschieben und runden; hier in 11 Takten
inline int shr6r(int a) {
 asm(				// %B0	    %A0      r0       C
"	mov	r0,%A0	\n"	// fefcba98 76543210 76543210
"	mov	%A0,%B0	\n"	// fefcba98 fedcba98 76543210
"	rol	r0	\n"	// fefcba98 fedcba98 6543210x 7
"	rol	%A0	\n"	// fefcba98 edcba987 6543210x f
"	sbc	%B0,%B0	\n"	// ffffffff edcba987 6543210x
"	rol	r0	\n"	// ffffffff edcba987 543210xx 6
"	rol	%A0	\n"	// ffffffff dcba9876 543210xx e
"	rol	%B0	\n"	// fffffffe dcba9876 543210xx
"	rol	r0	\n"	// fffffffe dcba9876 43210xxx 5
"	adc	%A0,r1	\n"	// inkrementiere wenn Bit 5 = 1
"	adc	%B0,r1	\n"
:"+d"(a));
 return a;
}

// NEU 230321: Geschwindigkeitsmessung mittels Pulsabstandsmessung;
// Trigger nur noch auf 1 Flanke, Nutzung von umlaufendem Timer1
extern unsigned i1ts;

/***********
 * i2c.cpp *
 ***********/
class I2C{
public:
 static void init();
 static void poll();
};

/**********************************
 * Schicht 5: Regler (regler.cpp) *
 **********************************/

constexpr int8_t  cInf = 0x7F,		cNaN = 0x80;
constexpr int16_t iInf = 0x7FFF,	iNaN = 0x8000;
constexpr int32_t lInf = 0x7FFFFFFF,	lNaN = 0x80000000;

int limitInt(long);

// I²C-Daten Level 5
extern struct Regler{
 char so;	// Regler-Soll (in ganzen Grad bzw. cm/s)
 struct F{
  byte vmax:7;	// Regler-Speed = maximaler Ausgabewert für PWM vor Spreizung, 0 = 125
  bool losgerissen:1;
//  operator byte&() {return *(byte*)this;}
  F(byte v, bool b):vmax(v),losgerissen(b) {}
 }f;
 unsigned tout;	// in 20 ms
 int eΣ;	// Summierter Fehler
 int eΩ;	// Letzter Fehler (gefiltert wegen Rauschen)
 char neu_soll(int);
 void reset(byte to=0,bool cont=false);	// Restart für to!=0
 void periodic();
 static void on20ms();	// Aufruf alle 20 ms
 enum{
  fAdd=0x10,	// Addition auf Regelziel, wenn aktiv, sonst auf Istwert
 };		// Regler startet nicht wenn Istwert bereits erreicht (!)
 static bool set(byte,char,char);	// liefert true wenn Regelziel erreicht
 Regler():so(cNaN),f(125,false) {}	// Scheint auch im Array korrekt initialisiert zu werden!
 struct LogItem{
  int e,Σ,Δ,r;
 };
 static struct Log{	// Wenn mehrere Regler gleichzeitig arbeiten gibt's Kuddelmuddel.
  byte writeidx,readidx;
  LogItem items[16];	// ausreichend für 300 ms
  LogItem& current() {return items[writeidx];}
  void savecurrent();
  byte emit(byte*,byte=60);	// max. 60 Byte zum I²C-Puffer ausspucken
 }log;
}regler[4];

static_assert(sizeof regler==8*4);

/***************************************************************
 * Schicht 6: Odometrie (v.a. zur Funktionskontrolle, odo.cpp) *
 ***************************************************************/
extern struct Odo{
 long x,y;	// Position des Rechenpunktes im Koordinatensystem (Linkssystem), Einheit 0,1 µm
 long α;	// Winkellage des (geradeausgelenkten) Fahrzeugs, umlaufend, 24 Bit = 360°
 int q;		// 1/Kurvenradius: Addierglied für α
 int curλ;
 void on20ms(char n);	// Aufruf alle 20 ms
 void lenkTo(int λ);	// ermittelt q durch Interpolation aus eedata.fz.q
 static long lsin(long w,int α);
 static long lcos(long w,int α) {return lsin(w,α+16384);}
}odo;

static_assert(sizeof odo==16);

/*******************************************************************
 * Schicht 7: Koordinierte, geregelte Mehrfachbewegung (koord.cpp) *
 *******************************************************************/
struct L7{	// Beschreibt 1 Segment
 int8_t soll[4];	// Sollwerte der 4 Regler (cNaN = belassen)
 int8_t speed[4];	// Geschwindigkeiten für die Regler
 long s;	// Fahrstrecke in Ticks; 0 = keine Strecke
 unsigned w;	// Warten in 20 ms
 static L7*prepare() __attribute__((warn_unused_result));
 static void push();
// static void purge();	// Alle Aufträge löschen
 static void purge(byte);	// Im aktuellen Auftrag kanalspezifisch Auftrag beenden
 static void on20ms(char);
 static const byte&füllstand() {return fill;}
 static bool full();
private:
 static byte fill;
 static byte ri;
 static byte wi();
 static void eat();	// Ältesten (aktuell verarbeiteten) Auftrag löschen
 static unsigned lastuv;	// Letzte vorzeichenlose Geschwindigkeit
};
// Damit kann man während der Fahrt
// bremsen/beschleunigen/Kurven fahren/Hubarm/Löffel betätigen
// und (mit mathematischer Analyse) eine Zielfahrt vornehmen

/*******************
 * Schicht 9: Load *
 *******************/

struct Load;

extern struct Loadavg{
 struct LoadLong{
  byte min, max;	// Minimum wird nicht benutzt
  unsigned cnt;
  unsigned long time;
  void on20ms(Load&);
  inline void on20ms(volatile Load&);
 }isr,sleep;
 unsigned t1l,t1h;	// Gesamt-Takte
 void on20ms();
 void prepare();	// vor Abholen
 void reset();		// nach Abholen via I²C oder SPI+RFM69
}loadavg;
static_assert(sizeof loadavg==20);
Detected encoding: UTF-80