diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 22166945a8..e5e89b3a42 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -3135,26 +3135,13 @@ MavlinkReceiver::get_offb_cruising_speed() _vehicle_status_sub.copy(&vehicle_status); if (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && _offb_cruising_speed_mc > 0.0f) { - return _offb_cruising_speed_mc; + return _offb_cruising_speed_mc; + } else if (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && _offb_cruising_speed_fw > 0.0f) { - return _offb_cruising_speed_fw; - } else { - return -1.0f; - } - if (_offb_cruising_speed_mc > 0.0f) { - return _offb_cruising_speed_mc; - - } else { - return -1.0f; - } + return _offb_cruising_speed_fw; } else { - if (_offb_cruising_speed_fw > 0.0f) { - return _offb_cruising_speed_fw; - - } else { - return -1.0f; - } + return -1.0f; } }