mainloop/* moved to root
This commit is contained in:
@@ -1,91 +0,0 @@
|
||||
#
|
||||
# Copyright: Oleg Borodin <onborodin@gmail.com> 2018
|
||||
#
|
||||
|
||||
.SECONDARY:
|
||||
|
||||
CFLAGS+= -I. -O2 -DSTM32F4 #-std=c99
|
||||
CFLAGS+= -mfloat-abi=hard
|
||||
CFLAGS+= -mcpu=cortex-m4
|
||||
CFLAGS+= -mthumb
|
||||
CFLAGS+= -fno-common -ffunction-sections -fdata-sections
|
||||
CFLAGS+= -g -gdwarf-2
|
||||
CFLAGS+= -Wall
|
||||
|
||||
|
||||
LDFLAGS+= ${CFLAGS}
|
||||
LDFLAGS+= --static
|
||||
#LDFLAGS+= -nostartfiles
|
||||
LDFLAGS+= -T main.ld
|
||||
|
||||
LDFLAGS+= -Wl,-Map=main.map
|
||||
LDFLAGS+= -Wl,--cref -Wl,--gc-sections
|
||||
LDFLAGS+= -lopencm3_stm32f4
|
||||
LDFLAGS+= -Wl,--start-group -lc -lm -lgcc -lnosys -Wl,--end-group
|
||||
|
||||
TARGET= arm-eabi
|
||||
|
||||
all: main.bin
|
||||
|
||||
flow:
|
||||
cflow --tree --brief --no-number --omit-arguments *.c
|
||||
|
||||
OBJS+= main.o
|
||||
OBJS+= syscall.o
|
||||
OBJS+= usartu.o
|
||||
OBJS+= mpu6050.o
|
||||
OBJS+= geometry.o
|
||||
OBJS+= i2cdev.o
|
||||
OBJS+= pidcont.o
|
||||
OBJS+= mixer.o
|
||||
OBJS+= filter.o
|
||||
OBJS+= misc.o
|
||||
|
||||
|
||||
main.elf: $(OBJS)
|
||||
$(TARGET)-gcc $(^F) $(LDFLAGS) -o $@
|
||||
$(TARGET)-size --format=berkeley $@
|
||||
|
||||
%.o: %.c
|
||||
$(TARGET)-gcc $(CFLAGS) -c -o $@ $<
|
||||
|
||||
%.o: %.S
|
||||
$(TARGET)-gcc $(CFLAGS) -c -o $@ $<
|
||||
|
||||
%.bin: %.elf
|
||||
$(TARGET)-objcopy -O binary $< $@
|
||||
|
||||
%.elf: %.o
|
||||
$(TARGET)-gcc $(^F) $(LDFLAGS) -o $@
|
||||
$(TARGET)-size --format=berkeley $@
|
||||
|
||||
clean:
|
||||
rm -f *.i *.o *.elf *.bin *.map *~ *.hex *.d *.s
|
||||
|
||||
flash: main.bin
|
||||
@openocd \
|
||||
-c 'puts "--- START --------------------"' \
|
||||
-f 'interface/stlink.cfg' \
|
||||
-f 'target/stm32f4x.cfg' \
|
||||
-c 'puts "--- INIT --------------------"' \
|
||||
-c "init" \
|
||||
-c "reset halt" \
|
||||
-c 'puts "--- WRITE --------------------"' \
|
||||
-c "flash write_image erase $< 0x08000000"\
|
||||
-c 'puts "--- VERIFY --------------------"' \
|
||||
-c "verify_image $<" \
|
||||
-c 'puts "--- RESET --------------------"' \
|
||||
-c "reset" \
|
||||
-c 'puts "--- DONE --------------------"' \
|
||||
-c "shutdown"
|
||||
|
||||
debug: main.bin
|
||||
@openocd \
|
||||
-c 'puts "--- START --------------------"' \
|
||||
-f 'interface/stlink.cfg' \
|
||||
-f 'target/stm32f4x.cfg' \
|
||||
-c 'puts "--- INIT --------------------"' \
|
||||
-c "init" \
|
||||
-c "halt" \
|
||||
-c "poll"
|
||||
#EOF
|
||||
-21611
File diff suppressed because it is too large
Load Diff
@@ -1,47 +0,0 @@
|
||||
/*
|
||||
* Copyright 2022 Oleg Borodin <borodin@unix7.org>
|
||||
*/
|
||||
|
||||
|
||||
#include <math.h>
|
||||
#include <filter.h>
|
||||
|
||||
void lpf2_init(lpf2_t *lpf, double freq) {
|
||||
lpf->x0 = 0.0;
|
||||
lpf->x1 = 0.0;
|
||||
|
||||
double order = 1.5;
|
||||
double n = 1 / sqrt(pow(2, 1.0 / order) - 1);
|
||||
lpf->rc = 1 / (2 * n * M_PI * freq);
|
||||
}
|
||||
|
||||
|
||||
double lpf2_apply(lpf2_t *lpf, double x2, double dt) {
|
||||
|
||||
double k = dt / (lpf->rc + dt);
|
||||
|
||||
lpf->x1 = lpf->x1 + k * (x2 - lpf->x1);
|
||||
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;
|
||||
}
|
||||
@@ -1,29 +0,0 @@
|
||||
/*
|
||||
* Copyright 2022 Oleg Borodin <borodin@unix7.org>
|
||||
*/
|
||||
|
||||
#ifndef FILTER_H_QWERTY
|
||||
#define FILTER_H_QWERTY
|
||||
|
||||
|
||||
typedef struct {
|
||||
double x0;
|
||||
double x1;
|
||||
double rc;
|
||||
} lpf2_t;
|
||||
|
||||
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
|
||||
@@ -1,151 +0,0 @@
|
||||
/*
|
||||
* Copyright 2022 Oleg Borodin <borodin@unix7.org>
|
||||
*/
|
||||
|
||||
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#include <geometry.h>
|
||||
|
||||
void eulerangle_init(eulerangle_t* a) {
|
||||
a->z = 0.0;
|
||||
a->y = 0.0;
|
||||
a->x = 0.0;
|
||||
}
|
||||
|
||||
void eulerangle_todegress(eulerangle_t* a) {
|
||||
a->z *= (180.0 / M_PI);
|
||||
a->y *= (180.0 / M_PI);
|
||||
a->x *= (180.0 / M_PI);
|
||||
}
|
||||
|
||||
void eulerangle_toradians(eulerangle_t* a) {
|
||||
a->z *= (M_PI / 180.0);
|
||||
a->y *= (M_PI / 180.0);
|
||||
a->x *= (M_PI / 180.0);
|
||||
}
|
||||
|
||||
|
||||
void eulerangle_norm(eulerangle_t* a) {
|
||||
double n = sqrt(a->x*a->x + a->y*a->y + a->z*a->z);
|
||||
a->x *= n;
|
||||
a->y *= n;
|
||||
a->z *= n;
|
||||
}
|
||||
|
||||
void quaternion_init(quaternion_t* q) {
|
||||
q->w = 1.0;
|
||||
q->x = 0.0;
|
||||
q->y = 0.0;
|
||||
q->z = 0.0;
|
||||
}
|
||||
|
||||
void quaternion_toeuler(quaternion_t* q, eulerangle_t* a) {
|
||||
|
||||
double x = q->x;
|
||||
double y = q->y;
|
||||
double z = q->z;
|
||||
double w = q->w;
|
||||
|
||||
double ax = 0.0;
|
||||
double ay = 0.0;
|
||||
double az = 0.0;
|
||||
|
||||
double t0 = (x + z)*(x - z); // x^2-z^2
|
||||
double t1 = (w + y)*(w - y); // w^2-y^2
|
||||
|
||||
double n1 = 0.5 * (t0 + t1); // 1/2 x of x'
|
||||
double n2 = x*y + w*z; // 1/2 y of x'
|
||||
double n3 = w*y - x*z; // 1/2 z of x'
|
||||
|
||||
double t = n1*n1 + n2*n2; // cos(theta)^2
|
||||
double n4 = 2.0 * (y*z + w*x); // z of y'
|
||||
|
||||
az = atan2(n2, n1); // yaw (psi)
|
||||
ay = atan(n3 / sqrt(t)); // pitch (theta)
|
||||
|
||||
if (t != 0.0) { // roll
|
||||
ax = atan2(n4, t1 - t0);
|
||||
} else {
|
||||
ax = (2.0 * atan2(x, w) - copysign(1.0, n3) * az);
|
||||
}
|
||||
|
||||
a->x = ax;
|
||||
a->y = ay;
|
||||
a->z = az;
|
||||
}
|
||||
|
||||
|
||||
void quaternion_madgwick(quaternion_t* q, imuvec_t* m, double dt) {
|
||||
|
||||
double q0 = q->w;
|
||||
double q1 = q->x;
|
||||
double q2 = q->y;
|
||||
double q3 = q->z; // quaternion of sensor frame relative to auxiliary frame
|
||||
|
||||
double gx = m->gx;
|
||||
double gy = m->gy;
|
||||
double gz = m->gz;
|
||||
|
||||
double ax = m->ax;
|
||||
double ay = m->ay;
|
||||
double az = m->az;
|
||||
|
||||
double beta = 0.1f; // 2 * proportional gain (Kp)
|
||||
double recipNorm;
|
||||
double s0, s1, s2, s3;
|
||||
double qDot1, qDot2, qDot3, qDot4;
|
||||
|
||||
// Rate of change of quaternion from gyroscope
|
||||
qDot1 = 0.5 * (-q1*gx - q2*gy - q3*gz);
|
||||
qDot2 = 0.5 * ( q0*gx + q2*gz - q3*gy);
|
||||
qDot3 = 0.5 * ( q0*gy - q1*gz + q3*gx);
|
||||
qDot4 = 0.5 * ( q0*gz + q1*gy - q2*gx);
|
||||
|
||||
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
|
||||
if (!((ax == 0.0) && (ay == 0.0) && (az == 0.0))) {
|
||||
|
||||
// Normalise accelerometer measurement
|
||||
recipNorm = 1.0 / sqrt(ax*ax + ay*ay + az*az);
|
||||
ax *= recipNorm;
|
||||
ay *= recipNorm;
|
||||
az *= recipNorm;
|
||||
|
||||
// Gradient decent algorithm corrective step
|
||||
s0 = 4.0*q0*q2*q2 + 2.0*q2*ax + 4.0*q0*q1*q1 - 2.0*q1*ay;
|
||||
s1 = 4.0*q1*q3*q3 - 2.0*q3*ax + 4.0*q0*q0*q1 - 2.0*q0*ay - 4.0*q1 + 8.0*q1*q1*q1 + 8.0*q1*q2*q2 + 4.0*q1*az;
|
||||
s2 = 4.0*q0*q0*q2 + 2.0*q0*ax + 4.0*q2*q3*q3 - 2.0*q3*ay - 4.0*q2 + 8.0*q2*q1*q1 + 8.0*q2*q2*q2 + 4.0*q2*az;
|
||||
s3 = 4.0*q1*q1*q3 - 2.0*q1*ax + 4.0*q2*q2*q3 - 2.0*q2*ay;
|
||||
|
||||
// Normalise step magnitude
|
||||
recipNorm = 1.0 / sqrt(s0*s0 + s1*s1 + s2*s2 + s3*s3);
|
||||
s0 *= recipNorm;
|
||||
s1 *= recipNorm;
|
||||
s2 *= recipNorm;
|
||||
s3 *= recipNorm;
|
||||
|
||||
// Apply feedback step
|
||||
qDot1 -= beta * s0;
|
||||
qDot2 -= beta * s1;
|
||||
qDot3 -= beta * s2;
|
||||
qDot4 -= beta * s3;
|
||||
}
|
||||
|
||||
// Integrate rate of change of quaternion to yield quaternion
|
||||
q0 += qDot1 * dt;
|
||||
q1 += qDot2 * dt;
|
||||
q2 += qDot3 * dt;
|
||||
q3 += qDot4 * dt;
|
||||
|
||||
// Normalise quaternion
|
||||
recipNorm = 1.0 / sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
|
||||
q0 *= recipNorm;
|
||||
q1 *= recipNorm;
|
||||
q2 *= recipNorm;
|
||||
q3 *= recipNorm;
|
||||
|
||||
q->w = q0;
|
||||
q->x = q1;
|
||||
q->y = q2;
|
||||
q->z = q3;
|
||||
}
|
||||
@@ -1,42 +0,0 @@
|
||||
/*
|
||||
* Copyright 2022 Oleg Borodin <borodin@unix7.org>
|
||||
*/
|
||||
|
||||
#ifndef GEOMETRY_H_QWERTY
|
||||
#define GEOMETRY_H_QWERTY
|
||||
|
||||
typedef struct {
|
||||
union { double z; double yaw; };
|
||||
union { double y; double pitch; };
|
||||
union { double x; double roll; };
|
||||
} eulerangle_t;
|
||||
|
||||
|
||||
typedef struct quaternion_s {
|
||||
union { double w; double q0; };
|
||||
union { double x; double q1; };
|
||||
union { double y; double q2; };
|
||||
union { double z; double q3; };
|
||||
} quaternion_t;
|
||||
|
||||
|
||||
typedef struct {
|
||||
double ax;
|
||||
double ay;
|
||||
double az;
|
||||
double gx;
|
||||
double gy;
|
||||
double gz;
|
||||
} imuvec_t;
|
||||
|
||||
void eulerangle_init(eulerangle_t* a);
|
||||
void eulerangle_norm(eulerangle_t* a);
|
||||
void eulerangle_todegress(eulerangle_t* a);
|
||||
void eulerangle_toradians(eulerangle_t* a);
|
||||
|
||||
void quaternion_init(quaternion_t* q);
|
||||
void quaternion_toeuler(quaternion_t* q, eulerangle_t* a);
|
||||
|
||||
void quaternion_madgwick(quaternion_t* q, imuvec_t* m, double dt);
|
||||
|
||||
#endif
|
||||
@@ -1,44 +0,0 @@
|
||||
/*
|
||||
* Copyright 2022 Oleg Borodin <borodin@unix7.org>
|
||||
*/
|
||||
|
||||
|
||||
#include <libopencm3/stm32/i2c.h>
|
||||
#include <stdint.h>
|
||||
#include <math.h>
|
||||
|
||||
|
||||
void i2cdev_write_reg8(uint32_t i2c, uint8_t addr, uint8_t reg, uint8_t value) {
|
||||
uint8_t buffer[2];
|
||||
buffer[0] = reg;
|
||||
buffer[1] = value;
|
||||
i2c_transfer7(i2c, addr, buffer, 2, NULL, 0);
|
||||
}
|
||||
|
||||
uint8_t i2cdev_read_reg8(uint32_t i2c, uint8_t addr, uint8_t reg) {
|
||||
uint8_t val;
|
||||
i2c_transfer7(i2c, addr, ®, 1, &val, 1);
|
||||
return val;
|
||||
}
|
||||
|
||||
void i2cdev_reg_setbits(uint32_t i2c, uint8_t addr, uint8_t reg, uint8_t mask) {
|
||||
uint8_t buffer[2];
|
||||
buffer[0] = reg;
|
||||
buffer[1] = 0x00;
|
||||
i2c_transfer7(i2c, addr, &buffer[0], 1, &buffer[1], 1);
|
||||
buffer[1] |= mask;
|
||||
i2c_transfer7(i2c, addr, buffer, 2, NULL, 0);
|
||||
}
|
||||
|
||||
void i2cdev_reg_cleanbits(uint32_t i2c, uint8_t addr, uint8_t reg, uint8_t mask) {
|
||||
uint8_t buffer[2];
|
||||
buffer[0] = reg;
|
||||
buffer[1] = 0x00;
|
||||
i2c_transfer7(i2c, addr, &buffer[0], 1, &buffer[1], 1);
|
||||
buffer[1] &= ~mask;
|
||||
i2c_transfer7(i2c, addr, buffer, 2, NULL, 0);
|
||||
}
|
||||
|
||||
void i2cdev_read_seq8(uint32_t i2c, uint8_t addr, uint8_t reg, uint8_t* buffer, uint8_t size) {
|
||||
i2c_transfer7(i2c, addr, ®, 1, buffer, size);
|
||||
}
|
||||
@@ -1,18 +0,0 @@
|
||||
/*
|
||||
* Copyright 2022 Oleg Borodin <borodin@unix7.org>
|
||||
*/
|
||||
|
||||
#ifndef I2CDEV_H_QWERTY
|
||||
#define I2CDEV_H_QWERTY
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
void i2cdev_write_reg8(uint32_t i2c, uint8_t addr, uint8_t reg, uint8_t value);
|
||||
uint8_t i2cdev_read_reg8(uint32_t i2c, uint8_t addr, uint8_t reg);
|
||||
|
||||
void i2cdev_read_seq8(uint32_t i2c, uint8_t addr, uint8_t reg, uint8_t* buffer, uint8_t size);
|
||||
|
||||
void i2cdev_reg_setbits(uint32_t i2c, uint8_t addr, uint8_t reg, uint8_t mask);
|
||||
void i2cdev_reg_cleanbits(uint32_t i2c, uint8_t addr, uint8_t reg, uint8_t mask);
|
||||
|
||||
#endif
|
||||
+51
-31
@@ -99,7 +99,7 @@ uint32_t sys_tick_counter(void) {
|
||||
#define PWM300 3333
|
||||
#define PWM330 3030
|
||||
|
||||
static void timer_init(uint32_t timer) {
|
||||
static void timer_mode_init(uint32_t timer) {
|
||||
|
||||
int prescale = rcc_ahb_frequency / (2 * 1000 * 1000) - 1;
|
||||
int period = PWM330;
|
||||
@@ -115,7 +115,7 @@ static void timer_init(uint32_t timer) {
|
||||
timer_enable_counter(timer);
|
||||
}
|
||||
|
||||
static void tc_init(uint32_t timer, uint32_t channel) {
|
||||
static void timer_channel_init(uint32_t timer, uint32_t channel) {
|
||||
|
||||
timer_disable_oc_output(timer, channel);
|
||||
timer_set_oc_value(timer, channel, 0);
|
||||
@@ -131,7 +131,7 @@ static void tc_init(uint32_t timer, uint32_t channel) {
|
||||
timer_enable_oc_output(timer, channel);
|
||||
}
|
||||
|
||||
static void tc_setratio(uint32_t timer, uint32_t channel, uint32_t ratio) {
|
||||
static void timer_channel_setratio(uint32_t timer, uint32_t channel, uint32_t ratio) {
|
||||
uint32_t period = TIM_ARR(timer);
|
||||
uint32_t value = (period * ratio) / 1000;
|
||||
timer_set_oc_value(timer, channel, value);
|
||||
@@ -178,6 +178,12 @@ typedef struct {
|
||||
quaternion_t q;
|
||||
eulerangle_t a;
|
||||
|
||||
eulerangle_t da;
|
||||
|
||||
pidcont_t px;
|
||||
pidcont_t py;
|
||||
pidcont_t pz;
|
||||
|
||||
mixer_t mix;
|
||||
double outx;
|
||||
double outy;
|
||||
@@ -190,26 +196,27 @@ void uav_init(uav_t* uav) {
|
||||
usart_setup(USART1, GPIOA, GPIO9 | GPIO10, 460800);
|
||||
|
||||
i2c_setup(I2C1, GPIOB, GPIO8 | GPIO9);
|
||||
|
||||
systick_setup(g_systick_freq);
|
||||
|
||||
uav->timer = TIM2;
|
||||
timer_mode_init(uav->timer);
|
||||
|
||||
timer_channel_init(uav->timer, TIM_OC1);
|
||||
timer_channel_init(uav->timer, TIM_OC2);
|
||||
timer_channel_init(uav->timer, TIM_OC3);
|
||||
timer_channel_init(uav->timer, TIM_OC4);
|
||||
|
||||
timer_channel_setratio(uav->timer, TIM_OC1, 50);
|
||||
timer_channel_setratio(uav->timer, TIM_OC2, 50);
|
||||
timer_channel_setratio(uav->timer, TIM_OC3, 50);
|
||||
timer_channel_setratio(uav->timer, TIM_OC4, 50);
|
||||
|
||||
timer_gpio_setup(GPIOA, GPIO_AF1, GPIO0);
|
||||
timer_gpio_setup(GPIOA, GPIO_AF1, GPIO1);
|
||||
timer_gpio_setup(GPIOA, GPIO_AF1, GPIO2);
|
||||
timer_gpio_setup(GPIOA, GPIO_AF1, GPIO3);
|
||||
|
||||
uav->timer = TIM2;
|
||||
timer_init(uav->timer);
|
||||
|
||||
tc_init(uav->timer, TIM_OC1);
|
||||
tc_init(uav->timer, TIM_OC2);
|
||||
tc_init(uav->timer, TIM_OC3);
|
||||
tc_init(uav->timer, TIM_OC4);
|
||||
|
||||
tc_setratio(uav->timer, TIM_OC1, 10);
|
||||
tc_setratio(uav->timer, TIM_OC2, 30);
|
||||
tc_setratio(uav->timer, TIM_OC3, 50);
|
||||
tc_setratio(uav->timer, TIM_OC4, 70);
|
||||
|
||||
imu_setup(&(uav->imu), I2C1, 0x68);
|
||||
imu_calibrate(&(uav->imu), 100);
|
||||
|
||||
@@ -227,10 +234,25 @@ void uav_init(uav_t* uav) {
|
||||
|
||||
quaternion_init(&(uav->q));
|
||||
|
||||
eulerangle_init(&(uav->a));
|
||||
eulerangle_init(&(uav->da));
|
||||
|
||||
pidcont_init(&(uav->px));
|
||||
pidcont_init(&(uav->py));
|
||||
pidcont_init(&(uav->pz));
|
||||
|
||||
double kp = 0.0;
|
||||
double ki = 400.0;
|
||||
double kd = 0.0;
|
||||
|
||||
pidcont_setup(&(uav->px), kp, ki, kd);
|
||||
pidcont_setup(&(uav->py), kp, ki, kd);
|
||||
pidcont_setup(&(uav->pz), kp, ki, kd);
|
||||
|
||||
mixer_init(&(uav->mix));
|
||||
|
||||
mixer_iset(&(uav->mix), 0, &(uav->a.x));
|
||||
mixer_iset(&(uav->mix), 1, &(uav->a.y));
|
||||
mixer_iset(&(uav->mix), 0, &(uav->da.x));
|
||||
mixer_iset(&(uav->mix), 1, &(uav->da.y));
|
||||
|
||||
mixer_oset(&(uav->mix), 0, &(uav->outx));
|
||||
mixer_oset(&(uav->mix), 1, &(uav->outy));
|
||||
@@ -240,12 +262,6 @@ void uav_init(uav_t* uav) {
|
||||
mixer_rset(&(uav->mix), 2, 0, 1, -0.7);
|
||||
mixer_rset(&(uav->mix), 3, 1, 0, 0.7);
|
||||
|
||||
//pidcont_t p;
|
||||
//pidcont_init(&p);
|
||||
|
||||
//double kp = 0;
|
||||
//double ki = 4000.0;
|
||||
//pidcont_setup(&p, kp, ki, 0);
|
||||
}
|
||||
|
||||
|
||||
@@ -269,9 +285,13 @@ void uav_loop(uav_t* uav) {
|
||||
|
||||
eulerangle_todegress(&(uav->a));
|
||||
|
||||
mixer_apply(&(uav->mix));
|
||||
uav->da.x = pidcont_apply(&(uav->px), 0, uav->a.x, dt);
|
||||
uav->da.y = pidcont_apply(&(uav->py), 0, uav->a.y, dt);
|
||||
uav->da.z = pidcont_apply(&(uav->pz), 0, uav->a.z, dt);
|
||||
|
||||
printf("dt=%.6f outx=%8.3f outy=%8.3f\r\n", dt, uav->outx, uav->outy);
|
||||
printf("dt=%.6f %8.3f %8.3f %8.3f\r\n", dt, uav->da.x, uav->da.y, uav->da.z);
|
||||
|
||||
mixer_apply(&(uav->mix));
|
||||
|
||||
double pmin = -90.0;
|
||||
double pmax = 90.0;
|
||||
@@ -282,13 +302,13 @@ void uav_loop(uav_t* uav) {
|
||||
uint32_t out1 = (uint32_t)mapval(pmin, pmax, omin, omax, uav->outx, true);
|
||||
uint32_t out2 = (uint32_t)mapval(pmin, pmax, omin, omax, uav->outy, false);
|
||||
|
||||
tc_setratio(uav->timer, TIM_OC1, out1);
|
||||
tc_setratio(uav->timer, TIM_OC2, out2);
|
||||
|
||||
//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_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 pitch = pidcont_apply(&p, 0, a.pitch, dt);
|
||||
|
||||
timer_channel_setratio(uav->timer, TIM_OC1, out1);
|
||||
timer_channel_setratio(uav->timer, TIM_OC2, out2);
|
||||
|
||||
};
|
||||
}
|
||||
|
||||
|
||||
@@ -1,68 +0,0 @@
|
||||
|
||||
MEMORY {
|
||||
FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 512K
|
||||
SRAM (rwx) : ORIGIN = 0x20000000, LENGTH = 128K
|
||||
}
|
||||
|
||||
EXTERN (vector_table)
|
||||
|
||||
ENTRY(reset_handler)
|
||||
|
||||
SECTIONS
|
||||
{
|
||||
.text : {
|
||||
*(.vectors)
|
||||
*(.text*)
|
||||
. = ALIGN(4);
|
||||
*(.rodata*)
|
||||
. = ALIGN(4);
|
||||
} >FLASH
|
||||
|
||||
.preinit_array : {
|
||||
. = ALIGN(4);
|
||||
__preinit_array_start = .;
|
||||
KEEP (*(.preinit_array))
|
||||
__preinit_array_end = .;
|
||||
} >FLASH
|
||||
|
||||
.init_array : {
|
||||
. = ALIGN(4);
|
||||
__init_array_start = .;
|
||||
KEEP (*(SORT(.init_array.*)))
|
||||
KEEP (*(.init_array))
|
||||
__init_array_end = .;
|
||||
} >FLASH
|
||||
|
||||
.fini_array : {
|
||||
. = ALIGN(4);
|
||||
__fini_array_start = .;
|
||||
KEEP (*(.fini_array))
|
||||
KEEP (*(SORT(.fini_array.*)))
|
||||
__fini_array_end = .;
|
||||
} >FLASH
|
||||
|
||||
. = ALIGN(4);
|
||||
_etext = .;
|
||||
|
||||
.data : {
|
||||
_data = .;
|
||||
*(.data*)
|
||||
. = ALIGN(4);
|
||||
_edata = .;
|
||||
} >SRAM AT >FLASH
|
||||
|
||||
_data_loadaddr = LOADADDR(.data);
|
||||
|
||||
.bss : {
|
||||
*(.bss*)
|
||||
*(COMMON)
|
||||
. = ALIGN(4);
|
||||
_ebss = .;
|
||||
} >SRAM
|
||||
|
||||
|
||||
. = ALIGN(4);
|
||||
_end = .;
|
||||
}
|
||||
|
||||
PROVIDE(_stack = ORIGIN(SRAM) + LENGTH(SRAM));
|
||||
@@ -1,18 +0,0 @@
|
||||
/*
|
||||
* Copyright 2022 Oleg Borodin <borodin@unix7.org>
|
||||
*/
|
||||
|
||||
#include <stdbool.h>
|
||||
|
||||
double mapval(double imin, double imax, double omin, double omax, double in, bool inv) {
|
||||
double odiap = (omax - omin);
|
||||
double k = 1.0 / (imax - imin);
|
||||
double out = 0.0;
|
||||
if (inv) {
|
||||
out = (omax - odiap/2.0) - in * k * odiap;
|
||||
} else {
|
||||
out = in * k * odiap + (odiap/2.0 + omin);
|
||||
}
|
||||
|
||||
return out;
|
||||
}
|
||||
@@ -1,10 +0,0 @@
|
||||
/*
|
||||
* Copyright 2022 Oleg Borodin <borodin@unix7.org>
|
||||
*/
|
||||
|
||||
#ifndef MISC_H_QWERTY
|
||||
#define MISC_H_QWERTY
|
||||
|
||||
double mapval(double imin, double imax, double omin, double omax, double in, bool inv);
|
||||
|
||||
#endif
|
||||
@@ -1,60 +0,0 @@
|
||||
/*
|
||||
* Copyright 2022 Oleg Borodin <borodin@unix7.org>
|
||||
*/
|
||||
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include <mixer.h>
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
@@ -1,38 +0,0 @@
|
||||
/*
|
||||
* Copyright 2022 Oleg Borodin <borodin@unix7.org>
|
||||
*/
|
||||
|
||||
|
||||
#ifndef MIXER_H_QWERTY
|
||||
#define MIXER_H_QWERTY
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdint.h>
|
||||
|
||||
|
||||
#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
|
||||
@@ -1,196 +0,0 @@
|
||||
/*
|
||||
* Copyright 2022 Oleg Borodin <borodin@unix7.org>
|
||||
*/
|
||||
|
||||
#include <libopencm3/stm32/i2c.h>
|
||||
#include <stdint.h>
|
||||
#include <math.h>
|
||||
|
||||
#include <i2cdev.h>
|
||||
#include <mpu6050.h>
|
||||
|
||||
#define MPU_REG_SMPLRT_DIV 0x19
|
||||
|
||||
#define MPU_REG_CONFIG 0x1A
|
||||
#define MPU_REG_GYRO_CONFIG 0x1B
|
||||
#define MPU_REG_ACCEL_CONFIG 0x1C
|
||||
|
||||
|
||||
#define MPU_REG_ACCEL_XOUT_H 0x3B
|
||||
#define MPU_REG_ACCEL_XOUT_L 0x3C
|
||||
#define MPU_REG_ACCEL_YOUT_H 0x3D
|
||||
#define MPU_REG_ACCEL_YOUT_L 0x3E
|
||||
#define MPU_REG_ACCEL_ZOUT_H 0x3F
|
||||
#define MPU_REG_ACCEL_ZOUT_L 0x40
|
||||
#define MPU_REG_TEMP_OUT_H 0x41
|
||||
#define MPU_REG_TEMP_OUT_L 0x42
|
||||
#define MPU_REG_GYRO_XOUT_H 0x43
|
||||
#define MPU_REG_GYRO_XOUT_L 0x44
|
||||
#define MPU_REG_GYRO_YOUT_H 0x45
|
||||
#define MPU_REG_GYRO_YOUT_L 0x46
|
||||
#define MPU_REG_GYRO_ZOUT_H 0x47
|
||||
#define MPU_REG_GYRO_ZOUT_L 0x48
|
||||
|
||||
#define MPU_REG_PWR_MGMT_1 0x6B
|
||||
#define MPU_REG_PWR_MGMT_2 0x6C
|
||||
|
||||
/* GYRO_CONFIG 0x1B */
|
||||
#define MPU_GYRO_FS_BASE 3
|
||||
#define MPU_GYRO_FS_LEN 2
|
||||
|
||||
#define MPU_GYRO_FS_250 0
|
||||
#define MPU_GYRO_FS_500 1
|
||||
#define MPU_GYRO_FS_1000 2
|
||||
#define MPU_GYRO_FS_2000 3
|
||||
|
||||
/* ACCEL_CONFIG 0x1C */
|
||||
#define MPU_ACCEL_FS_BASE 3
|
||||
#define MPU_ACCEL_FS_LEN 2
|
||||
#define MPU_ACCEL_FS_2 0
|
||||
#define MPU_ACCEL_FS_4 1
|
||||
#define MPU_ACCEL_FS_8 2
|
||||
#define MPU_ACCEL_FS_16 3
|
||||
|
||||
#define MPU_GYRO_LSB_250 131.0f
|
||||
#define MPU_GYRO_LSB_500 65.5f
|
||||
#define MPU_GYRO_LSB_1000 32.8f
|
||||
#define MPU_GYRO_LSB_2000 16.4f
|
||||
|
||||
#define MPU_ACCEL_LSB_2 16384.0f
|
||||
#define MPU_ACCEL_LSB_4 8192.0f
|
||||
#define MPU_ACCEL_LSB_8 4096.0f
|
||||
#define MPU_ACCEL_LSB_16 2048.0f
|
||||
|
||||
/* PWR_MGMT_1 0x6B */
|
||||
#define MPU_PWR1_DEVICE_RESET_BIT 7
|
||||
#define MPU_PWR1_SLEEP_BIT 6
|
||||
#define MPU_PWR1_CYCLE_BIT 5
|
||||
#define MPU_PWR1_TEMP_DIS_BIT 3
|
||||
|
||||
#define MPU_PWR1_CLKSEL_BASE 0
|
||||
#define MPU_PWR1_CLKSEL_LEN 3
|
||||
|
||||
#define MPU_PWR1_CLKSEL_INTERNAL 0
|
||||
#define MPU_PWR1_CLKSEL_PLL_XGYRO 1
|
||||
#define MPU_PWR1_CLKSEL_PLL_YGYRO 2
|
||||
#define MPU_PWR1_CLKSEL_PLL_ZGYRO 3
|
||||
#define MPU_PWR1_CLKSEL_PLL_EXT32K 4
|
||||
#define MPU_PWR1_CLKSEL_PLL_EXT19M 5
|
||||
#define MPU_PWR1_CLKSEL_KEEP_RESET 7
|
||||
|
||||
/* PWR_MGMT_2 0x6C */
|
||||
#define MPU_PWR2_LP_WAKE_CTRL_BASE 6
|
||||
#define MPU_PWR2_LP_WAKE_CTRL_LEN 2
|
||||
|
||||
#define MPU_PWR2_WAKE_FREQ_1P25 0
|
||||
#define MPU_PWR2_WAKE_FREQ_2P5 1
|
||||
#define MPU_PWR2_WAKE_FREQ_5 2
|
||||
#define MPU_PWR2_WAKE_FREQ_10 3
|
||||
|
||||
#define MPU_PWR2_STBY_XA_BIT 5
|
||||
#define MPU_PWR2_STBY_YA_BIT 4
|
||||
#define MPU_PWR2_STBY_ZA_BIT 3
|
||||
#define MPU_PWR2_STBY_XG_BIT 2
|
||||
#define MPU_PWR2_STBY_YG_BIT 1
|
||||
#define MPU_PWR2_STBY_ZG_BIT 0
|
||||
|
||||
#define MPU_GYRO_LSB MPU_GYRO_LSB_1000
|
||||
#define MPU_GYRO_FS MPU_GYRO_FS_1000
|
||||
|
||||
#define MPU_ACCEL_LSB MPU_ACCEL_LSB_16
|
||||
#define MPU_ACCEL_FS MPU_ACCEL_FS_16
|
||||
|
||||
void imu_setup(imu_t* imu, uint32_t i2c, uint8_t addr) {
|
||||
|
||||
imu->bus = i2c;
|
||||
imu->addr = addr;
|
||||
|
||||
imu->gxe = 0;
|
||||
imu->gye = 0;
|
||||
imu->gze = 0;
|
||||
|
||||
//i2cdev_write_reg8(i2c, addr, MPU_REG_PWR_MGMT_1, 1 << MPU_PWR1_DEVICE_RESET_BIT);
|
||||
//for (int i = 0; i < 10000; i++) __asm__("nop");
|
||||
|
||||
i2cdev_write_reg8(i2c, addr, MPU_REG_PWR_MGMT_1, 0x00);
|
||||
|
||||
i2cdev_write_reg8(i2c, addr, MPU_REG_GYRO_CONFIG, MPU_GYRO_FS << MPU_GYRO_FS_BASE);
|
||||
i2cdev_write_reg8(i2c, addr, MPU_REG_ACCEL_CONFIG, MPU_ACCEL_FS << MPU_ACCEL_FS_BASE);
|
||||
i2cdev_write_reg8(i2c, addr, MPU_REG_SMPLRT_DIV, 4);
|
||||
}
|
||||
|
||||
|
||||
static void imu_rawread(imu_t* imu, imuvec_t* val) {
|
||||
|
||||
uint8_t buffer[14];
|
||||
i2cdev_read_seq8(imu->bus, imu->addr, MPU_REG_ACCEL_XOUT_H, (uint8_t*)buffer, 14);
|
||||
|
||||
int16_t ax = (((int16_t)buffer[0]) << 8) | buffer[1];
|
||||
int16_t ay = (((int16_t)buffer[2]) << 8) | buffer[3];
|
||||
int16_t az = (((int16_t)buffer[4]) << 8) | buffer[5];
|
||||
|
||||
int16_t gx = (((int16_t)buffer[8]) << 8) | buffer[9];
|
||||
int16_t gy = (((int16_t)buffer[10]) << 8) | buffer[11];
|
||||
int16_t gz = (((int16_t)buffer[12]) << 8) | buffer[13];
|
||||
|
||||
val->ax = (double)ax / (double)MPU_ACCEL_LSB;
|
||||
val->ay = (double)ay / (double)MPU_ACCEL_LSB;
|
||||
val->az = (double)az / (double)MPU_ACCEL_LSB;
|
||||
|
||||
val->gx = (double)gx / (double)MPU_GYRO_LSB;
|
||||
val->gy = (double)gy / (double)MPU_GYRO_LSB;
|
||||
val->gz = (double)gz / (double)MPU_GYRO_LSB;
|
||||
|
||||
val->gx *= M_PI / 180.0;
|
||||
val->gy *= M_PI / 180.0;
|
||||
val->gz *= M_PI / 180.0;
|
||||
|
||||
}
|
||||
|
||||
void imu_calibrate(imu_t* imu, int loops) {
|
||||
imuvec_t val;
|
||||
val.ax = 0;
|
||||
val.ay = 0;
|
||||
val.az = 0;
|
||||
val.gx = 0;
|
||||
val.gy = 0;
|
||||
val.gz = 0;
|
||||
|
||||
for (int i = 0; i < loops; i++) {
|
||||
imu_rawread(imu, &val);
|
||||
imu->gxe += val.gx / (double)loops;
|
||||
imu->gye += val.gy / (double)loops;
|
||||
imu->gze += val.gz / (double)loops;
|
||||
}
|
||||
}
|
||||
|
||||
void imu_gettilt(imu_t* imu, int loops, eulerangle_t* a) {
|
||||
imuvec_t val;
|
||||
val.ax = 0;
|
||||
val.ay = 0;
|
||||
val.az = 0;
|
||||
val.gx = 0;
|
||||
val.gy = 0;
|
||||
val.gz = 0;
|
||||
|
||||
double ax = 0;
|
||||
double ay = 0;
|
||||
double az = 0;
|
||||
|
||||
for (int i = 0; i < loops; i++) {
|
||||
imu_rawread(imu, &val);
|
||||
ax += val.ax / (double)loops;
|
||||
ay += val.ay / (double)loops;
|
||||
az += val.az / (double)loops;
|
||||
}
|
||||
a->x = atan(ax / sqrt(ay*ay + az*az));
|
||||
a->y = atan(ay / sqrt(ax*ax + az*az));
|
||||
a->z = atan(az / sqrt(ax*ax + ay*ay));
|
||||
}
|
||||
|
||||
void imu_getvec(imu_t* imu, imuvec_t* val) {
|
||||
imu_rawread(imu, val);
|
||||
val->gx -= imu->gxe;
|
||||
val->gy -= imu->gye;
|
||||
val->gz -= imu->gze;
|
||||
}
|
||||
@@ -1,20 +0,0 @@
|
||||
/*
|
||||
* Copyright 2022 Oleg Borodin <borodin@unix7.org>
|
||||
*/
|
||||
|
||||
|
||||
#include <stdint.h>
|
||||
#include <geometry.h>
|
||||
|
||||
typedef struct {
|
||||
uint32_t bus;
|
||||
uint8_t addr;
|
||||
double gxe;
|
||||
double gye;
|
||||
double gze;
|
||||
} imu_t;
|
||||
|
||||
void imu_setup(imu_t* imu, uint32_t i2c, uint8_t addr);
|
||||
void imu_calibrate(imu_t* imu, int count);
|
||||
void imu_gettilt(imu_t* imu, int loops, eulerangle_t* a);
|
||||
void imu_getvec(imu_t* imu, imuvec_t* val);
|
||||
@@ -1,43 +0,0 @@
|
||||
/*
|
||||
* Copyright 2022 Oleg Borodin <borodin@unix7.org>
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
#include <stdint.h>
|
||||
#include <pidcont.h>
|
||||
|
||||
void pidcont_init(pidcont_t* p) {
|
||||
p->perror = 0.0;
|
||||
p->integ = 0.0;
|
||||
|
||||
p->kp = 0.0;
|
||||
p->ki = 0.0;
|
||||
p->kd = 0.0;
|
||||
}
|
||||
|
||||
|
||||
void pidcont_setup(pidcont_t* p, double kp, double ki, double kd) {
|
||||
p->kp = kp;
|
||||
p->ki = ki;
|
||||
p->kd = kd;
|
||||
}
|
||||
|
||||
double pidcont_apply(pidcont_t* p, double target, double actual, double dt) {
|
||||
|
||||
double error = 0.0;
|
||||
double deriv = 0.0;
|
||||
double integ = 0.0;
|
||||
double output = 0.0;
|
||||
|
||||
error = target - actual;
|
||||
integ += error * dt;
|
||||
|
||||
deriv = error - p->perror;
|
||||
|
||||
output = (p->kp * error) + (p->ki * integ) + (p->kd * deriv);
|
||||
|
||||
p->perror = error;
|
||||
p->integ = integ;
|
||||
return output;
|
||||
}
|
||||
@@ -1,25 +0,0 @@
|
||||
/*
|
||||
* Copyright 2022 Oleg Borodin <borodin@unix7.org>
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
#ifndef PIDCONT_H_QWERTY
|
||||
#define PIDCONT_H_QWERTY
|
||||
|
||||
|
||||
typedef struct {
|
||||
double perror;
|
||||
double integ;
|
||||
|
||||
double kp;
|
||||
double ki;
|
||||
double kd;
|
||||
} pidcont_t;
|
||||
|
||||
|
||||
void pidcont_init(pidcont_t* p);
|
||||
void pidcont_setup(pidcont_t* p, double kp, double ki, double kd);
|
||||
double pidcont_apply(pidcont_t* p, double target, double actual, double dt);
|
||||
|
||||
#endif
|
||||
@@ -1,159 +0,0 @@
|
||||
/*
|
||||
* Copyright 2022 Oleg Borodin <borodin@unix7.org>
|
||||
*/
|
||||
|
||||
|
||||
#include <libopencm3/stm32/usart.h>
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <reent.h>
|
||||
#include <sys/stat.h>
|
||||
#include <sys/errno.h>
|
||||
#include <errno.h>
|
||||
|
||||
#include <usartu.h>
|
||||
|
||||
#undef errno
|
||||
extern int errno;
|
||||
|
||||
char *__env[1] = { 0 };
|
||||
|
||||
char **environ = __env;
|
||||
|
||||
int _execve(char *name, char **argv, char **env) {
|
||||
errno = ENOMEM;
|
||||
return -1;
|
||||
}
|
||||
|
||||
int _fork(void) {
|
||||
errno = EAGAIN;
|
||||
return -1;
|
||||
}
|
||||
|
||||
int _getpid(void) {
|
||||
return 1;
|
||||
}
|
||||
|
||||
int _kill(int pid, int sig) {
|
||||
errno = EINVAL;
|
||||
return -1;
|
||||
}
|
||||
|
||||
void _exit(int i) {
|
||||
while (1);
|
||||
}
|
||||
|
||||
int _isatty(int file) {
|
||||
return 1;
|
||||
}
|
||||
|
||||
int _fstat(int file, struct stat *st) {
|
||||
st->st_mode = S_IFCHR;
|
||||
return 0;
|
||||
}
|
||||
|
||||
int _link(char *old, char *new) {
|
||||
errno = EMLINK;
|
||||
return -1;
|
||||
}
|
||||
|
||||
int _lseek(int file, int ptr, int dir) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
int _open(const char *name, int flags, int mode) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
#define STDIN 0
|
||||
#define STDOUT 1
|
||||
#define STDERR 3
|
||||
|
||||
|
||||
int _read(int file, char *ptr, int len) {
|
||||
int i = 0;
|
||||
while (i < len) {
|
||||
ptr[i++] = 0;
|
||||
}
|
||||
return i;
|
||||
}
|
||||
|
||||
|
||||
int _write(int file, char *ptr, int len) {
|
||||
int i;
|
||||
|
||||
if ((file == STDOUT) || (file == STDERR)) {
|
||||
for (i = 0; i < len; i++) {
|
||||
usart_putc(USART1, ptr[i]);
|
||||
}
|
||||
return len;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
int _stat(char *file, struct stat *st) {
|
||||
st->st_mode = S_IFCHR;
|
||||
return 0;
|
||||
}
|
||||
|
||||
int _close(int file) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
|
||||
int _times(struct tms *buf) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
int _unlink(char *name) {
|
||||
errno = ENOENT;
|
||||
return -1;
|
||||
}
|
||||
|
||||
int _wait(int *status) {
|
||||
errno = ECHILD;
|
||||
return -1;
|
||||
}
|
||||
|
||||
void *xxx_sbrk(int incr) {
|
||||
|
||||
extern unsigned char *_end;
|
||||
static unsigned char *heap = NULL;
|
||||
unsigned char *prev_heap;
|
||||
|
||||
if (heap == NULL) {
|
||||
heap = (unsigned char*)&_end;
|
||||
}
|
||||
prev_heap = heap;
|
||||
|
||||
heap += incr;
|
||||
return prev_heap;
|
||||
}
|
||||
|
||||
register char* stack_ptr __asm__ ("sp");
|
||||
|
||||
caddr_t __attribute__((weak)) _sbrk (int incr) {
|
||||
extern char end __asm__ ("_end");
|
||||
static char * heap_end;
|
||||
char * prev_heap_end;
|
||||
|
||||
if (heap_end == NULL) {
|
||||
heap_end = &end;
|
||||
}
|
||||
|
||||
prev_heap_end = heap_end;
|
||||
|
||||
if (heap_end + incr > stack_ptr) {
|
||||
#if 0
|
||||
extern void abort (void);
|
||||
_write (1, "_sbrk: Heap and stack collision\n", 32);
|
||||
abort ();
|
||||
#else
|
||||
errno = ENOMEM;
|
||||
return (caddr_t) -1;
|
||||
#endif
|
||||
}
|
||||
|
||||
heap_end += incr;
|
||||
return (caddr_t) prev_heap_end;
|
||||
}
|
||||
@@ -1,19 +0,0 @@
|
||||
/*
|
||||
* Copyright 2022 Oleg Borodin <borodin@unix7.org>
|
||||
*/
|
||||
|
||||
#include <libopencm3/stm32/usart.h>
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
|
||||
void usart_puts(uint32_t usart, char* str) {
|
||||
int i = 0;
|
||||
while (str[i] != 0) {
|
||||
usart_send_blocking(usart, str[i++]);
|
||||
}
|
||||
}
|
||||
|
||||
void usart_putc(uint32_t usart, char c) {
|
||||
usart_send_blocking(usart, c);
|
||||
}
|
||||
@@ -1,7 +0,0 @@
|
||||
#ifndef _USARTU_H_XYZ
|
||||
#define _USARTU_H_XYZ
|
||||
|
||||
void usart_puts(uint32_t usart, char * str);
|
||||
void usart_putc(uint32_t usart, char c);
|
||||
|
||||
#endif
|
||||
Reference in New Issue
Block a user