From 747f15b5141f398fb148beea3876def874d33829 Mon Sep 17 00:00:00 2001 From: Lucas De Marchi Date: Tue, 31 Jan 2017 09:19:09 -0800 Subject: [PATCH] AP_GPS: add casts to consider literals as doubles Since we pass -fsingle-precision-constant to the compiler, add casts to make literals real doubles. --- libraries/AP_GPS/AP_GPS_ERB.cpp | 6 +++--- libraries/AP_GPS/AP_GPS_GSOF.cpp | 6 +++--- libraries/AP_GPS/AP_GPS_NOVA.cpp | 6 +++--- libraries/AP_GPS/AP_GPS_SBF.cpp | 6 +++--- libraries/AP_GPS/AP_GPS_SBP.cpp | 6 +++--- 5 files changed, 15 insertions(+), 15 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS_ERB.cpp b/libraries/AP_GPS/AP_GPS_ERB.cpp index e9d549c9ab..e2534ab763 100644 --- a/libraries/AP_GPS/AP_GPS_ERB.cpp +++ b/libraries/AP_GPS/AP_GPS_ERB.cpp @@ -154,9 +154,9 @@ AP_GPS_ERB::_parse_gps(void) case MSG_POS: Debug("Message POS"); _last_pos_time = _buffer.pos.time; - state.location.lng = (int32_t)(_buffer.pos.longitude * 1e7); - state.location.lat = (int32_t)(_buffer.pos.latitude * 1e7); - state.location.alt = (int32_t)(_buffer.pos.altitude_msl * 1e2); + state.location.lng = (int32_t)(_buffer.pos.longitude * (double)1e7); + state.location.lat = (int32_t)(_buffer.pos.latitude * (double)1e7); + state.location.alt = (int32_t)(_buffer.pos.altitude_msl * 100); state.status = next_fix; _new_position = true; state.horizontal_accuracy = _buffer.pos.horizontal_accuracy * 1.0e-3f; diff --git a/libraries/AP_GPS/AP_GPS_GSOF.cpp b/libraries/AP_GPS/AP_GPS_GSOF.cpp index 227c24cc7f..45705d7e19 100644 --- a/libraries/AP_GPS/AP_GPS_GSOF.cpp +++ b/libraries/AP_GPS/AP_GPS_GSOF.cpp @@ -294,9 +294,9 @@ AP_GPS_GSOF::process_message(void) } else if (output_type == 2) // position { - state.location.lat = (int32_t)(RAD_TO_DEG_DOUBLE * (SwapDouble(gsof_msg.data, a)) * 1e7); - state.location.lng = (int32_t)(RAD_TO_DEG_DOUBLE * (SwapDouble(gsof_msg.data, a + 8)) * 1e7); - state.location.alt = (int32_t)(SwapDouble(gsof_msg.data, a + 16) * 1e2); + state.location.lat = (int32_t)(RAD_TO_DEG_DOUBLE * (SwapDouble(gsof_msg.data, a)) * (double)1e7); + state.location.lng = (int32_t)(RAD_TO_DEG_DOUBLE * (SwapDouble(gsof_msg.data, a + 8)) * (double)1e7); + state.location.alt = (int32_t)(SwapDouble(gsof_msg.data, a + 16) * 100); state.last_gps_time_ms = state.time_week_ms; diff --git a/libraries/AP_GPS/AP_GPS_NOVA.cpp b/libraries/AP_GPS/AP_GPS_NOVA.cpp index 0f79646c2a..98b1fb96dd 100644 --- a/libraries/AP_GPS/AP_GPS_NOVA.cpp +++ b/libraries/AP_GPS/AP_GPS_NOVA.cpp @@ -196,9 +196,9 @@ AP_GPS_NOVA::process_message(void) state.time_week_ms = (uint32_t) nova_msg.header.nova_headeru.tow; state.last_gps_time_ms = state.time_week_ms; - state.location.lat = (int32_t) (bestposu.lat*1e7); - state.location.lng = (int32_t) (bestposu.lng*1e7); - state.location.alt = (int32_t) (bestposu.hgt*1e2); + state.location.lat = (int32_t) (bestposu.lat * (double)1e7); + state.location.lng = (int32_t) (bestposu.lng * (double)1e7); + state.location.alt = (int32_t) (bestposu.hgt * 100); state.num_sats = bestposu.svsused; diff --git a/libraries/AP_GPS/AP_GPS_SBF.cpp b/libraries/AP_GPS/AP_GPS_SBF.cpp index 4ba2e2b4e9..f70f75b0ac 100644 --- a/libraries/AP_GPS/AP_GPS_SBF.cpp +++ b/libraries/AP_GPS/AP_GPS_SBF.cpp @@ -236,9 +236,9 @@ AP_GPS_SBF::process_message(void) // Update position state (don't use −2·10^10) if (temp.Latitude > -200000) { - state.location.lat = (int32_t)(temp.Latitude * RAD_TO_DEG_DOUBLE * 1e7); - state.location.lng = (int32_t)(temp.Longitude * RAD_TO_DEG_DOUBLE * 1e7); - state.location.alt = (int32_t)(((float)temp.Height - temp.Undulation) * 1e2f ); + state.location.lat = (int32_t)(temp.Latitude * RAD_TO_DEG_DOUBLE * (double)1e7); + state.location.lng = (int32_t)(temp.Longitude * RAD_TO_DEG_DOUBLE * (double)1e7); + state.location.alt = (int32_t)(((float)temp.Height - temp.Undulation) * 1e2f); } if (temp.NrSV != 255) { diff --git a/libraries/AP_GPS/AP_GPS_SBP.cpp b/libraries/AP_GPS/AP_GPS_SBP.cpp index 07f8c28564..df574a9a34 100644 --- a/libraries/AP_GPS/AP_GPS_SBP.cpp +++ b/libraries/AP_GPS/AP_GPS_SBP.cpp @@ -274,9 +274,9 @@ AP_GPS_SBP::_attempt_state_update() // Update position state - state.location.lat = (int32_t) (pos_llh->lat*1e7); - state.location.lng = (int32_t) (pos_llh->lon*1e7); - state.location.alt = (int32_t) (pos_llh->height*1e2); + state.location.lat = (int32_t) (pos_llh->lat * (double)1e7); + state.location.lng = (int32_t) (pos_llh->lon * (double)1e7); + state.location.alt = (int32_t) (pos_llh->height * 100); state.num_sats = pos_llh->n_sats; if (pos_llh->flags == 0) {