Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: factor out methods for guided-mode commands

This commit is contained in:
Peter Barker 2024-04-12 13:28:43 +10:00 committed by Peter Barker
parent 1cafe25854
commit 5de6e20ecd
2 changed files with 43 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

76
ArduCopter/GCS_Mavlink.cpp
View File

@ -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 =
@ -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;
@ -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;
@ -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;
}
}
@ -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
@ -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;
@ -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
@ -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
@ -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
@ -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};
}
@ -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) {
@ -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

4
ArduCopter/GCS_Mavlink.h
View File

@ -46,6 +46,10 @@ protected:
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;