Skip to content

Commit d819b3b

Browse files
bkleinerbkleiner
committed
output: add logical mixer rules
1 parent acb4c8a commit d819b3b

16 files changed

Lines changed: 392 additions & 295 deletions

File tree

‎src/config/config.h‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -279,4 +279,3 @@
279279

280280
// ---- DEVELOPMENT OPTIONS ----
281281
// #define NOMOTORS // disable motor output for bench testing
282-
// #define MOTOR_PLUS_CONFIGURATION // use + motor layout instead of X

‎src/control/control.h‎

Lines changed: 64 additions & 59 deletions
Original file line numberDiff line numberDiff line change
@@ -3,10 +3,11 @@
33
#include <stdint.h>
44

55
#include "core/failloop.h"
6+
#include "core/profile.h"
67
#include "core/project.h"
78
#include "core/scheduler.h"
8-
#include "rx/rx.h"
99
#include "io/gps.h"
10+
#include "rx/rx.h"
1011
#include "util/vector.h"
1112

1213
// Throttle must drop below this value if arming feature is enabled for arming to take place.
@@ -146,71 +147,75 @@ typedef struct {
146147
vec3_t pid_d_term;
147148
vec3_t pidoutput; // combinded output of the pid controller
148149

149-
vec4_t motor_mix;
150+
float mixer_source[OUTPUT_SOURCE_MAX];
151+
float output[MOTOR_PIN_MAX];
152+
uint8_t output_active[MOTOR_PIN_MAX];
150153

151154
vec3_t angle_error;
152155
vec3_t stick_vector;
153156

154157
uint32_t dshot_rpm[4];
155158
} control_state_t;
156159

157-
#define STATE_MEMBERS \
158-
MEMBER(failloop, uint8_t) \
159-
MEMBER(looptime, float) \
160-
MEMBER(looptime_us, float) \
161-
MEMBER(looptime_autodetect, float) \
162-
MEMBER(looptime_inverse, float) \
163-
MEMBER(loop_counter, uint32_t) \
164-
MEMBER(uptime, float) \
165-
MEMBER(armtime, float) \
166-
MEMBER(cpu_load, uint32_t) \
167-
MEMBER(failsafe_time_ms, uint32_t) \
168-
MEMBER(lipo_cell_count, uint8_t) \
169-
MEMBER(cpu_temp, float) \
170-
MEMBER(vbat, float) \
171-
MEMBER(vbat_filtered, float) \
172-
MEMBER(vbat_sag_filtered, float) \
173-
MEMBER(vbat_cell_avg, float) \
174-
MEMBER(vbat_compensated, float) \
175-
MEMBER(vbat_compensated_cell_avg, float) \
176-
MEMBER(ibat, float) \
177-
MEMBER(ibat_filtered, float) \
178-
MEMBER(ibat_sag_filtered, float) \
179-
MEMBER(ibat_drawn, float) \
180-
MEMBER(rx, vec4_t) \
181-
MEMBER(rx_filtered, vec4_t) \
182-
MEMBER(rx_override, vec4_t) \
183-
ARRAY_MEMBER(rx_channels, RX_CHANNEL_MAX, uint16_t) \
184-
MEMBER(stick_calibration_wizard, uint8_t) \
185-
MEMBER(rx_rssi, float) \
186-
MEMBER(rx_status, uint32_t) \
187-
MEMBER(throttle, float) \
188-
MEMBER(thrsum, float) \
189-
MEMBER(aux_active, uint32_t) \
190-
MEMBER(accel_raw, vec3_t) \
191-
MEMBER(accel, vec3_t) \
192-
MEMBER(gyro_temp, float) \
193-
MEMBER(gyro_raw, vec3_t) \
194-
MEMBER(gyro, vec3_t) \
195-
MEMBER(gyro_delta_angle, vec3_t) \
196-
MEMBER(GEstG, vec3_t) \
197-
MEMBER(attitude, vec3_t) \
198-
MEMBER(gps_lock, bool) \
199-
MEMBER(gps_sats, uint8_t) \
200-
MEMBER(gps_speed, float) \
201-
MEMBER(gps_heading, float) \
202-
MEMBER(gps_heading_accuracy, float) \
203-
MEMBER(gps_coord, gps_coord_t) \
204-
MEMBER(gps_altitude, float) \
205-
MEMBER(setpoint, vec3_t) \
206-
MEMBER(error, vec3_t) \
207-
MEMBER(pid_p_term, vec3_t) \
208-
MEMBER(pid_i_term, vec3_t) \
209-
MEMBER(pid_d_term, vec3_t) \
210-
MEMBER(pidoutput, vec3_t) \
211-
MEMBER(motor_mix, vec4_t) \
212-
MEMBER(angle_error, vec3_t) \
213-
MEMBER(stick_vector, vec3_t) \
160+
#define STATE_MEMBERS \
161+
MEMBER(failloop, uint8_t) \
162+
MEMBER(looptime, float) \
163+
MEMBER(looptime_us, float) \
164+
MEMBER(looptime_autodetect, float) \
165+
MEMBER(looptime_inverse, float) \
166+
MEMBER(loop_counter, uint32_t) \
167+
MEMBER(uptime, float) \
168+
MEMBER(armtime, float) \
169+
MEMBER(cpu_load, uint32_t) \
170+
MEMBER(failsafe_time_ms, uint32_t) \
171+
MEMBER(lipo_cell_count, uint8_t) \
172+
MEMBER(cpu_temp, float) \
173+
MEMBER(vbat, float) \
174+
MEMBER(vbat_filtered, float) \
175+
MEMBER(vbat_sag_filtered, float) \
176+
MEMBER(vbat_cell_avg, float) \
177+
MEMBER(vbat_compensated, float) \
178+
MEMBER(vbat_compensated_cell_avg, float) \
179+
MEMBER(ibat, float) \
180+
MEMBER(ibat_filtered, float) \
181+
MEMBER(ibat_sag_filtered, float) \
182+
MEMBER(ibat_drawn, float) \
183+
MEMBER(rx, vec4_t) \
184+
MEMBER(rx_filtered, vec4_t) \
185+
MEMBER(rx_override, vec4_t) \
186+
ARRAY_MEMBER(rx_channels, RX_CHANNEL_MAX, uint16_t) \
187+
MEMBER(stick_calibration_wizard, uint8_t) \
188+
MEMBER(rx_rssi, float) \
189+
MEMBER(rx_status, uint32_t) \
190+
MEMBER(throttle, float) \
191+
MEMBER(thrsum, float) \
192+
MEMBER(aux_active, uint32_t) \
193+
MEMBER(accel_raw, vec3_t) \
194+
MEMBER(accel, vec3_t) \
195+
MEMBER(gyro_temp, float) \
196+
MEMBER(gyro_raw, vec3_t) \
197+
MEMBER(gyro, vec3_t) \
198+
MEMBER(gyro_delta_angle, vec3_t) \
199+
MEMBER(GEstG, vec3_t) \
200+
MEMBER(attitude, vec3_t) \
201+
MEMBER(gps_lock, bool) \
202+
MEMBER(gps_sats, uint8_t) \
203+
MEMBER(gps_speed, float) \
204+
MEMBER(gps_heading, float) \
205+
MEMBER(gps_heading_accuracy, float) \
206+
MEMBER(gps_coord, gps_coord_t) \
207+
MEMBER(gps_altitude, float) \
208+
MEMBER(setpoint, vec3_t) \
209+
MEMBER(error, vec3_t) \
210+
MEMBER(pid_p_term, vec3_t) \
211+
MEMBER(pid_i_term, vec3_t) \
212+
MEMBER(pid_d_term, vec3_t) \
213+
MEMBER(pidoutput, vec3_t) \
214+
ARRAY_MEMBER(mixer_source, OUTPUT_SOURCE_MAX, float) \
215+
ARRAY_MEMBER(output, MOTOR_PIN_MAX, float) \
216+
ARRAY_MEMBER(output_active, MOTOR_PIN_MAX, uint8_t) \
217+
MEMBER(angle_error, vec3_t) \
218+
MEMBER(stick_vector, vec3_t) \
214219
ARRAY_MEMBER(dshot_rpm, 4, uint32_t)
215220

216221
typedef struct {

‎src/control/multi/control.c‎

Lines changed: 7 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -201,10 +201,11 @@ void control() {
201201
}
202202

203203
if (flags.motortest_override) {
204-
motor_test_calc(motortest_usb, state.motor_mix.axis);
205-
motor_output_calc(state.motor_mix.axis);
204+
motor_test_calc(motortest_usb);
206205
#ifndef NOMOTORS
207-
output_apply_mapped_sources();
206+
output_activate_count(MULTI_MOTOR_COUNT);
207+
output_finalize_motor_values();
208+
output_write_values();
208209
#endif
209210
} else if (!flags.arm_state || flags.failsafe || (state.throttle < 0.001f)) {
210211
flags.on_ground = 1;
@@ -221,10 +222,10 @@ void control() {
221222
}
222223
}
223224

224-
motor_mixer_calc(state.motor_mix.axis);
225-
motor_output_calc(state.motor_mix.axis);
225+
motor_mixer_calc();
226226
#ifndef NOMOTORS
227-
output_apply_mapped_sources();
227+
output_finalize_motor_values();
228+
output_write_values();
228229
#endif
229230
}
230231

‎src/control/multi/motor.c‎

Lines changed: 41 additions & 82 deletions
Original file line numberDiff line numberDiff line change
@@ -47,7 +47,7 @@ static float motord(float in, int x) {
4747
return in + out;
4848
}
4949

50-
static void motor_brushless_mixer_scale_calc(float throttle, float mix[MULTI_MOTOR_COUNT]) {
50+
static void motor_brushless_mixer_scale_calc(float throttle) {
5151
// only enable once really in the air
5252
if (flags.on_ground || !flags.in_air) {
5353
return;
@@ -57,11 +57,11 @@ static void motor_brushless_mixer_scale_calc(float throttle, float mix[MULTI_MOT
5757
float max = FLT_MIN;
5858

5959
for (uint32_t i = 0; i < MULTI_MOTOR_COUNT; i++) {
60-
if (mix[i] < min) {
61-
min = mix[i];
60+
if (state.output[i] < min) {
61+
min = state.output[i];
6262
}
63-
if (mix[i] > max) {
64-
max = mix[i];
63+
if (state.output[i] > max) {
64+
max = state.output[i];
6565
}
6666
}
6767

@@ -73,23 +73,23 @@ static void motor_brushless_mixer_scale_calc(float throttle, float mix[MULTI_MOT
7373
const float scaled_throttle = constrain(throttle, -scaled_min, 1.0f - scaled_max);
7474

7575
for (uint32_t i = 0; i < MULTI_MOTOR_COUNT; i++) {
76-
mix[i] = mix[i] * scale + scaled_throttle;
76+
state.output[i] = state.output[i] * scale + scaled_throttle;
7777
}
7878
}
7979

80-
static void motor_brushed_mixer_scale_calc(float throttle, float mix[MULTI_MOTOR_COUNT]) {
80+
static void motor_brushed_mixer_scale_calc(float throttle) {
8181
// throttle reduction
8282
float overthrottle = 0;
8383
float underthrottle = 0.001f;
8484
static float overthrottlefilt = 0;
8585

8686
for (uint32_t i = 0; i < MULTI_MOTOR_COUNT; i++) {
87-
mix[i] += throttle;
87+
state.output[i] += throttle;
8888

89-
if (mix[i] > overthrottle)
90-
overthrottle = mix[i];
91-
if (mix[i] < underthrottle)
92-
underthrottle = mix[i];
89+
if (state.output[i] > overthrottle)
90+
overthrottle = state.output[i];
91+
if (state.output[i] < underthrottle)
92+
underthrottle = state.output[i];
9393
}
9494

9595
overthrottle -= MIX_MOTOR_MAX;
@@ -118,7 +118,7 @@ static void motor_brushed_mixer_scale_calc(float throttle, float mix[MULTI_MOTOR
118118
if (overthrottle > 0) { // exceeding max motor thrust
119119
float temp = overthrottle;
120120
for (uint32_t i = 0; i < MULTI_MOTOR_COUNT; i++) {
121-
mix[i] -= temp;
121+
state.output[i] -= temp;
122122
}
123123
}
124124

@@ -127,8 +127,8 @@ static void motor_brushed_mixer_scale_calc(float throttle, float mix[MULTI_MOTOR
127127
float underthrottle = 0;
128128

129129
for (uint32_t i = 0; i < MULTI_MOTOR_COUNT; i++) {
130-
if (mix[i] < underthrottle)
131-
underthrottle = mix[i];
130+
if (state.output[i] < underthrottle)
131+
underthrottle = state.output[i];
132132
}
133133

134134
// limit to half throttle max reduction
@@ -137,104 +137,63 @@ static void motor_brushed_mixer_scale_calc(float throttle, float mix[MULTI_MOTOR
137137

138138
if (underthrottle < 0.0f) {
139139
for (uint32_t i = 0; i < MULTI_MOTOR_COUNT; i++)
140-
mix[i] -= underthrottle;
140+
state.output[i] -= underthrottle;
141141
}
142142
}
143143
}
144144

145-
static void motor_mixer_scale_calc(float throttle, float mix[MULTI_MOTOR_COUNT]) {
146-
if (target.brushless) {
147-
return motor_brushless_mixer_scale_calc(throttle, mix);
148-
}
149-
return motor_brushed_mixer_scale_calc(throttle, mix);
150-
}
151-
152-
void motor_test_calc(bool motortest_usb, float mix[MULTI_MOTOR_COUNT]) {
145+
void motor_test_calc(bool motortest_usb) {
153146
if (motortest_usb) {
154147
// set mix according to values we got via usb
155-
mix[MOTOR_FR] = motor_test.value[MOTOR_FR];
156-
mix[MOTOR_FL] = motor_test.value[MOTOR_FL];
157-
mix[MOTOR_BR] = motor_test.value[MOTOR_BR];
158-
mix[MOTOR_BL] = motor_test.value[MOTOR_BL];
148+
state.output[MOTOR_FR] = motor_test.value[MOTOR_FR];
149+
state.output[MOTOR_FL] = motor_test.value[MOTOR_FL];
150+
state.output[MOTOR_BR] = motor_test.value[MOTOR_BR];
151+
state.output[MOTOR_BL] = motor_test.value[MOTOR_BL];
159152
} else {
160153
// set mix according to sticks
161154
if (state.rx_filtered.roll < -0.5f || state.rx_filtered.pitch < -0.5f) {
162-
mix[MOTOR_FR] = 0;
155+
state.output[MOTOR_FR] = 0;
163156
} else {
164-
mix[MOTOR_FR] = state.throttle;
157+
state.output[MOTOR_FR] = state.throttle;
165158
}
166159

167160
if (state.rx_filtered.roll > 0.5f || state.rx_filtered.pitch < -0.5f) {
168-
mix[MOTOR_FL] = 0;
161+
state.output[MOTOR_FL] = 0;
169162
} else {
170-
mix[MOTOR_FL] = state.throttle;
163+
state.output[MOTOR_FL] = state.throttle;
171164
}
172165

173166
if (state.rx_filtered.roll < -0.5f || state.rx_filtered.pitch > 0.5f) {
174-
mix[MOTOR_BR] = 0;
167+
state.output[MOTOR_BR] = 0;
175168
} else {
176-
mix[MOTOR_BR] = state.throttle;
169+
state.output[MOTOR_BR] = state.throttle;
177170
}
178171

179172
if (state.rx_filtered.roll > 0.5f || state.rx_filtered.pitch > 0.5f) {
180-
mix[MOTOR_BL] = 0;
173+
state.output[MOTOR_BL] = 0;
181174
} else {
182-
mix[MOTOR_BL] = state.throttle;
175+
state.output[MOTOR_BL] = state.throttle;
183176
}
184177
}
185178
}
186179

187-
void motor_mixer_calc(float mix[MULTI_MOTOR_COUNT]) {
188-
const float yaw = profile.motor.invert_yaw ? -state.pidoutput.yaw : state.pidoutput.yaw;
189-
190-
#ifndef MOTOR_PLUS_CONFIGURATION
191-
// normal mode, we set mix according to pidoutput
192-
mix[MOTOR_FR] = -state.pidoutput.roll - state.pidoutput.pitch + yaw; // FR
193-
mix[MOTOR_FL] = +state.pidoutput.roll - state.pidoutput.pitch - yaw; // FL
194-
mix[MOTOR_BR] = -state.pidoutput.roll + state.pidoutput.pitch - yaw; // BR
195-
mix[MOTOR_BL] = +state.pidoutput.roll + state.pidoutput.pitch + yaw; // BL
196-
#else
197-
// plus mode, we set mix according to pidoutput
198-
mix[MOTOR_FR] = -state.pidoutput.pitch + yaw; // FRONT
199-
mix[MOTOR_FL] = +state.pidoutput.roll - yaw; // LEFT
200-
mix[MOTOR_BR] = -state.pidoutput.roll - yaw; // RIGHT
201-
mix[MOTOR_BL] = +state.pidoutput.pitch + yaw; // BACK
202-
#endif
180+
void motor_mixer_calc(void) {
181+
state.mixer_source[OUTPUT_SOURCE_THROTTLE] = state.throttle;
182+
state.mixer_source[OUTPUT_SOURCE_ROLL] = state.pidoutput.roll;
183+
state.mixer_source[OUTPUT_SOURCE_PITCH] = state.pidoutput.pitch;
184+
state.mixer_source[OUTPUT_SOURCE_YAW] = state.pidoutput.yaw;
185+
186+
output_apply_mixer_rules();
203187

204188
if (profile.motor.torque_boost > 0.0f) {
205189
for (uint32_t i = 0; i < MULTI_MOTOR_COUNT; i++) {
206-
mix[i] = motord(mix[i], i);
190+
state.output[i] = motord(state.output[i], i);
207191
}
208192
}
209193

210-
motor_mixer_scale_calc(state.throttle, mix);
211-
}
212-
213-
//********************************MOTOR OUTPUT***********************************************************
214-
void motor_output_calc(float mix[MULTI_MOTOR_COUNT]) {
215-
state.thrsum = 0; // reset throttle sum for voltage monitoring logic in main loop
216-
217-
// only apply digital idle if we are armed and not in motor test
218-
float motor_min_value = 0;
219-
if (!flags.on_ground && flags.arm_state && !flags.motortest_override) {
220-
// 0.0001 for legacy purposes, force motor drivers downstream to round up
221-
motor_min_value = 0.0001f + (float)profile.motor.digital_idle * 0.01f;
222-
}
223-
224-
// Begin for-loop to send motor commands
225-
for (uint32_t i = 0; i < MULTI_MOTOR_COUNT; i++) {
226-
if (!flags.motortest_override) {
227-
// use values as supplied in motor test mode
228-
mix[i] = constrain(mix[i], 0, 1);
229-
mix[i] = mapf(mix[i], 0.0f, 1.0f, motor_min_value, profile.motor.motor_limit * 0.01f);
230-
} else if (mix[i] <= 0.0f) {
231-
// zero throttle in motor test must map to MOTOR_OFF so the dshot driver sends stop rather than min throttle
232-
mix[i] = MOTOR_OFF;
233-
}
234-
235-
state.thrsum += mix[i];
194+
if (target.brushless) {
195+
motor_brushless_mixer_scale_calc(state.throttle);
196+
} else {
197+
motor_brushed_mixer_scale_calc(state.throttle);
236198
}
237-
238-
// calculate throttle sum for voltage monitoring logic in main loop
239-
state.thrsum = state.thrsum / MULTI_MOTOR_COUNT;
240199
}

‎src/control/multi/motor.h‎

Lines changed: 2 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -6,7 +6,5 @@
66

77
#define MULTI_MOTOR_COUNT 4
88

9-
void motor_test_calc(bool motortest_usb, float mix[MULTI_MOTOR_COUNT]);
10-
void motor_mixer_calc(float mix[MULTI_MOTOR_COUNT]);
11-
12-
void motor_output_calc(float mix[MULTI_MOTOR_COUNT]);
9+
void motor_test_calc(bool motortest_usb);
10+
void motor_mixer_calc(void);

0 commit comments

Comments
 (0)