ziggi/stm32uav
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

added mixer.*;clean olds; added lpf3, etc

Browse Source
This commit is contained in:
Oleg Borodin
2022-09-10 22:38:13 +02:00
parent f3e30f3aa7
commit 13f9b3f8de
6 changed files with 70 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
mculoop/Makefile
View File

@@ -35,6 +35,8 @@ OBJS+= geometry.o
OBJS+= madgwick.o
OBJS+= i2cdev.o
OBJS+= pidcont.o
OBJS+= mixer.o
main.elf: $(OBJS)
$(TARGET)-gcc $(^F) $(LDFLAGS) -o $@

25
mculoop/filter.c
View File

@@ -6,7 +6,7 @@
#include <math.h>
#include <filter.h>
void lpf_init(lpf_t *lpf, double freq) {
void lpf2_init(lpf2_t *lpf, double freq) {
lpf->x0 = 0.0;
lpf->x1 = 0.0;
@@ -16,7 +16,7 @@ void lpf_init(lpf_t *lpf, double freq) {
}
double lpf_apply(lpf_t *lpf, double x2, double dt) {
double lpf2_apply(lpf2_t *lpf, double x2, double dt) {
double k = dt / (lpf->rc + dt);
@@ -24,3 +24,24 @@ double lpf_apply(lpf_t *lpf, double x2, double dt) {
lpf->x0 = lpf->x0 + k * (lpf->x1 - lpf->x0);
return lpf->x0;
}
void lpf3_init(lpf3_t *lpf, double freq) {
lpf->x1 = 0.0;
lpf->x2 = 0.0;
lpf->x3 = 0.0;
double order = 2.0;
double c = 1.0 / sqrt(powf(2.0, 1.0 / order) - 1.0);
lpf->rc = 1.0 / (2.0 * c * M_PI * freq);
}
double lpf3_apply(lpf3_t *lpf, double x0, double dt) {
double k = dt / (lpf->rc + dt);
lpf->x1 = lpf->x1 + k * (x0 - lpf->x1);
lpf->x2 = lpf->x2 + k * (lpf->x1 - lpf->x2);
lpf->x3 = lpf->x3 + k * (lpf->x2 - lpf->x3);
return lpf->x3;
}

17
mculoop/filter.h
View File

@@ -10,9 +10,20 @@ typedef struct {
double x0;
double x1;
double rc;
} lpf_t;
} lpf2_t;
void lpf_init(lpf_t *lpf, double freq);
double lpf_apply(lpf_t *lpf, double x2, double dt);
void lpf2_init(lpf2_t *lpf, double freq);
double lpf2_apply(lpf2_t *lpf, double x, double dt);
typedef struct {
double x1;
double x2;
double x3;
double rc;
} lpf3_t;
void lpf3_init(lpf3_t *lpf, double freq);
double lpf3_apply(lpf3_t *lpf, double x, double dt);
#endif

98
mculoop/main.c
View File

@@ -33,11 +33,6 @@
const uint32_t g_systick_freq = 50 * 1000;
uint32_t g_sys_tick_counter;
//static void _delay(uint32_t n) {
// for (int i = 0; i < n * ; i++)
// __asm__("nop");
//}
static void clock_setup(void) {
rcc_clock_setup_pll(&rcc_hse_8mhz_3v3[RCC_CLOCK_3V3_168MHZ]);
rcc_periph_clock_enable(RCC_GPIOA);
@@ -81,9 +76,6 @@ static void i2c_setup(uint32_t i2c, uint32_t gpioport, uint32_t gpiopins) {
static void systick_setup(uint32_t systic_freq) {
g_sys_tick_counter = 0;
//gpio_mode_setup(GPIOB, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO6);
//gpio_set_output_options(GPIOB, GPIO_OTYPE_PP, GPIO_OSPEED_100MHZ, GPIO6);
systick_set_frequency(systic_freq, rcc_ahb_frequency);
systick_interrupt_enable();
systick_counter_enable();
@@ -92,7 +84,6 @@ static void systick_setup(uint32_t systic_freq) {
void sys_tick_handler(void) {
g_sys_tick_counter++;
//gpio_toggle(GPIOB, GPIO6);
}
uint32_t sys_tick_counter(void) {
@@ -108,7 +99,6 @@ uint32_t sys_tick_counter(void) {
#define PWM300 3333
#define PWM330 3030
static void timer_init(uint32_t timer) {
int prescale = rcc_ahb_frequency / (2 * 1000 * 1000) - 1;
@@ -154,29 +144,6 @@ static void timer_gpio_setup(uint32_t gpio_port, uint32_t gpio_af, uint32_t gpio
}
//typedef struct {
// double k;
// double l;
//} lpf_t;
//void lpf_init(lpf_t* lpf, double k) {
// lpf->k = k;
// lpf->l = 0;
//}
//double simplp (double *x, double *y, int M, double xm1) {
// int n;
// y[0] = x[0] + xm1;
// for (n = 1; n < M ; n++) {
// y[n] = x[n] + x[n-1];
// }
// return x[M-1];
//}
typedef struct {
double start;
double freq;
@@ -187,7 +154,7 @@ void systimer_init(systimer_t* timer, double freq, double start) {
timer->freq = freq;
}
double systimer_next(systimer_t* timer, double timestamp) {
double systimer_apply(systimer_t* timer, double timestamp) {
double difftime = (timestamp - timer->start) / timer->freq;
timer->start = timestamp;
return difftime;
@@ -219,7 +186,6 @@ int main(void) {
tc_setratio(timer, TIM_OC3, 50);
tc_setratio(timer, TIM_OC4, 70);
imu_t imu;
imu_setup(&imu, I2C1, 0x68);
imu_calibrate(&imu, 100);
@@ -230,6 +196,26 @@ int main(void) {
quaternion_t q;
quaternion_init(&q);
double ak = 50.0;
lpf3_t lpfax;
lpf3_t lpfay;
lpf3_t lpfaz;
lpf3_init(&lpfax, ak);
lpf3_init(&lpfay, ak);
lpf3_init(&lpfaz, ak);
lpf3_t lpfgx;
lpf3_t lpfgy;
lpf3_t lpfgz;
lpf3_init(&lpfgx, ak);
lpf3_init(&lpfgy, ak);
lpf3_init(&lpfgz, ak);
systimer_t systimer;
systimer_init(&systimer, (double)g_systick_freq, (double)g_sys_tick_counter);
pidcont_t p;
pidcont_init(&p);
@@ -238,39 +224,18 @@ int main(void) {
pidcont_setup(&p, kp, ki, 0);
double ak = 50.0;
lpf_t lpfax;
lpf_t lpfay;
lpf_t lpfaz;
lpf_init(&lpfax, ak);
lpf_init(&lpfay, ak);
lpf_init(&lpfaz, ak);
lpf_t lpfgx;
lpf_t lpfgy;
lpf_t lpfgz;
lpf_init(&lpfgx, ak);
lpf_init(&lpfgy, ak);
lpf_init(&lpfgz, ak);
systimer_t systimer;
systimer_init(&systimer, (double)g_systick_freq, (double)g_sys_tick_counter);
while (true) {
imu_getvec(&imu, &mval);
double dt = systimer_next(&systimer, g_sys_tick_counter);
double dt = systimer_apply(&systimer, g_sys_tick_counter);
mval.ax = lpf_apply(&lpfax, mval.ax, dt);
mval.ay = lpf_apply(&lpfay, mval.ay, dt);
mval.az = lpf_apply(&lpfaz, mval.az, dt);
mval.ax = lpf3_apply(&lpfax, mval.ax, dt);
mval.ay = lpf3_apply(&lpfay, mval.ay, dt);
mval.az = lpf3_apply(&lpfaz, mval.az, dt);
mval.gx = lpf_apply(&lpfgx, mval.gx, dt);
mval.gy = lpf_apply(&lpfgy, mval.gy, dt);
mval.gz = lpf_apply(&lpfgz, mval.gz, dt);
mval.gx = lpf3_apply(&lpfgx, mval.gx, dt);
mval.gy = lpf3_apply(&lpfgy, mval.gy, dt);
mval.gz = lpf3_apply(&lpfgz, mval.gz, dt);
madgwick(dt, &q, &mval);
@@ -292,12 +257,7 @@ int main(void) {
printf("dt=%.6f %lu <-pitch=%8.3f roll=%8.3f yaw=%8.3f\r\n", dt, out1, a.y, a.x, a.z);
//double out = pidcont_next(&p, 0, a.pitch, dt);
//double out = pidcont_apply(&p, 0, a.pitch, dt);
//printf("dt=%.6f pitch=%8.3f out=%10.3f %10.6f \r\n", dt, a.pitch, out, p.integ);
//double b2 = 0.0;
//double K = 0.01;
//b2 = b2*(1 - K) + b*K;
};
}

3
mculoop/pidcont.c
View File

@@ -23,8 +23,7 @@ void pidcont_setup(pidcont_t* p, double kp, double ki, double kd) {
p->kd = kd;
}
double pidcont_next(pidcont_t* p, double target, double actual, double dt) {
double pidcont_apply(pidcont_t* p, double target, double actual, double dt) {
double error = 0.0;
double deriv = 0.0;

2
mculoop/pidcont.h
View File

@@ -20,6 +20,6 @@ typedef struct {
void pidcont_init(pidcont_t* p);
void pidcont_setup(pidcont_t* p, double kp, double ki, double kd);
double pidcont_next(pidcont_t* p, double target, double actual, double dt);
double pidcont_apply(pidcont_t* p, double target, double actual, double dt);
#endif