diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 3c10b8e0ce..4024150388 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -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 diff --git a/ArduCopter/GCS_Mavlink.h b/ArduCopter/GCS_Mavlink.h index 30d325c01d..e38c9b6e7a 100644 --- a/ArduCopter/GCS_Mavlink.h +++ b/ArduCopter/GCS_Mavlink.h @@ -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;