Skip to content

Commit 02dd553

Browse files
JMoore5353JMoore5353
committed
feat: switch default ordering of command vector to be [forces, torques] to match mixers
1 parent 320a465 commit 02dd553

File tree

7 files changed

+227
-233
lines changed

7 files changed

+227
-233
lines changed

‎include/command_manager.h‎

Lines changed: 24 additions & 24 deletions
Original file line numberDiff line numberDiff line change
@@ -111,52 +111,52 @@ class CommandManager : public ParamListenerInterface
111111

112112
// clang-format off
113113
control_t rc_command_ = {0,
114-
{{false, ANGLE, 0.0},
115-
{false, ANGLE, 0.0},
116-
{false, RATE, 0.0},
117-
{false, THROTTLE, 0.0},
114+
{{false, THROTTLE, 0.0},
118115
{false, THROTTLE, 0.0},
119116
{false, THROTTLE, 0.0},
117+
{false, ANGLE, 0.0},
118+
{false, ANGLE, 0.0},
119+
{false, RATE, 0.0},
120120
{false, PASSTHROUGH, 0.0},
121121
{false, PASSTHROUGH, 0.0},
122122
{false, PASSTHROUGH, 0.0},
123123
{false, PASSTHROUGH, 0.0}}};
124124
control_t offboard_command_ = {0,
125-
{{false, ANGLE, 0.0},
126-
{false, ANGLE, 0.0},
127-
{false, RATE, 0.0},
128-
{false, THROTTLE, 0.0},
125+
{{false, THROTTLE, 0.0},
129126
{false, THROTTLE, 0.0},
130127
{false, THROTTLE, 0.0},
128+
{false, ANGLE, 0.0},
129+
{false, ANGLE, 0.0},
130+
{false, RATE, 0.0},
131131
{false, PASSTHROUGH, 0.0},
132132
{false, PASSTHROUGH, 0.0},
133133
{false, PASSTHROUGH, 0.0},
134134
{false, PASSTHROUGH, 0.0}}};
135135
control_t combined_command_ = {0,
136-
{{false, ANGLE, 0.0},
137-
{false, ANGLE, 0.0},
138-
{false, RATE, 0.0},
139-
{false, THROTTLE, 0.0},
136+
{{false, THROTTLE, 0.0},
140137
{false, THROTTLE, 0.0},
141138
{false, THROTTLE, 0.0},
139+
{false, ANGLE, 0.0},
140+
{false, ANGLE, 0.0},
141+
{false, RATE, 0.0},
142142
{false, PASSTHROUGH, 0.0},
143143
{false, PASSTHROUGH, 0.0},
144144
{false, PASSTHROUGH, 0.0},
145145
{false, PASSTHROUGH, 0.0}}};
146146

147147
control_t multirotor_failsafe_command_ = {0,
148-
{{true, ANGLE, 0.0},
149-
{true, ANGLE, 0.0},
150-
{true, RATE, 0.0},
151-
{true, THROTTLE, 0.0},
148+
{{true, THROTTLE, 0.0},
152149
{true, THROTTLE, 0.0},
153150
{true, THROTTLE, 0.0},
151+
{true, ANGLE, 0.0},
152+
{true, ANGLE, 0.0},
153+
{true, RATE, 0.0},
154154
{false, PASSTHROUGH, 0.0},
155155
{false, PASSTHROUGH, 0.0},
156156
{false, PASSTHROUGH, 0.0},
157157
{false, PASSTHROUGH, 0.0}}};
158158
control_t fixedwing_failsafe_command_ = {0,
159-
{{true, PASSTHROUGH, 0.0},
159+
{{true, PASSTHROUGH, 0.0},
160160
{true, PASSTHROUGH, 0.0},
161161
{true, PASSTHROUGH, 0.0},
162162
{true, PASSTHROUGH, 0.0},
@@ -176,12 +176,12 @@ class CommandManager : public ParamListenerInterface
176176

177177
enum MuxChannel
178178
{
179-
MUX_QX,
180-
MUX_QY,
181-
MUX_QZ,
182179
MUX_FX,
183180
MUX_FY,
184181
MUX_FZ,
182+
MUX_QX,
183+
MUX_QY,
184+
MUX_QZ,
185185
NUM_MUX_CHANNELS
186186
};
187187

@@ -195,13 +195,13 @@ class CommandManager : public ParamListenerInterface
195195
} channel_override_t;
196196

197197
channel_override_t channel_override_[6] = {
198-
{RC::STICK_X, 0, OVERRIDE_X, OVERRIDE_OFFBOARD_X_INACTIVE, X_OVERRIDDEN},
199-
{RC::STICK_Y, 0, OVERRIDE_Y, OVERRIDE_OFFBOARD_Y_INACTIVE, Y_OVERRIDDEN},
200-
{RC::STICK_Z, 0, OVERRIDE_Z, OVERRIDE_OFFBOARD_Z_INACTIVE, Z_OVERRIDDEN},
201198
{RC::STICK_F, 0, OVERRIDE_T, OVERRIDE_OFFBOARD_T_INACTIVE, T_OVERRIDDEN},
202199
{RC::STICK_F, 0, OVERRIDE_T, OVERRIDE_OFFBOARD_T_INACTIVE, T_OVERRIDDEN},
203200
{RC::STICK_F, 0, OVERRIDE_T, OVERRIDE_OFFBOARD_T_INACTIVE,
204-
T_OVERRIDDEN} // Note that throttle overriding works a bit differently
201+
T_OVERRIDDEN}, // Note that throttle overriding works a bit differently
202+
{RC::STICK_X, 0, OVERRIDE_X, OVERRIDE_OFFBOARD_X_INACTIVE, X_OVERRIDDEN},
203+
{RC::STICK_Y, 0, OVERRIDE_Y, OVERRIDE_OFFBOARD_Y_INACTIVE, Y_OVERRIDDEN},
204+
{RC::STICK_Z, 0, OVERRIDE_Z, OVERRIDE_OFFBOARD_Z_INACTIVE, Z_OVERRIDDEN}
205205
};
206206

207207
ROSflight & RF_;

‎include/controller.h‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -52,6 +52,7 @@ class Controller : public ParamListenerInterface
5252
struct Output
5353
{
5454
float u[10];
55+
Output() : u{0,0,0,0,0,0,0,0,0,0} {}
5556
};
5657

5758
Controller(ROSflight & rf);

‎src/comm_manager.cpp‎

Lines changed: 17 additions & 28 deletions
Original file line numberDiff line numberDiff line change
@@ -243,40 +243,31 @@ void CommManager::offboard_control_callback(const CommLinkInterface::OffboardCon
243243
}
244244

245245
// translate modes into standard message
246-
// TODO: Need to standardize which channels are interpreted as what when using modes
247-
// other than the passthrough mode... We should set all unused channels to passthrough?
248246
switch (control.mode) {
249247
case CommLinkInterface::OffboardControl::Mode::PASS_THROUGH:
250-
new_offboard_command.u[0].type = PASSTHROUGH;
251-
new_offboard_command.u[1].type = PASSTHROUGH;
252-
new_offboard_command.u[2].type = PASSTHROUGH;
253-
new_offboard_command.u[3].type = PASSTHROUGH;
254-
new_offboard_command.u[4].type = PASSTHROUGH;
255-
new_offboard_command.u[5].type = PASSTHROUGH;
256-
new_offboard_command.u[6].type = PASSTHROUGH;
257-
new_offboard_command.u[7].type = PASSTHROUGH;
258-
new_offboard_command.u[8].type = PASSTHROUGH;
259-
new_offboard_command.u[9].type = PASSTHROUGH;
248+
for (std::size_t i=0; i<std::size(control.u); ++i) {
249+
new_offboard_command.u[i].type = PASSTHROUGH;
250+
}
260251
break;
261252
case CommLinkInterface::OffboardControl::Mode::ROLLRATE_PITCHRATE_YAWRATE_THROTTLE:
262-
new_offboard_command.u[0].type = RATE;
263-
new_offboard_command.u[1].type = RATE;
264-
new_offboard_command.u[2].type = RATE;
265-
new_offboard_command.u[3].type = THROTTLE;
266-
new_offboard_command.u[4].type = THROTTLE;
267-
new_offboard_command.u[5].type = THROTTLE;
253+
new_offboard_command.u[0].type = THROTTLE;
254+
new_offboard_command.u[1].type = THROTTLE;
255+
new_offboard_command.u[2].type = THROTTLE;
256+
new_offboard_command.u[3].type = RATE;
257+
new_offboard_command.u[4].type = RATE;
258+
new_offboard_command.u[5].type = RATE;
268259
new_offboard_command.u[6].type = PASSTHROUGH;
269260
new_offboard_command.u[7].type = PASSTHROUGH;
270261
new_offboard_command.u[8].type = PASSTHROUGH;
271262
new_offboard_command.u[9].type = PASSTHROUGH;
272263
break;
273264
case CommLinkInterface::OffboardControl::Mode::ROLL_PITCH_YAWRATE_THROTTLE:
274-
new_offboard_command.u[0].type = ANGLE;
275-
new_offboard_command.u[1].type = ANGLE;
276-
new_offboard_command.u[2].type = RATE;
277-
new_offboard_command.u[3].type = THROTTLE;
278-
new_offboard_command.u[4].type = THROTTLE;
279-
new_offboard_command.u[5].type = THROTTLE;
265+
new_offboard_command.u[0].type = THROTTLE;
266+
new_offboard_command.u[1].type = THROTTLE;
267+
new_offboard_command.u[2].type = THROTTLE;
268+
new_offboard_command.u[3].type = ANGLE;
269+
new_offboard_command.u[4].type = ANGLE;
270+
new_offboard_command.u[5].type = RATE;
280271
new_offboard_command.u[6].type = PASSTHROUGH;
281272
new_offboard_command.u[7].type = PASSTHROUGH;
282273
new_offboard_command.u[8].type = PASSTHROUGH;
@@ -370,11 +361,9 @@ void CommManager::send_status(void)
370361

371362
uint8_t control_mode = 0;
372363
if (RF_.params_.get_param_int(PARAM_FIXED_WING)
373-
// TODO: This works, since the first value in the control vector is interpreted as the attitude
374-
// commands (previously the Qx field). It feels a bit fragile... Is there a more robust way to do this?
375-
|| RF_.command_manager_.combined_control().u[0].type == PASSTHROUGH) {
364+
|| RF_.command_manager_.combined_control().u[3].type == PASSTHROUGH) {
376365
control_mode = MODE_PASS_THROUGH;
377-
} else if (RF_.command_manager_.combined_control().u[0].type == ANGLE) {
366+
} else if (RF_.command_manager_.combined_control().u[3].type == ANGLE) {
378367
control_mode = MODE_ROLL_PITCH_YAWRATE_THROTTLE;
379368
} else {
380369
control_mode = MODE_ROLLRATE_PITCHRATE_YAWRATE_THROTTLE;

‎src/command_manager.cpp‎

Lines changed: 28 additions & 28 deletions
Original file line numberDiff line numberDiff line change
@@ -78,18 +78,18 @@ void CommandManager::init_failsafe()
7878
// Make sure the failsafe is set to the axis associated with the RC F command
7979
switch (static_cast<rc_f_axis_t>(RF_.params_.get_param_int(PARAM_RC_F_AXIS))) {
8080
case X_AXIS:
81-
multirotor_failsafe_command_.u[3].value = failsafe_thr_param;
81+
multirotor_failsafe_command_.u[0].value = failsafe_thr_param;
8282
break;
8383
case Y_AXIS:
84-
multirotor_failsafe_command_.u[4].value = failsafe_thr_param;
84+
multirotor_failsafe_command_.u[1].value = failsafe_thr_param;
8585
break;
8686
case Z_AXIS:
87-
multirotor_failsafe_command_.u[5].value = failsafe_thr_param;
87+
multirotor_failsafe_command_.u[2].value = failsafe_thr_param;
8888
break;
8989
default:
9090
RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_WARNING,
9191
"Invalid RC F axis. Defaulting to z-axis.");
92-
multirotor_failsafe_command_.u[5].value = failsafe_thr_param;
92+
multirotor_failsafe_command_.u[2].value = failsafe_thr_param;
9393
break;
9494
}
9595

@@ -103,28 +103,28 @@ void CommandManager::init_failsafe()
103103
void CommandManager::interpret_rc(void)
104104
{
105105
// get initial, unscaled RC values
106-
rc_command_.u[0].value = RF_.rc_.stick(RC::STICK_X);
107-
rc_command_.u[1].value = RF_.rc_.stick(RC::STICK_Y);
108-
rc_command_.u[2].value = RF_.rc_.stick(RC::STICK_Z);
106+
rc_command_.u[3].value = RF_.rc_.stick(RC::STICK_X);
107+
rc_command_.u[4].value = RF_.rc_.stick(RC::STICK_Y);
108+
rc_command_.u[5].value = RF_.rc_.stick(RC::STICK_Z);
109109

110110
// Load the RC command based on the axis associated with the RC F command
111-
rc_command_.u[3].value = 0.0;
112-
rc_command_.u[4].value = 0.0;
113-
rc_command_.u[5].value = 0.0;
111+
rc_command_.u[0].value = 0.0;
112+
rc_command_.u[1].value = 0.0;
113+
rc_command_.u[2].value = 0.0;
114114
switch (static_cast<rc_f_axis_t>(RF_.params_.get_param_int(PARAM_RC_F_AXIS))) {
115115
case X_AXIS: // RC F = X axis
116-
rc_command_.u[3].value = RF_.rc_.stick(RC::STICK_F);
116+
rc_command_.u[0].value = RF_.rc_.stick(RC::STICK_F);
117117
break;
118118
case Y_AXIS: // RC F = Y axis
119-
rc_command_.u[4].value = RF_.rc_.stick(RC::STICK_F);
119+
rc_command_.u[1].value = RF_.rc_.stick(RC::STICK_F);
120120
break;
121121
case Z_AXIS:
122-
rc_command_.u[5].value = RF_.rc_.stick(RC::STICK_F);
122+
rc_command_.u[2].value = RF_.rc_.stick(RC::STICK_F);
123123
break;
124124
default:
125125
RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_WARNING,
126126
"Invalid RC F axis. Defaulting to z-axis.");
127-
rc_command_.u[5].value = RF_.rc_.stick(RC::STICK_F);
127+
rc_command_.u[2].value = RF_.rc_.stick(RC::STICK_F);
128128
break;
129129
}
130130

@@ -146,30 +146,30 @@ void CommandManager::interpret_rc(void)
146146
(RF_.params_.get_param_int(PARAM_RC_ATTITUDE_MODE) == ATT_MODE_RATE) ? RATE : ANGLE;
147147
}
148148

149-
rc_command_.u[0].type = roll_pitch_type;
150-
rc_command_.u[1].type = roll_pitch_type;
149+
rc_command_.u[3].type = roll_pitch_type;
150+
rc_command_.u[4].type = roll_pitch_type;
151151

152152
// Scale command to appropriate units
153153
switch (roll_pitch_type) {
154154
case RATE:
155-
rc_command_.u[0].value *= RF_.params_.get_param_float(PARAM_RC_MAX_ROLLRATE);
156-
rc_command_.u[1].value *= RF_.params_.get_param_float(PARAM_RC_MAX_PITCHRATE);
155+
rc_command_.u[3].value *= RF_.params_.get_param_float(PARAM_RC_MAX_ROLLRATE);
156+
rc_command_.u[4].value *= RF_.params_.get_param_float(PARAM_RC_MAX_PITCHRATE);
157157
break;
158158
case ANGLE:
159-
rc_command_.u[0].value *= RF_.params_.get_param_float(PARAM_RC_MAX_ROLL);
160-
rc_command_.u[1].value *= RF_.params_.get_param_float(PARAM_RC_MAX_PITCH);
159+
rc_command_.u[3].value *= RF_.params_.get_param_float(PARAM_RC_MAX_ROLL);
160+
rc_command_.u[4].value *= RF_.params_.get_param_float(PARAM_RC_MAX_PITCH);
161161
default:
162162
break;
163163
}
164164

165165
// yaw
166-
rc_command_.u[2].type = RATE;
167-
rc_command_.u[2].value *= RF_.params_.get_param_float(PARAM_RC_MAX_YAWRATE);
166+
rc_command_.u[5].type = RATE;
167+
rc_command_.u[5].value *= RF_.params_.get_param_float(PARAM_RC_MAX_YAWRATE);
168168

169169
// throttle
170-
rc_command_.u[3].type = THROTTLE;
171-
rc_command_.u[4].type = THROTTLE;
172-
rc_command_.u[5].type = THROTTLE;
170+
rc_command_.u[0].type = THROTTLE;
171+
rc_command_.u[1].type = THROTTLE;
172+
rc_command_.u[2].type = THROTTLE;
173173
}
174174
}
175175

@@ -313,12 +313,12 @@ bool CommandManager::run()
313313
if (RF_.board_.clock_millis()
314314
> offboard_command_.stamp_ms + RF_.params_.get_param_int(PARAM_OFFBOARD_TIMEOUT)) {
315315
// If it has been longer than 100 ms, then disable the offboard control
316-
offboard_command_.u[3].active = false;
317-
offboard_command_.u[4].active = false;
318-
offboard_command_.u[5].active = false;
319316
offboard_command_.u[0].active = false;
320317
offboard_command_.u[1].active = false;
321318
offboard_command_.u[2].active = false;
319+
offboard_command_.u[3].active = false;
320+
offboard_command_.u[4].active = false;
321+
offboard_command_.u[5].active = false;
322322
}
323323

324324
// Perform muxing

0 commit comments

Comments
 (0)