Skip to content

Commit 34a0540

Browse files
JMoore5353JMoore5353
authored
Merge pull request #438 from rosflight/432-new-mixing-system
432 new mixing system
2 parents d16f4ac + 6f2f889 commit 34a0540

25 files changed

+1502
-434
lines changed

‎.github/workflows/unit_tests.yml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -12,7 +12,7 @@ jobs:
1212
- name: clone
1313
run: git submodule update --init --recursive
1414
- name: apt install
15-
run: sudo apt-get install -y build-essential cmake libgtest-dev libeigen3-dev
15+
run: sudo apt-get install -y build-essential cmake libgtest-dev
1616
- name: install gtest
1717
run: |
1818
cd /usr/src/gtest

‎.gitmodules‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -4,3 +4,6 @@
44
[submodule "boards/varmint_h7"]
55
path = boards/varmint_h7
66
url = https://github.com/rosflight/varmint_h7.git
7+
[submodule "lib/eigen"]
8+
path = lib/eigen
9+
url = https://gitlab.com/libeigen/eigen.git

‎CMakeLists.txt‎

Lines changed: 0 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -29,15 +29,13 @@ if("${GIT_VERSION_HASH}" STREQUAL "")
2929
endif()
3030

3131
### source files ###
32-
3332
include_directories(
3433
include
3534
include/interface
3635
lib
3736
comms/mavlink
3837
comms/mavlink/v1.0
3938
comms/mavlink/v1.0/common
40-
comms/mavlink/v1.0/rosflight
4139
)
4240

4341
file(GLOB_RECURSE ROSFLIGHT_SOURCES

‎comms/mavlink/mavlink.cpp‎

Lines changed: 13 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -487,15 +487,19 @@ void Mavlink::handle_msg_offboard_control(const mavlink_message_t * const msg)
487487
return;
488488
}
489489

490-
control.x.value = ctrl.x;
491-
control.y.value = ctrl.y;
492-
control.z.value = ctrl.z;
493-
control.F.value = ctrl.F;
494-
495-
control.x.valid = !(ctrl.ignore & IGNORE_VALUE1);
496-
control.y.valid = !(ctrl.ignore & IGNORE_VALUE2);
497-
control.z.valid = !(ctrl.ignore & IGNORE_VALUE3);
498-
control.F.valid = !(ctrl.ignore & IGNORE_VALUE4);
490+
control.Qx.value = ctrl.Qx;
491+
control.Qy.value = ctrl.Qy;
492+
control.Qz.value = ctrl.Qz;
493+
control.Fx.value = ctrl.Fx;
494+
control.Fy.value = ctrl.Fy;
495+
control.Fz.value = ctrl.Fz;
496+
497+
control.Qx.valid = !(ctrl.ignore & IGNORE_VALUE1);
498+
control.Qy.valid = !(ctrl.ignore & IGNORE_VALUE2);
499+
control.Qz.valid = !(ctrl.ignore & IGNORE_VALUE3);
500+
control.Fx.valid = !(ctrl.ignore & IGNORE_VALUE4);
501+
control.Fy.valid = !(ctrl.ignore & IGNORE_VALUE5);
502+
control.Fz.valid = !(ctrl.ignore & IGNORE_VALUE6);
499503

500504
if (listener_ != nullptr) { listener_->offboard_control_callback(control); }
501505
}

‎comms/mavlink/v1.0‎

‎include/command_manager.h‎

Lines changed: 41 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -47,7 +47,7 @@ typedef enum
4747
{
4848
RATE, // Channel is is in rate mode (mrad/s)
4949
ANGLE, // Channel command is in angle mode (mrad)
50-
THROTTLE, // Channel is direcly controlling throttle max/1000
50+
THROTTLE, // Channel is controlling throttle setting, which will be converted to force
5151
PASSTHROUGH, // Channel directly passes PWM input to the mixer
5252
} control_type_t;
5353

@@ -58,13 +58,22 @@ typedef struct
5858
float value; // The value of the channel
5959
} control_channel_t;
6060

61+
typedef enum
62+
{
63+
X_AXIS,
64+
Y_AXIS,
65+
Z_AXIS,
66+
} rc_f_axis_t;
67+
6168
typedef struct
6269
{
6370
uint32_t stamp_ms;
64-
control_channel_t x;
65-
control_channel_t y;
66-
control_channel_t z;
67-
control_channel_t F;
71+
control_channel_t Qx;
72+
control_channel_t Qy;
73+
control_channel_t Qz;
74+
control_channel_t Fx;
75+
control_channel_t Fy;
76+
control_channel_t Fz;
6877
} control_t;
6978

7079
class CommandManager : public ParamListenerInterface
@@ -77,38 +86,50 @@ class CommandManager : public ParamListenerInterface
7786
control_channel_t * combined;
7887
} mux_t;
7988

80-
mux_t muxes[4] = {{&rc_command_.x, &offboard_command_.x, &combined_command_.x},
81-
{&rc_command_.y, &offboard_command_.y, &combined_command_.y},
82-
{&rc_command_.z, &offboard_command_.z, &combined_command_.z},
83-
{&rc_command_.F, &offboard_command_.F, &combined_command_.F}};
89+
mux_t muxes[6] = {{&rc_command_.Qx, &offboard_command_.Qx, &combined_command_.Qx},
90+
{&rc_command_.Qy, &offboard_command_.Qy, &combined_command_.Qy},
91+
{&rc_command_.Qz, &offboard_command_.Qz, &combined_command_.Qz},
92+
{&rc_command_.Fx, &offboard_command_.Fx, &combined_command_.Fx},
93+
{&rc_command_.Fy, &offboard_command_.Fy, &combined_command_.Fy},
94+
{&rc_command_.Fz, &offboard_command_.Fz, &combined_command_.Fz}};
8495

8596
// clang-format off
8697
control_t rc_command_ = {0,
8798
{false, ANGLE, 0.0},
8899
{false, ANGLE, 0.0},
89100
{false, RATE, 0.0},
101+
{false, THROTTLE, 0.0},
102+
{false, THROTTLE, 0.0},
90103
{false, THROTTLE, 0.0}};
91104
control_t offboard_command_ = {0,
92105
{false, ANGLE, 0.0},
93106
{false, ANGLE, 0.0},
94107
{false, RATE, 0.0},
108+
{false, THROTTLE, 0.0},
109+
{false, THROTTLE, 0.0},
95110
{false, THROTTLE, 0.0}};
96111
control_t combined_command_ = {0,
97112
{false, ANGLE, 0.0},
98113
{false, ANGLE, 0.0},
99114
{false, RATE, 0.0},
115+
{false, THROTTLE, 0.0},
116+
{false, THROTTLE, 0.0},
100117
{false, THROTTLE, 0.0}};
101118

102119
control_t multirotor_failsafe_command_ = {0,
103120
{true, ANGLE, 0.0},
104121
{true, ANGLE, 0.0},
105122
{true, RATE, 0.0},
123+
{true, THROTTLE, 0.0},
124+
{true, THROTTLE, 0.0},
106125
{true, THROTTLE, 0.0}};
107126
control_t fixedwing_failsafe_command_ = {0,
108127
{true, PASSTHROUGH, 0.0},
109128
{true, PASSTHROUGH, 0.0},
110129
{true, PASSTHROUGH, 0.0},
111-
{true, THROTTLE, 0.0}};
130+
{true, PASSTHROUGH, 0.0},
131+
{true, PASSTHROUGH, 0.0},
132+
{true, PASSTHROUGH, 0.0}};
112133
// clang-format on
113134

114135
typedef enum
@@ -119,10 +140,12 @@ class CommandManager : public ParamListenerInterface
119140

120141
enum MuxChannel
121142
{
122-
MUX_X,
123-
MUX_Y,
124-
MUX_Z,
125-
MUX_F,
143+
MUX_QX,
144+
MUX_QY,
145+
MUX_QZ,
146+
MUX_FX,
147+
MUX_FY,
148+
MUX_FZ,
126149
};
127150

128151
typedef struct
@@ -138,7 +161,8 @@ class CommandManager : public ParamListenerInterface
138161
ROSflight & RF_;
139162

140163
bool new_command_;
141-
bool rc_override_;
164+
bool rc_throttle_override_;
165+
bool rc_attitude_override_;
142166

143167
control_t & failsafe_command_;
144168

@@ -157,6 +181,8 @@ class CommandManager : public ParamListenerInterface
157181
void init();
158182
bool run();
159183
bool rc_override_active();
184+
bool rc_throttle_override_active();
185+
bool rc_attitude_override_active();
160186
bool offboard_control_active();
161187
void set_new_offboard_command(control_t new_offboard_command);
162188
void set_new_rc_command(control_t new_rc_command);

‎include/controller.h‎

Lines changed: 13 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -51,21 +51,26 @@ class Controller : public ParamListenerInterface
5151
public:
5252
struct Output
5353
{
54-
float F;
55-
float x;
56-
float y;
57-
float z;
54+
float Fx;
55+
float Fy;
56+
float Fz;
57+
float Qx;
58+
float Qy;
59+
float Qz;
5860
};
5961

6062
Controller(ROSflight & rf);
6163

6264
inline const Output & output() const { return output_; }
65+
inline float max_thrust() const { return max_thrust_; }
6366

6467
void init();
6568
void run();
6669

70+
void calculate_max_thrust();
6771
void calculate_equilbrium_torque_from_rc();
6872
void param_change_callback(uint16_t param_id) override;
73+
bool is_throttle_high(float threshold);
6974

7075
private:
7176
class PID
@@ -92,8 +97,8 @@ class Controller : public ParamListenerInterface
9297

9398
ROSflight & RF_;
9499

95-
turbomath::Vector run_pid_loops(uint32_t dt, const Estimator::State & state,
96-
const control_t & command, bool update_integrators);
100+
Controller::Output run_pid_loops(uint32_t dt, const Estimator::State & state,
101+
const control_t & command, bool update_integrators);
97102

98103
Output output_;
99104

@@ -103,6 +108,8 @@ class Controller : public ParamListenerInterface
103108
PID pitch_rate_;
104109
PID yaw_rate_;
105110

111+
float max_thrust_;
112+
106113
uint64_t prev_time_us_;
107114
};
108115

‎include/interface/comm_link.h‎

Lines changed: 6 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -84,10 +84,12 @@ class CommLinkInterface
8484
};
8585

8686
Mode mode;
87-
Channel x;
88-
Channel y;
89-
Channel z;
90-
Channel F;
87+
Channel Qx;
88+
Channel Qy;
89+
Channel Qz;
90+
Channel Fx;
91+
Channel Fy;
92+
Channel Fz;
9193
};
9294

9395
struct AuxCommand

0 commit comments

Comments
 (0)