stm32flight/runvals.c at main · PinkFluffyUnic0rn/stm32flight · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
#include <math.h>
#include "dshot.h"

#include "runvals.h"

int Init = 0;

struct timev Evs[TEV_COUNT];
struct dsp_lpf Lpf[LPF_COUNT];
struct dsp_compl Cmpl[CMPL_COUNT];
struct dsp_pidblval Pid[PID_COUNT];

const char *Evnames[TEV_COUNT] = {
	"imu", "log", "pid", "check",
	"baro", "mag", "tele", "power",
	"autopilot"
};

//struct qmc_data Qmcdata;
struct mag_data Magdata;
struct imu_data Imudata;
struct baro_data Barodata;
struct gnss_data Gnss;
struct crsf_tele Tele;
struct msp_osd Osd;

float Thrust = 0.0;
float Rolltarget = 0.0;
float Pitchtarget = 0.0;
float Yawtarget = 0.0;
float En = 0.0;
enum ALTMODE Altmode = 0;
enum GNSSMODE Gnssmode = 0;
int Speedpid = 0;
int Yawspeedpid = 0;
int Hovermode = 0;
int Elrs = 0;
int Autopilot = 0;

float Alt0 = 0.0;
float Lat0 = 0.0;
float Lon0 = 0.0;
float Goffset = 0.0;

struct trackpoint Points[MAX_POINT_COUNT];
int Pointscount = 0;
int Curpoint = 0;
float Autopilottimer = 0.0;

int Curslot = 0;

int Loops = 0;

int Loopscount = 0;

int Elrstimeout = ELRS_TIMEOUT;

int Emergencydisarm = 0;

int setthrust(struct cdevice *dev,
	float ltd, float rtd, float lbd, float rbd)
{
	struct dshot_data dd;
	float avgthrust;

	// incorrect thrust value protection and
	// extra check for disabled ELRS transmitter
	if (isnan(ltd) || isnan(rtd) || isnan(rbd)
		|| isnan(lbd) || !Elrs)
		ltd = rtd = rbd = lbd = 0.0;

	// update average thrust LPF
	avgthrust = (ltd + rtd + rbd + lbd) / 4.0;
	avgthrust = avgthrust < 0.0 ? 0.0 : avgthrust;

	dsp_updatelpf(Lpf + LPF_AVGTHR, avgthrust);
	dsp_updatelpf(Lpf + LPF_AVGTHRA, avgthrust);

	// linearize thrust
	ltd = pow(trimuf(ltd), 1.0 / St.adj.mtrsc.thr);
	rtd = pow(trimuf(rtd), 1.0 / St.adj.mtrsc.thr);
	lbd = pow(trimuf(lbd), 1.0 / St.adj.mtrsc.thr);
	rbd = pow(trimuf(rbd), 1.0 / St.adj.mtrsc.thr);

	// set values for thrust channels using
	// motor to PWM channel mapping values
	dd.thrust[St.mtr.lt] = ltd;
	dd.thrust[St.mtr.lb] = lbd;
	dd.thrust[St.mtr.rt] = rtd;
	dd.thrust[St.mtr.rb] = rbd;

	// write throttle values for each motor into log
	writelog(LOG_LT, ltd);
	writelog(LOG_LB, lbd);
	writelog(LOG_RB, rbd);
	writelog(LOG_RT, rtd);

	// write average throttle values into log
	writelog(LOG_AVGTHR, dsp_getlpf(Lpf + LPF_AVGTHR));

	// set thrust values using DShot output device
	dev->write(dev->priv, &dd, sizeof(struct dshot_data));

	return 0;
}