diff --git a/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp b/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp index 601471cfd5..398953b526 100644 --- a/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp +++ b/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp @@ -222,7 +222,8 @@ void AP_Proximity_LightWareSF40C::set_forward_direction() // set forward direction char request_str[15]; int16_t yaw_corr = frontend.get_yaw_correction(state.instance); - snprintf(request_str, sizeof(request_str), "#MBF,%d\r\n", (int)yaw_corr); + yaw_corr = constrain_int16(yaw_corr, -999, 999); + snprintf(request_str, sizeof(request_str), "#MBF,%d\r\n", yaw_corr); uart->write(request_str); // request update on motor direction @@ -286,7 +287,9 @@ bool AP_Proximity_LightWareSF40C::send_request_for_distance() // prepare request char request_str[16]; - snprintf(request_str, sizeof(request_str), "?TS,%d,%d\r\n", (int)(_sector_width_deg[_last_sector]), (int)(_sector_middle_deg[_last_sector])); + snprintf(request_str, sizeof(request_str), "?TS,%u,%u\r\n", + MIN(_sector_width_deg[_last_sector], 999), + MIN(_sector_middle_deg[_last_sector], 999)); uart->write(request_str);