-
Notifications
You must be signed in to change notification settings - Fork 8
/
control.h
75 lines (55 loc) · 2.04 KB
/
control.h
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
/*
* Flybrix Flight Controller -- Copyright 2018 Flying Selfie Inc. d/b/a Flybrix
*
* http://www.flybrix.com
*/
#ifndef control_h
#define control_h
#include "PIDCascade.h"
#include "controlVectors.h"
#include "utility/vector3.h"
class Kinematics;
struct RcCommand;
class Control {
public:
struct PIDParameters;
struct VelocityPIDParameters;
Control(const PIDParameters& config, const VelocityPIDParameters& velocity_config);
void parseConfig();
ControlVectors calculateControlVectors(const Vector3<float>& velocity, const Kinematics& feedback, const RcCommand& setpoint);
struct __attribute__((packed)) PIDParameters {
PIDParameters();
bool verify() const;
PIDSettings thrust_master;
PIDSettings pitch_master;
PIDSettings roll_master;
PIDSettings yaw_master;
PIDSettings thrust_slave;
PIDSettings pitch_slave;
PIDSettings roll_slave;
PIDSettings yaw_slave;
float thrust_gain;
float pitch_gain;
float roll_gain;
float yaw_gain;
uint8_t pid_bypass; // bitfield order for bypass: {thrustMaster, pitchMaster, rollMaster, yawMaster, thrustSlave, pitchSlave, rollSlave, yawSlave} (LSB-->MSB)
} pid_parameters;
static_assert(sizeof(PIDParameters) == 4 * 8 * 7 + 4 * 4 + 1, "Data is not packed");
struct __attribute__((packed)) VelocityPIDParameters {
VelocityPIDParameters();
bool verify() const;
PIDSettings forward_master;
PIDSettings right_master;
PIDSettings up_master;
PIDSettings forward_slave;
PIDSettings right_slave;
PIDSettings up_slave;
uint8_t pid_bypass; // bitfield order for bypass: {xMaster, yMaster, zMaster, -, xSlave, ySlave, zSlave, -} (LSB-->MSB)
} velocity_pid_parameters;
static_assert(sizeof(VelocityPIDParameters) == 3 * 8 * 7 + 1, "Data is not packed");
// controllers
PIDCascade<4> forward_pid, right_pid, up_pid;
PIDCascade<2> yaw_pid;
bool bidirectional_throttle{false};
};
#endif