Skip to content

Commit 6acd5dc

Browse files
JMoore5353JMoore5353
committed
add unit tests for command manager get rc override function
1 parent 5ebf1b9 commit 6acd5dc

File tree

6 files changed

+127
-27
lines changed

6 files changed

+127
-27
lines changed

‎comms/mavlink/mavlink.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -260,7 +260,7 @@ void Mavlink::send_sonar(uint8_t system_id,
260260
send_message(msg);
261261
}
262262

263-
void Mavlink::send_status(uint8_t system_id, bool armed, bool failsafe, uint8_t rc_override,
263+
void Mavlink::send_status(uint8_t system_id, bool armed, bool failsafe, uint16_t rc_override,
264264
bool offboard, uint8_t error_code, uint8_t control_mode,
265265
int16_t num_errors, int16_t loop_time_us)
266266
{

‎comms/mavlink/mavlink.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -79,7 +79,7 @@ class Mavlink : public CommLinkInterface
7979
void send_sonar(uint8_t system_id,
8080
/* TODO enum type*/ uint8_t type, float range, float max_range,
8181
float min_range) override;
82-
void send_status(uint8_t system_id, bool armed, bool failsafe, uint8_t rc_override, bool offboard,
82+
void send_status(uint8_t system_id, bool armed, bool failsafe, uint16_t rc_override, bool offboard,
8383
uint8_t error_code, uint8_t control_mode, int16_t num_errors,
8484
int16_t loop_time_us) override;
8585
void send_timesync(uint8_t system_id, int64_t tc1, int64_t ts1) override;

‎include/comm_link.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -156,7 +156,7 @@ class CommLinkInterface
156156
virtual void send_sonar(uint8_t system_id,
157157
/* TODO enum type*/ uint8_t type, float range, float max_range,
158158
float min_range) = 0;
159-
virtual void send_status(uint8_t system_id, bool armed, bool failsafe, uint8_t rc_override,
159+
virtual void send_status(uint8_t system_id, bool armed, bool failsafe, uint16_t rc_override,
160160
bool offboard, uint8_t error_code, uint8_t control_mode,
161161
int16_t num_errors, int16_t loop_time_us) = 0;
162162
virtual void send_timesync(uint8_t system_id, int64_t tc1, int64_t ts1) = 0;

‎include/command_manager.h‎

Lines changed: 23 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -78,6 +78,27 @@ typedef struct
7878

7979
class CommandManager : public ParamListenerInterface
8080
{
81+
public:
82+
enum RCOverrideReason : uint16_t
83+
{
84+
OVERRIDE_NO_OVERRIDE = 0x0,
85+
OVERRIDE_ATT_SWITCH = 0x1,
86+
OVERRIDE_THR_SWITCH = 0x2,
87+
OVERRIDE_X = 0x4,
88+
OVERRIDE_Y = 0x8,
89+
OVERRIDE_Z = 0x10,
90+
OVERRIDE_T = 0x20,
91+
OVERRIDE_OFFBOARD_X_INACTIVE = 0x40,
92+
OVERRIDE_OFFBOARD_Y_INACTIVE = 0x80,
93+
OVERRIDE_OFFBOARD_Z_INACTIVE = 0x100,
94+
OVERRIDE_OFFBOARD_T_INACTIVE = 0x200,
95+
};
96+
static constexpr uint16_t X_OVERRIDDEN{OVERRIDE_ATT_SWITCH | OVERRIDE_X | OVERRIDE_OFFBOARD_X_INACTIVE};
97+
static constexpr uint16_t Y_OVERRIDDEN{OVERRIDE_ATT_SWITCH | OVERRIDE_Y | OVERRIDE_OFFBOARD_Y_INACTIVE};
98+
static constexpr uint16_t Z_OVERRIDDEN{OVERRIDE_ATT_SWITCH | OVERRIDE_Z | OVERRIDE_OFFBOARD_Z_INACTIVE};
99+
static constexpr uint16_t T_OVERRIDDEN{OVERRIDE_THR_SWITCH | OVERRIDE_T | OVERRIDE_OFFBOARD_T_INACTIVE};
100+
static constexpr uint16_t ATTITUDE_OVERRIDDEN{X_OVERRIDDEN | Y_OVERRIDDEN | Z_OVERRIDDEN};
101+
81102
private:
82103
typedef struct
83104
{
@@ -149,21 +170,6 @@ class CommandManager : public ParamListenerInterface
149170
NUM_MUX_CHANNELS
150171
};
151172

152-
enum RCOverrideReason : uint16_t
153-
{
154-
OVERRIDE_NO_OVERRIDE = 0x0,
155-
OVERRIDE_ATT_SWITCH = 0x1,
156-
OVERRIDE_THR_SWITCH = 0x2,
157-
OVERRIDE_X = 0x4,
158-
OVERRIDE_Y = 0x8,
159-
OVERRIDE_Z = 0x10,
160-
OVERRIDE_T = 0x20,
161-
OVERRIDE_OFFBOARD_X_INACTIVE = 0x40,
162-
OVERRIDE_OFFBOARD_Y_INACTIVE = 0x80,
163-
OVERRIDE_OFFBOARD_Z_INACTIVE = 0x100,
164-
OVERRIDE_OFFBOARD_T_INACTIVE = 0x200,
165-
};
166-
167173
typedef struct
168174
{
169175
RC::Stick rc_channel;
@@ -214,12 +220,6 @@ class CommandManager : public ParamListenerInterface
214220
bool stick_deviated(MuxChannel channel);
215221

216222
public:
217-
static constexpr uint16_t X_OVERRIDDEN{OVERRIDE_ATT_SWITCH | OVERRIDE_X | OVERRIDE_OFFBOARD_X_INACTIVE};
218-
static constexpr uint16_t Y_OVERRIDDEN{OVERRIDE_ATT_SWITCH | OVERRIDE_Y | OVERRIDE_OFFBOARD_Y_INACTIVE};
219-
static constexpr uint16_t Z_OVERRIDDEN{OVERRIDE_ATT_SWITCH | OVERRIDE_Z | OVERRIDE_OFFBOARD_Z_INACTIVE};
220-
static constexpr uint16_t T_OVERRIDDEN{OVERRIDE_THR_SWITCH | OVERRIDE_T | OVERRIDE_OFFBOARD_T_INACTIVE};
221-
static constexpr uint16_t ATTITUDE_OVERRIDDEN{X_OVERRIDDEN | Y_OVERRIDDEN | Z_OVERRIDDEN};
222-
223223
CommandManager(ROSflight & _rf);
224224
void init();
225225
bool run();
@@ -235,13 +235,14 @@ class CommandManager : public ParamListenerInterface
235235
* This value is updated if a new RC command is available
236236
* @return A bitfield, with overriden reasons indicated
237237
*/
238-
uint16_t get_rc_override();
238+
uint16_t get_rc_override() const;
239239
bool offboard_control_active();
240240
void set_new_offboard_command(control_t new_offboard_command);
241241
void set_new_rc_command(control_t new_rc_command);
242242
void override_combined_command_with_rc();
243243
inline const control_t & combined_control() const { return combined_command_; }
244244
inline const control_t & rc_control() const { return rc_command_; }
245+
245246
};
246247

247248
} // namespace rosflight_firmware

‎src/command_manager.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -270,7 +270,7 @@ void CommandManager::do_channel_muxing(MuxChannel channel, uint16_t rc_override
270270
*muxes_[channel].combined = override_this_channel ? *muxes_[channel].rc : *muxes_[channel].onboard;
271271
}
272272

273-
uint16_t CommandManager::get_rc_override() { return rc_override_; }
273+
uint16_t CommandManager::get_rc_override() const { return rc_override_; }
274274

275275
bool CommandManager::offboard_control_active()
276276
{

‎test/command_manager_test.cpp‎

Lines changed: 100 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -40,6 +40,14 @@ class CommandManagerTest : public ::testing::Test
4040
{true, THROTTLE, OFFBOARD_FY},
4141
{true, THROTTLE, OFFBOARD_FZ}};
4242

43+
uint16_t default_rc_override = CommandManager::OVERRIDE_X
44+
| CommandManager::OVERRIDE_Y
45+
| CommandManager::OVERRIDE_Z
46+
| CommandManager::OVERRIDE_OFFBOARD_X_INACTIVE
47+
| CommandManager::OVERRIDE_OFFBOARD_Y_INACTIVE
48+
| CommandManager::OVERRIDE_OFFBOARD_Z_INACTIVE
49+
| CommandManager::OVERRIDE_OFFBOARD_T_INACTIVE;
50+
4351
CommandManagerTest()
4452
: mavlink(board)
4553
, rf(board, mavlink)
@@ -205,7 +213,7 @@ TEST_F(CommandManagerTest, DisarmStickReversed)
205213
EXPECT_EQ(rf.state_manager_.state().armed, false);
206214
}
207215

208-
TEST_F(CommandManagerTest, DefaultRCOutputd)
216+
TEST_F(CommandManagerTest, DefaultRCOutput)
209217
{
210218
stepFirmware(600000);
211219

@@ -337,6 +345,7 @@ TEST_F(CommandManagerTest, OffboardCommandMuxNoMinThrottle)
337345
EXPECT_CLOSE(output.Fx.value, OFFBOARD_FX);
338346
EXPECT_CLOSE(output.Fy.value, OFFBOARD_FY);
339347
EXPECT_CLOSE(output.Fz.value, OFFBOARD_FZ);
348+
EXPECT_EQ(rf.command_manager_.get_rc_override(), CommandManager::OVERRIDE_NO_OVERRIDE);
340349
}
341350

342351
TEST_F(CommandManagerTest, OffboardCommandMuxMinThrottle)
@@ -356,6 +365,8 @@ TEST_F(CommandManagerTest, OffboardCommandMuxMinThrottle)
356365
EXPECT_CLOSE(output.Fx.value, 0.0);
357366
EXPECT_CLOSE(output.Fy.value, 0.0);
358367
EXPECT_CLOSE(output.Fz.value, 0.0);
368+
369+
EXPECT_EQ(rf.command_manager_.get_rc_override(), CommandManager::OVERRIDE_T);
359370
}
360371

361372
TEST_F(CommandManagerTest, OffboardCommandMuxRollDeviation)
@@ -370,7 +381,14 @@ TEST_F(CommandManagerTest, OffboardCommandMuxRollDeviation)
370381
EXPECT_CLOSE(output.Qx.value, -0.5 * rf.params_.get_param_float(PARAM_RC_MAX_ROLL));
371382
EXPECT_CLOSE(output.Qy.value, OFFBOARD_QY);
372383
EXPECT_CLOSE(output.Qz.value, OFFBOARD_QZ);
384+
EXPECT_CLOSE(output.Fx.value, 0.0);
385+
EXPECT_CLOSE(output.Fy.value, 0.0);
373386
EXPECT_CLOSE(output.Fz.value, 0.0);
387+
388+
uint16_t correct_rc_override =
389+
CommandManager::OVERRIDE_T
390+
| CommandManager::OVERRIDE_X;
391+
EXPECT_EQ(rf.command_manager_.get_rc_override(), correct_rc_override);
374392
}
375393

376394
TEST_F(CommandManagerTest, OffboardCommandMuxPitchDeviation)
@@ -385,7 +403,14 @@ TEST_F(CommandManagerTest, OffboardCommandMuxPitchDeviation)
385403
EXPECT_CLOSE(output.Qx.value, OFFBOARD_QX);
386404
EXPECT_CLOSE(output.Qy.value, 0.5 * rf.params_.get_param_float(PARAM_RC_MAX_PITCH));
387405
EXPECT_CLOSE(output.Qz.value, OFFBOARD_QZ);
406+
EXPECT_CLOSE(output.Fx.value, 0.0);
407+
EXPECT_CLOSE(output.Fy.value, 0.0);
388408
EXPECT_CLOSE(output.Fz.value, 0.0);
409+
410+
uint16_t correct_rc_override =
411+
CommandManager::OVERRIDE_T
412+
| CommandManager::OVERRIDE_Y;
413+
EXPECT_EQ(rf.command_manager_.get_rc_override(), correct_rc_override);
389414
}
390415

391416
TEST_F(CommandManagerTest, OffboardCommandMuxYawrateDeviation)
@@ -400,7 +425,14 @@ TEST_F(CommandManagerTest, OffboardCommandMuxYawrateDeviation)
400425
EXPECT_CLOSE(output.Qx.value, OFFBOARD_QX);
401426
EXPECT_CLOSE(output.Qy.value, OFFBOARD_QY);
402427
EXPECT_CLOSE(output.Qz.value, -0.5 * rf.params_.get_param_float(PARAM_RC_MAX_YAWRATE));
428+
EXPECT_CLOSE(output.Fx.value, 0.0);
429+
EXPECT_CLOSE(output.Fy.value, 0.0);
403430
EXPECT_CLOSE(output.Fz.value, 0.0);
431+
432+
uint16_t correct_rc_override =
433+
CommandManager::OVERRIDE_T
434+
| CommandManager::OVERRIDE_Z;
435+
EXPECT_EQ(rf.command_manager_.get_rc_override(), correct_rc_override);
404436
}
405437

406438
TEST_F(CommandManagerTest, OffboardCommandMuxLag)
@@ -413,13 +445,17 @@ TEST_F(CommandManagerTest, OffboardCommandMuxLag)
413445

414446
control_t output = rf.command_manager_.combined_control();
415447
EXPECT_CLOSE(output.Qx.value, -0.5 * rf.params_.get_param_float(PARAM_RC_MAX_ROLL));
448+
EXPECT_EQ(rf.command_manager_.get_rc_override(),
449+
rf.command_manager_.get_rc_override() | CommandManager::OVERRIDE_X);
416450

417451
rc_values[0] = 1500; // return stick to center
418452

419453
stepFirmware(500000);
420454
setOffboard(offboard_command);
421455
output = rf.command_manager_.combined_control();
422456
EXPECT_CLOSE(output.Qx.value, 0.0); // lag
457+
EXPECT_EQ(rf.command_manager_.get_rc_override(),
458+
rf.command_manager_.get_rc_override() | CommandManager::OVERRIDE_X);
423459

424460
stepFirmware(600000);
425461
setOffboard(offboard_command);
@@ -430,6 +466,8 @@ TEST_F(CommandManagerTest, OffboardCommandMuxLag)
430466
stepFirmware(20000);
431467
output = rf.command_manager_.combined_control();
432468
EXPECT_CLOSE(output.Qx.value, OFFBOARD_QX);
469+
EXPECT_NE(rf.command_manager_.get_rc_override(),
470+
rf.command_manager_.get_rc_override() | CommandManager::OVERRIDE_X);
433471
}
434472

435473
TEST_F(CommandManagerTest, StaleOffboardCommand)
@@ -443,6 +481,8 @@ TEST_F(CommandManagerTest, StaleOffboardCommand)
443481

444482
control_t output = rf.command_manager_.combined_control();
445483
EXPECT_CLOSE(output.Qx.value, 0.0);
484+
EXPECT_EQ(rf.command_manager_.get_rc_override(),
485+
rf.command_manager_.get_rc_override() | CommandManager::OVERRIDE_OFFBOARD_X_INACTIVE);
446486
}
447487

448488
TEST_F(CommandManagerTest, PartialMux)
@@ -456,7 +496,11 @@ TEST_F(CommandManagerTest, PartialMux)
456496
EXPECT_CLOSE(output.Qx.value, 0.0);
457497
EXPECT_CLOSE(output.Qy.value, OFFBOARD_QY);
458498
EXPECT_CLOSE(output.Qz.value, OFFBOARD_QZ);
499+
EXPECT_CLOSE(output.Fx.value, 0.0);
500+
EXPECT_CLOSE(output.Fy.value, 0.0);
459501
EXPECT_CLOSE(output.Fz.value, 0.0);
502+
EXPECT_EQ(rf.command_manager_.get_rc_override(),
503+
rf.command_manager_.get_rc_override() | CommandManager::OVERRIDE_OFFBOARD_X_INACTIVE);
460504
}
461505

462506
TEST_F(CommandManagerTest, MixedTypes)
@@ -472,3 +516,58 @@ TEST_F(CommandManagerTest, MixedTypes)
472516
EXPECT_EQ(output.Qz.type, RATE);
473517
EXPECT_EQ(output.Fz.type, THROTTLE);
474518
}
519+
520+
TEST_F(CommandManagerTest, DefaultRCOverride)
521+
{
522+
uint16_t override = rf.command_manager_.get_rc_override();
523+
524+
EXPECT_EQ(override, default_rc_override);
525+
}
526+
527+
TEST_F(CommandManagerTest, RCOverrideGivenBothOvrdSwitchesOn)
528+
{
529+
rf.params_.set_param_int(PARAM_RC_ATTITUDE_OVERRIDE_CHANNEL, 4);
530+
rf.params_.set_param_int(PARAM_RC_THROTTLE_OVERRIDE_CHANNEL, 4);
531+
rc_values[4] = CHN_HIGH;
532+
stepFirmware(1000000);
533+
534+
uint16_t override = rf.command_manager_.get_rc_override();
535+
536+
uint16_t correct_override =
537+
CommandManager::OVERRIDE_ATT_SWITCH
538+
| CommandManager::OVERRIDE_THR_SWITCH
539+
| default_rc_override;
540+
EXPECT_EQ(override, correct_override);
541+
}
542+
543+
TEST_F(CommandManagerTest, RCOverrideOnlyThrOvrdSwitchOn)
544+
{
545+
rf.params_.set_param_int(PARAM_RC_ATTITUDE_OVERRIDE_CHANNEL, 5);
546+
rf.params_.set_param_int(PARAM_RC_THROTTLE_OVERRIDE_CHANNEL, 4);
547+
rc_values[4] = CHN_HIGH;
548+
rc_values[5] = CHN_LOW;
549+
stepFirmware(1000000);
550+
551+
uint16_t override = rf.command_manager_.get_rc_override();
552+
553+
uint16_t correct_override =
554+
CommandManager::OVERRIDE_THR_SWITCH
555+
| default_rc_override;
556+
EXPECT_EQ(override, correct_override);
557+
}
558+
559+
TEST_F(CommandManagerTest, RCOverrideOnlyAttOvrdSwitchOn)
560+
{
561+
rf.params_.set_param_int(PARAM_RC_ATTITUDE_OVERRIDE_CHANNEL, 4);
562+
rf.params_.set_param_int(PARAM_RC_THROTTLE_OVERRIDE_CHANNEL, 5);
563+
rc_values[4] = CHN_HIGH;
564+
rc_values[5] = CHN_LOW;
565+
stepFirmware(1000000);
566+
567+
uint16_t override = rf.command_manager_.get_rc_override();
568+
569+
uint16_t correct_override =
570+
CommandManager::OVERRIDE_ATT_SWITCH
571+
| default_rc_override;
572+
EXPECT_EQ(override, correct_override);
573+
}

0 commit comments

Comments
 (0)