Skip to content

Commit

Permalink
Copter: factor out methods for guided-mode commands
Browse files Browse the repository at this point in the history
  • Loading branch information
@peterbarker
peterbarker committed Apr 14, 2024
1 parent 1cafe25 commit 5de6e20
Show file tree
Hide file tree
Showing 2 changed files with 43 additions and 37 deletions.
76 changes: 39 additions & 37 deletions ArduCopter/GCS_Mavlink.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1162,8 +1162,6 @@ bool GCS_MAVLINK_Copter::sane_vel_or_acc_vector(const Vector3f &vec) const
return true;
}

void GCS_MAVLINK_Copter::handle_message(const mavlink_message_t &msg)
{
#if MODE_GUIDED_ENABLED == ENABLED
// for mavlink SET_POSITION_TARGET messages
constexpr uint32_t MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE =
Expand All @@ -1189,18 +1187,16 @@ void GCS_MAVLINK_Copter::handle_message(const mavlink_message_t &msg)
POSITION_TARGET_TYPEMASK_FORCE_SET;
#endif

switch (msg.msgid) {

#if MODE_GUIDED_ENABLED == ENABLED
case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET: // MAV ID: 82
{
void GCS_MAVLINK_Copter::handle_message_set_attitude_target(const mavlink_message_t &msg)
{
// decode packet
mavlink_set_attitude_target_t packet;
mavlink_msg_set_attitude_target_decode(&msg, &packet);

// exit if vehicle is not in Guided mode or Auto-Guided mode
if (!copter.flightmode->in_guided_mode()) {
break;
return;
}

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

// ensure thrust field is not ignored
if (throttle_ignore) {
break;
return;
}

Quaternion attitude_quat;
Expand All @@ -1225,7 +1221,7 @@ void GCS_MAVLINK_Copter::handle_message(const mavlink_message_t &msg)
// this limit is somewhat greater than sqrt(FLT_EPSL)
if (!attitude_quat.is_unit_length()) {
// The attitude quaternion is ill-defined
break;
return;
}
}

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

copter.mode_guided.set_angle(attitude_quat, ang_vel,
climb_rate_or_thrust, use_thrust);
}

break;
}

case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: // MAV ID: 84
{
void GCS_MAVLINK_Copter::handle_message_set_position_target_local_ned(const mavlink_message_t &msg)
{
// decode packet
mavlink_set_position_target_local_ned_t packet;
mavlink_msg_set_position_target_local_ned_decode(&msg, &packet);

// exit if vehicle is not in Guided mode or Auto-Guided mode
if (!copter.flightmode->in_guided_mode()) {
break;
return;
}

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

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

// prepare position
Expand Down Expand Up @@ -1371,19 +1365,17 @@ void GCS_MAVLINK_Copter::handle_message(const mavlink_message_t &msg)
// input is not valid so stop
copter.mode_guided.init(true);
}
}

break;
}

case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT: // MAV ID: 86
{
void GCS_MAVLINK_Copter::handle_message_set_position_target_global_int(const mavlink_message_t &msg)
{
// decode packet
mavlink_set_position_target_global_int_t packet;
mavlink_msg_set_position_target_global_int_decode(&msg, &packet);

// exit if vehicle is not in Guided mode or Auto-Guided mode
if (!copter.flightmode->in_guided_mode()) {
break;
return;
}

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

// extract location from message
Expand All @@ -1408,14 +1400,14 @@ void GCS_MAVLINK_Copter::handle_message(const mavlink_message_t &msg)
if (!check_latlng(packet.lat_int, packet.lon_int)) {
// input is not valid so stop
copter.mode_guided.init(true);
break;
return;
}
Location::AltFrame frame;
if (!mavlink_coordinate_frame_to_location_alt_frame((MAV_FRAME)packet.coordinate_frame, frame)) {
// unknown coordinate frame
// input is not valid so stop
copter.mode_guided.init(true);
break;
return;
}
loc = {packet.lat_int, packet.lon_int, int32_t(packet.alt*100), frame};
}
Expand Down Expand Up @@ -1456,13 +1448,13 @@ void GCS_MAVLINK_Copter::handle_message(const mavlink_message_t &msg)
// posvel controller does not support alt-above-terrain
// input is not valid so stop
copter.mode_guided.init(true);
break;
return;
}
Vector3f pos_neu_cm;
if (!loc.get_vector_from_origin_NEU(pos_neu_cm)) {
// input is not valid so stop
copter.mode_guided.init(true);
break;
return;
}
copter.mode_guided.set_destination_posvel(pos_neu_cm, vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds);
} else if (pos_ignore && !vel_ignore) {
Expand All @@ -1475,30 +1467,40 @@ void GCS_MAVLINK_Copter::handle_message(const mavlink_message_t &msg)
// input is not valid so stop
copter.mode_guided.init(true);
}
}
#endif // MODE_GUIDED_ENABLED == ENABLED

void GCS_MAVLINK_Copter::handle_message(const mavlink_message_t &msg)
{

switch (msg.msgid) {
#if MODE_GUIDED_ENABLED == ENABLED
case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET:
handle_message_set_attitude_target(msg);
break;
case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED:
handle_message_set_position_target_local_ned(msg);
break;
case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT:
handle_message_set_position_target_global_int(msg);
break;
}
#endif

#if AP_TERRAIN_AVAILABLE
case MAVLINK_MSG_ID_TERRAIN_DATA:
case MAVLINK_MSG_ID_TERRAIN_CHECK:
#if AP_TERRAIN_AVAILABLE
copter.terrain.handle_data(chan, msg);
#endif
break;

#endif
#if TOY_MODE_ENABLED == ENABLED
case MAVLINK_MSG_ID_NAMED_VALUE_INT:
copter.g2.toy_mode.handle_message(msg);
break;
#endif

default:
GCS_MAVLINK::handle_message(msg);
break;
} // end switch
} // end handle mavlink

}
}

MAV_RESULT GCS_MAVLINK_Copter::handle_flight_termination(const mavlink_command_int_t &packet) {
#if ADVANCED_FAILSAFE == ENABLED
Expand Down
4 changes: 4 additions & 0 deletions ArduCopter/GCS_Mavlink.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,6 +46,10 @@ class GCS_MAVLINK_Copter : public GCS_MAVLINK
void handle_mount_message(const mavlink_message_t &msg) override;
#endif

void handle_message_set_attitude_target(const mavlink_message_t &msg);
void handle_message_set_position_target_global_int(const mavlink_message_t &msg);
void handle_message_set_position_target_local_ned(const mavlink_message_t &msg);

void handle_landing_target(const mavlink_landing_target_t &packet, uint32_t timestamp_ms) override;

void send_nav_controller_output() const override;
Expand Down

0 comments on commit 5de6e20

Please sign in to comment.