From d2bee30dd4eda45867c4c8cb2606648a03d304b7 Mon Sep 17 00:00:00 2001 From: Oleg Borodin Date: Sun, 11 Sep 2022 00:26:27 +0200 Subject: [PATCH] update gen draft --- draft02-gen.dia | 1284 +++++++++++++++++++++++++++++++++++++++++++++++ draft02-gen.svg | 163 ++++++ mculoop/mixer.c | 60 +++ mculoop/mixer.h | 38 ++ 4 files changed, 1545 insertions(+) create mode 100644 draft02-gen.dia create mode 100644 draft02-gen.svg create mode 100644 mculoop/mixer.c create mode 100644 mculoop/mixer.h diff --git a/draft02-gen.dia b/draft02-gen.dia new file mode 100644 index 0000000..9f883a7 --- /dev/null +++ b/draft02-gen.dia @@ -0,0 +1,1284 @@ + + + + + + + + + + + + + #A4# + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + #IMU DRV# + + + + + + + + + + + + + + + + + + + + + + + + + + ## + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + #CORRELATOR# + + + + + + + + + + + + + + + + + + + + + + + + + + ## + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + #AZIMUT CALC# + + + + + + + + + + + + + + + + + + + + + + + + + + ## + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + #GPS DRV# + + + + + + + + + + + + + + + + + + + + + + + + + + ## + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + #POS CONV# + + + + + + + + + + + + + + + + + + + + + + + + + + ## + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + #NAVIGATOR# + + + + + + + + + + + + + + + + + + + + + + + + + + ## + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + #LQ FILTER# + + + + + + + + + + + + + + + + + + + + + + + + + + ## + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + #RX DRV# + + + + + + + + + + + + + + + + + + + + + + + + + + ## + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + #OUT MIXER# + + + + + + + + + + + + + + + + + + + + + + + + + + ## + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + #SERVO DRV# + + + + + + + + + + + + + + + + + + + + + + + + + + ## + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + #MOTOR DRV# + + + + + + + + + + + + + + + + + + + + + + + + + + ## + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/draft02-gen.svg b/draft02-gen.svg new file mode 100644 index 0000000..3e298e8 --- /dev/null +++ b/draft02-gen.svg @@ -0,0 +1,163 @@ + + + + + + + + IMU DRV + + + + + + + + CORRELATOR + + + + + + + + AZIMUT CALC + + + + + + + + GPS DRV + + + + + + + + + + + + + + + + + + + + + + + POS CONV + + + + + + + + + + + + + NAVIGATOR + + + + + + + + LQ FILTER + + + + + + + + + + + + + + + + + + RX DRV + + + + + + + + + + + + + OUT MIXER + + + + + + + + + + + + + SERVO DRV + + + + + + + + MOTOR DRV + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/mculoop/mixer.c b/mculoop/mixer.c new file mode 100644 index 0000000..9594151 --- /dev/null +++ b/mculoop/mixer.c @@ -0,0 +1,60 @@ +/* + * Copyright 2022 Oleg Borodin + */ + + +#include +#include + +#include + +void mixer_init(mixer_t* mix) { + for (int i = 0; i < ICOUNT; i++) { + mix->i[i] = NULL; + } + for (int i = 0; i < OCOUNT; i++) { + mix->o[i] = NULL; + } + for (int i = 0; i < RCOUNT; i++) { + mix->r[i].i = -1; + mix->r[i].o = -1; + mix->r[i].k = 0.0; + } +} + +void mixer_iset(mixer_t* mix, int n, double* i) { + mix->i[n] = i; +} + +void mixer_oset(mixer_t* mix, int n, double* o) { + mix->o[n] = o; +} + +void mixer_rset(mixer_t* mix, int n, int i, int o, double k) { + mix->r[n].i = i; + mix->r[n].o = o; + mix->r[n].k = k; +} + +void mixer_apply(mixer_t* mix) { + for (int ridx = 0; ridx < RCOUNT; ridx++) { + int oidx = mix->r[ridx].o; + if (oidx < 0) continue; + if (mix->o[oidx] == NULL) continue; + *(mix->o[oidx]) = 0.0; + } + + for (int ridx = 0; ridx < RCOUNT; ridx++) { + + int iidx = mix->r[ridx].i; + int oidx = mix->r[ridx].o; + + if (iidx < 0) continue; + if (oidx < 0) continue; + + if (mix->i[iidx] == NULL) continue; + if (mix->o[oidx] == NULL) continue; + + *(mix->o[oidx]) += *(mix->i[iidx]) * mix->r[ridx].k; + } +} diff --git a/mculoop/mixer.h b/mculoop/mixer.h new file mode 100644 index 0000000..a894e15 --- /dev/null +++ b/mculoop/mixer.h @@ -0,0 +1,38 @@ +/* + * Copyright 2022 Oleg Borodin + */ + + +#ifndef MIXER_H_QWERTY +#define MIXER_H_QWERTY + +#include +#include + + +#define ICOUNT 16 +#define OCOUNT 16 +#define RCOUNT 16 + + +typedef struct { + int i; + int o; + double k; +} rule_t; + +typedef struct { + double* i[ICOUNT]; + double* o[OCOUNT]; + + rule_t r[RCOUNT]; +} mixer_t; + + +void mixer_init(mixer_t* mix); +void mixer_iset(mixer_t* mix, int n, double* i); +void mixer_oset(mixer_t* mix, int n, double* o); +void mixer_rset(mixer_t* mix, int n, int i, int o, double k); +void mixer_apply(mixer_t* mix); + +#endif