Skip to content

Commit beec9c6

Browse files
peterbarkerpeterbarker
committed
Copter: factor out methods for guided-mode commands
1 parent 860498d commit beec9c6

File tree

2 files changed

+43
-37
lines changed

2 files changed

+43
-37
lines changed

‎ArduCopter/GCS_Mavlink.cpp

Lines changed: 39 additions & 37 deletions
Original file line numberDiff line numberDiff line change
@@ -1162,8 +1162,6 @@ bool GCS_MAVLINK_Copter::sane_vel_or_acc_vector(const Vector3f &vec) const
11621162
return true;
11631163
}
11641164

1165-
void GCS_MAVLINK_Copter::handle_message(const mavlink_message_t &msg)
1166-
{
11671165
#if MODE_GUIDED_ENABLED == ENABLED
11681166
// for mavlink SET_POSITION_TARGET messages
11691167
constexpr uint32_t MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE =
@@ -1189,18 +1187,16 @@ void GCS_MAVLINK_Copter::handle_message(const mavlink_message_t &msg)
11891187
POSITION_TARGET_TYPEMASK_FORCE_SET;
11901188
#endif
11911189

1192-
switch (msg.msgid) {
1193-
11941190
#if MODE_GUIDED_ENABLED == ENABLED
1195-
case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET: // MAV ID: 82
1196-
{
1191+
void GCS_MAVLINK_Copter::handle_message_set_attitude_target(const mavlink_message_t &msg)
1192+
{
11971193
// decode packet
11981194
mavlink_set_attitude_target_t packet;
11991195
mavlink_msg_set_attitude_target_decode(&msg, &packet);
12001196

12011197
// exit if vehicle is not in Guided mode or Auto-Guided mode
12021198
if (!copter.flightmode->in_guided_mode()) {
1203-
break;
1199+
return;
12041200
}
12051201

12061202
const bool roll_rate_ignore = packet.type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_ROLL_RATE_IGNORE;
@@ -1211,7 +1207,7 @@ void GCS_MAVLINK_Copter::handle_message(const mavlink_message_t &msg)
12111207

12121208
// ensure thrust field is not ignored
12131209
if (throttle_ignore) {
1214-
break;
1210+
return;
12151211
}
12161212

12171213
Quaternion attitude_quat;
@@ -1225,7 +1221,7 @@ void GCS_MAVLINK_Copter::handle_message(const mavlink_message_t &msg)
12251221
// this limit is somewhat greater than sqrt(FLT_EPSL)
12261222
if (!attitude_quat.is_unit_length()) {
12271223
// The attitude quaternion is ill-defined
1228-
break;
1224+
return;
12291225
}
12301226
}
12311227

@@ -1263,19 +1259,17 @@ void GCS_MAVLINK_Copter::handle_message(const mavlink_message_t &msg)
12631259

12641260
copter.mode_guided.set_angle(attitude_quat, ang_vel,
12651261
climb_rate_or_thrust, use_thrust);
1262+
}
12661263

1267-
break;
1268-
}
1269-
1270-
case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: // MAV ID: 84
1271-
{
1264+
void GCS_MAVLINK_Copter::handle_message_set_position_target_local_ned(const mavlink_message_t &msg)
1265+
{
12721266
// decode packet
12731267
mavlink_set_position_target_local_ned_t packet;
12741268
mavlink_msg_set_position_target_local_ned_decode(&msg, &packet);
12751269

12761270
// exit if vehicle is not in Guided mode or Auto-Guided mode
12771271
if (!copter.flightmode->in_guided_mode()) {
1278-
break;
1272+
return;
12791273
}
12801274

12811275
// check for supported coordinate frames
@@ -1285,7 +1279,7 @@ void GCS_MAVLINK_Copter::handle_message(const mavlink_message_t &msg)
12851279
packet.coordinate_frame != MAV_FRAME_BODY_OFFSET_NED) {
12861280
// input is not valid so stop
12871281
copter.mode_guided.init(true);
1288-
break;
1282+
return;
12891283
}
12901284

12911285
bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE;
@@ -1298,7 +1292,7 @@ void GCS_MAVLINK_Copter::handle_message(const mavlink_message_t &msg)
12981292
// Force inputs are not supported
12991293
// Do not accept command if force_set is true and acc_ignore is false
13001294
if (force_set && !acc_ignore) {
1301-
break;
1295+
return;
13021296
}
13031297

13041298
// prepare position
@@ -1371,19 +1365,17 @@ void GCS_MAVLINK_Copter::handle_message(const mavlink_message_t &msg)
13711365
// input is not valid so stop
13721366
copter.mode_guided.init(true);
13731367
}
1368+
}
13741369

1375-
break;
1376-
}
1377-
1378-
case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT: // MAV ID: 86
1379-
{
1370+
void GCS_MAVLINK_Copter::handle_message_set_position_target_global_int(const mavlink_message_t &msg)
1371+
{
13801372
// decode packet
13811373
mavlink_set_position_target_global_int_t packet;
13821374
mavlink_msg_set_position_target_global_int_decode(&msg, &packet);
13831375

13841376
// exit if vehicle is not in Guided mode or Auto-Guided mode
13851377
if (!copter.flightmode->in_guided_mode()) {
1386-
break;
1378+
return;
13871379
}
13881380

13891381
// todo: do we need to check for supported coordinate frames
@@ -1398,7 +1390,7 @@ void GCS_MAVLINK_Copter::handle_message(const mavlink_message_t &msg)
13981390
// Force inputs are not supported
13991391
// Do not accept command if force_set is true and acc_ignore is false
14001392
if (force_set && !acc_ignore) {
1401-
break;
1393+
return;
14021394
}
14031395

14041396
// extract location from message
@@ -1408,14 +1400,14 @@ void GCS_MAVLINK_Copter::handle_message(const mavlink_message_t &msg)
14081400
if (!check_latlng(packet.lat_int, packet.lon_int)) {
14091401
// input is not valid so stop
14101402
copter.mode_guided.init(true);
1411-
break;
1403+
return;
14121404
}
14131405
Location::AltFrame frame;
14141406
if (!mavlink_coordinate_frame_to_location_alt_frame((MAV_FRAME)packet.coordinate_frame, frame)) {
14151407
// unknown coordinate frame
14161408
// input is not valid so stop
14171409
copter.mode_guided.init(true);
1418-
break;
1410+
return;
14191411
}
14201412
loc = {packet.lat_int, packet.lon_int, int32_t(packet.alt*100), frame};
14211413
}
@@ -1456,13 +1448,13 @@ void GCS_MAVLINK_Copter::handle_message(const mavlink_message_t &msg)
14561448
// posvel controller does not support alt-above-terrain
14571449
// input is not valid so stop
14581450
copter.mode_guided.init(true);
1459-
break;
1451+
return;
14601452
}
14611453
Vector3f pos_neu_cm;
14621454
if (!loc.get_vector_from_origin_NEU(pos_neu_cm)) {
14631455
// input is not valid so stop
14641456
copter.mode_guided.init(true);
1465-
break;
1457+
return;
14661458
}
14671459
copter.mode_guided.set_destination_posvel(pos_neu_cm, vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds);
14681460
} else if (pos_ignore && !vel_ignore) {
@@ -1475,30 +1467,40 @@ void GCS_MAVLINK_Copter::handle_message(const mavlink_message_t &msg)
14751467
// input is not valid so stop
14761468
copter.mode_guided.init(true);
14771469
}
1470+
}
1471+
#endif // MODE_GUIDED_ENABLED == ENABLED
1472+
1473+
void GCS_MAVLINK_Copter::handle_message(const mavlink_message_t &msg)
1474+
{
14781475

1476+
switch (msg.msgid) {
1477+
#if MODE_GUIDED_ENABLED == ENABLED
1478+
case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET:
1479+
handle_message_set_attitude_target(msg);
1480+
break;
1481+
case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED:
1482+
handle_message_set_position_target_local_ned(msg);
1483+
break;
1484+
case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT:
1485+
handle_message_set_position_target_global_int(msg);
14791486
break;
1480-
}
14811487
#endif
1482-
1488+
#if AP_TERRAIN_AVAILABLE
14831489
case MAVLINK_MSG_ID_TERRAIN_DATA:
14841490
case MAVLINK_MSG_ID_TERRAIN_CHECK:
1485-
#if AP_TERRAIN_AVAILABLE
14861491
copter.terrain.handle_data(chan, msg);
1487-
#endif
14881492
break;
1489-
1493+
#endif
14901494
#if TOY_MODE_ENABLED == ENABLED
14911495
case MAVLINK_MSG_ID_NAMED_VALUE_INT:
14921496
copter.g2.toy_mode.handle_message(msg);
14931497
break;
14941498
#endif
1495-
14961499
default:
14971500
GCS_MAVLINK::handle_message(msg);
14981501
break;
1499-
} // end switch
1500-
} // end handle mavlink
1501-
1502+
}
1503+
}
15021504

15031505
MAV_RESULT GCS_MAVLINK_Copter::handle_flight_termination(const mavlink_command_int_t &packet) {
15041506
#if ADVANCED_FAILSAFE == ENABLED

‎ArduCopter/GCS_Mavlink.h

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -46,6 +46,10 @@ class GCS_MAVLINK_Copter : public GCS_MAVLINK
4646
void handle_mount_message(const mavlink_message_t &msg) override;
4747
#endif
4848

49+
void handle_message_set_attitude_target(const mavlink_message_t &msg);
50+
void handle_message_set_position_target_global_int(const mavlink_message_t &msg);
51+
void handle_message_set_position_target_local_ned(const mavlink_message_t &msg);
52+
4953
void handle_landing_target(const mavlink_landing_target_t &packet, uint32_t timestamp_ms) override;
5054

5155
void send_nav_controller_output() const override;

0 commit comments

Comments
 (0)