APMRover2: Remove, correct some cast

APMrover2/APMrover2.cpp

@@ -338,7 +338,7 @@ void Rover::one_second_loop(void)
     // write perf data every 20s
     if (counter % 10 == 0) {
         if (scheduler.debug() != 0) {
-            hal.console->printf("G_Dt_max=%lu\n", static_cast<uint64_t>(G_Dt_max));
+            hal.console->printf("G_Dt_max=%u\n", G_Dt_max);
         }
         if (should_log(MASK_LOG_PM)) {
             Log_Write_Performance();

APMrover2/GCS_Mavlink.cpp

@@ -100,7 +100,7 @@ void Rover::send_extended_status1(mavlink_channel_t chan)
         control_sensors_present,
         control_sensors_enabled,
         control_sensors_health,
-        static_cast<uint16_t>(scheduler.load_average(20000) * 1000),  // WRONG float to uint16_t
+        static_cast<uint16_t>(scheduler.load_average(20000) * 1000),
         battery.voltage() * 1000,  // mV
         battery_current,        // in 10mA units
         battery_remaining,      // in %
@@ -179,7 +179,7 @@ void Rover::send_vfr_hud(mavlink_channel_t chan)
         gps.ground_speed(),
         ahrs.groundspeed(),
         (ahrs.yaw_sensor / 100) % 360,
-        static_cast<uint16_t>(100 * fabsf(SRV_Channels::get_output_norm(SRV_Channel::k_throttle))),  // WRONG float to uint16
+        static_cast<uint16_t>(100 * fabsf(SRV_Channels::get_output_norm(SRV_Channel::k_throttle))),
         current_loc.alt / 100.0f,
         0);
 }
@@ -1059,18 +1059,18 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
                     break;
                 }
                 Location new_home_loc {};
-                new_home_loc.lat = (int32_t)(packet.param5 * 1.0e7f);
-                new_home_loc.lng = (int32_t)(packet.param6 * 1.0e7f);
-                new_home_loc.alt = (int32_t)(packet.param7 * 100.0f);
+                new_home_loc.lat = static_cast<int32_t>(packet.param5 * 1.0e7f);
+                new_home_loc.lng = static_cast<int32_t>(packet.param6 * 1.0e7f);
+                new_home_loc.alt = static_cast<int32_t>(packet.param7 * 100.0f);
                 rover.ahrs.set_home(new_home_loc);
                 rover.home_is_set = HOME_SET_NOT_LOCKED;
                 rover.Log_Write_Home_And_Origin();
                 GCS_MAVLINK::send_home_all(new_home_loc);
                 result = MAV_RESULT_ACCEPTED;
-                rover.gcs_send_text_fmt(MAV_SEVERITY_INFO, "Set HOME to %.6f %.6f at %um",
-                                          static_cast<double>(new_home_loc.lat * 1.0e-7f),
-                                          static_cast<double>(new_home_loc.lng * 1.0e-7f),
-                                          static_cast<uint32_t>(new_home_loc.alt * 0.01f));  // WRONG FLoat to uint32
+                rover.gcs_send_text_fmt(MAV_SEVERITY_INFO, "Set HOME to %.6f %.6f at %.2fm",
+                        static_cast<double>(new_home_loc.lat * 1.0e-7f),
+                        static_cast<double>(new_home_loc.lng * 1.0e-7f),
+                        static_cast<double>(new_home_loc.alt * 0.01f));
             }
             break;
         }
@@ -1588,7 +1588,7 @@ void Rover::gcs_send_text_fmt(MAV_SEVERITY severity, const char *fmt, ...)
     char str[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN] {};
     va_list arg_list;
     va_start(arg_list, fmt);
-    hal.util->vsnprintf(reinterpret_cast<char *>(str), sizeof(str), fmt, arg_list);
+    hal.util->vsnprintf(&str[0], sizeof(str), fmt, arg_list);
     va_end(arg_list);
     gcs().send_statustext(severity, 0xFF, str);
     notify.send_text(str);

APMrover2/Log.cpp

@@ -264,11 +264,11 @@ void Rover::Log_Write_Nav_Tuning()
     struct log_Nav_Tuning pkt = {
         LOG_PACKET_HEADER_INIT(LOG_NTUN_MSG),
         time_us             : AP_HAL::micros64(),
-        yaw                 : static_cast<uint16_t>(ahrs.yaw_sensor),  // int32 to uint16
+        yaw                 : static_cast<uint16_t>(ahrs.yaw_sensor),
         wp_distance         : wp_distance,
-        target_bearing_cd   : static_cast<uint16_t>(nav_controller->target_bearing_cd()),  // int32 to uint16
-        nav_bearing_cd      : static_cast<uint16_t>(nav_controller->nav_bearing_cd()),  // int32 to uint16
-        throttle            : static_cast<int8_t>((100 * SRV_Channels::get_output_norm(SRV_Channel::k_throttle))),  // wrong float to int8
+        target_bearing_cd   : static_cast<uint16_t>(fabsf(nav_controller->target_bearing_cd())),
+        nav_bearing_cd      : static_cast<uint16_t>(fabsf(nav_controller->nav_bearing_cd())),
+        throttle            : static_cast<int8_t>(100 * SRV_Channels::get_output_norm(SRV_Channel::k_throttle)),
         xtrack_error        : nav_controller->crosstrack_error()
     };
     DataFlash.WriteBlock(&pkt, sizeof(pkt));
@@ -323,10 +323,10 @@ void Rover::Log_Write_Sonar()
         sonar1_distance : sonar.distance_cm(0),
         sonar2_distance : sonar.distance_cm(1),
         detected_count  : obstacle.detected_count,
-        turn_angle      : static_cast<int8_t>(obstacle.turn_angle),  // WRONG float to int8
+        turn_angle      : static_cast<int8_t>(obstacle.turn_angle),
         turn_time       : turn_time,
-        ground_speed    : static_cast<uint16_t>(ground_speed * 100),  // WRONT float to uint16
-        throttle        : static_cast<int8_t>((100 * SRV_Channels::get_output_norm(SRV_Channel::k_throttle)))  //WRONG float to int8
+        ground_speed    : static_cast<uint16_t>(fabsf(ground_speed * 100)),
+        throttle        : static_cast<int8_t>(100 * SRV_Channels::get_output_norm(SRV_Channel::k_throttle))
     };
     DataFlash.WriteBlock(&pkt, sizeof(pkt));
 }

APMrover2/Parameters.cpp

@@ -617,7 +617,7 @@ void Rover::load_parameters(void)
                                       Parameters::k_param_rc_13_old, Parameters::k_param_rc_14_old };
     const uint16_t old_aux_chan_mask = 0x3FFA;
     SRV_Channels::upgrade_parameters(old_rc_keys, old_aux_chan_mask, &rcmap);
-    cliSerial->printf("load_all took %luus\n", static_cast<uint64_t>(micros() - before));
+    cliSerial->printf("load_all took %uus\n", micros() - before);
     // set a more reasonable default NAVL1_PERIOD for rovers
     L1_controller.set_default_period(NAVL1_PERIOD);
 }

APMrover2/commands_logic.cpp

@@ -259,7 +259,7 @@ bool Rover::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
         if (loiter_duration > 0) {
             // Check if this is the first time we have reached the waypoint
             if (!previously_reached_wp) {
-                gcs_send_text_fmt(MAV_SEVERITY_INFO, "Reached waypoint #%i. Loiter for %i seconds",
+                gcs_send_text_fmt(MAV_SEVERITY_INFO, "Reached waypoint #%u. Loiter for %u seconds",
                         static_cast<uint32_t>(cmd.index),
                         static_cast<uint32_t>(loiter_duration));
                 // record the current time i.e. start timer
@@ -274,9 +274,9 @@ bool Rover::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
                 return false;
             }
         } else {
-            gcs_send_text_fmt(MAV_SEVERITY_INFO, "Reached waypoint #%i. Distance %um",
+            gcs_send_text_fmt(MAV_SEVERITY_INFO, "Reached waypoint #%u. Distance %dm",
                     static_cast<uint32_t>(cmd.index),
-                    static_cast<uint32_t>(get_distance(current_loc, next_WP)));  // WRONG float to uint32
+                    static_cast<int32_t>(fabsf(get_distance(current_loc, next_WP))));
         }
         // set loiter_duration to 0 so we know we aren't or have finished loitering
         loiter_duration = 0;
@@ -294,7 +294,7 @@ bool Rover::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
         prev_WP = current_loc;
         // Check if this is the first time we have reached the waypoint even though we have gone past it
         if (!previously_reached_wp) {
-            gcs_send_text_fmt(MAV_SEVERITY_INFO, "Reached waypoint #%i. Loiter for %i seconds",
+            gcs_send_text_fmt(MAV_SEVERITY_INFO, "Reached waypoint #%u. Loiter for %u seconds",
                     static_cast<uint32_t>(cmd.index),
                     static_cast<uint32_t>(loiter_duration));
             // record the current time i.e. start timer
@@ -307,9 +307,9 @@ bool Rover::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
         const float dist_to_wp = get_distance(current_loc, next_WP);
         if (!is_equal(distance_past_wp, dist_to_wp)) {
             distance_past_wp = dist_to_wp;
-            gcs_send_text_fmt(MAV_SEVERITY_INFO, "Passed waypoint #%i. Distance %um",
+            gcs_send_text_fmt(MAV_SEVERITY_INFO, "Passed waypoint #%u. Distance %dm",
                     static_cast<uint32_t>(cmd.index),
-                    static_cast<uint32_t>(distance_past_wp));  // WRONG float to uint32
+                    static_cast<int32_t>(fabsf(distance_past_wp)));
         }
 
         // Check if we need to loiter at this waypoint
@@ -336,8 +336,8 @@ bool Rover::verify_RTL()
 
     // have we gone past the waypoint?
     if (location_passed_point(current_loc, prev_WP, next_WP)) {
-        gcs_send_text_fmt(MAV_SEVERITY_INFO, "Reached destination. Distance away %um",
-                static_cast<uint32_t>(get_distance(current_loc, next_WP)));  // WRONG float to uint32
+        gcs_send_text_fmt(MAV_SEVERITY_INFO, "Reached destination. Distance away %dm",
+                static_cast<int32_t>(fabsf(get_distance(current_loc, next_WP))));
         rtl_complete = true;
         return true;
     }
@@ -439,7 +439,7 @@ void Rover::do_change_speed(const AP_Mission::Mission_Command& cmd)
 
     if (cmd.content.speed.throttle_pct > 0.0f && cmd.content.speed.throttle_pct <= 100.0f) {
         g.throttle_cruise.set(cmd.content.speed.throttle_pct);
-        gcs_send_text_fmt(MAV_SEVERITY_INFO, "Cruise throttle: %.1f", g.throttle_cruise.get());
+        gcs_send_text_fmt(MAV_SEVERITY_INFO, "Cruise throttle: %.1f", static_cast<double>(g.throttle_cruise.get()));
     }
 }
 

APMrover2/test.cpp

@@ -161,14 +161,14 @@ int8_t Rover::test_wp(uint8_t argc, const Menu::arg *argv)
 
 void Rover::test_wp_print(const AP_Mission::Mission_Command& cmd)
 {
-    cliSerial->printf("command #: %d id:%d options:%d p1:%d p2:%ld p3:%ld p4:%ld \n",
+    cliSerial->printf("command #: %d id:%d options:%d p1:%d p2:%d p3:%d p4:%d \n",
             static_cast<int32_t>(cmd.index),
             static_cast<int32_t>(cmd.id),
             static_cast<int32_t>(cmd.content.location.options),
             static_cast<int32_t>(cmd.p1),
-            static_cast<int64_t>(cmd.content.location.alt),
-            static_cast<int64_t>(cmd.content.location.lat),
-            static_cast<int64_t>(cmd.content.location.lng));
+            (cmd.content.location.alt),
+            (cmd.content.location.lat),
+            (cmd.content.location.lng));
 }
 
 int8_t Rover::test_modeswitch(uint8_t argc, const Menu::arg *argv)
@@ -221,11 +221,11 @@ int8_t Rover::test_gps(uint8_t argc, const Menu::arg *argv)
         if (gps.last_message_time_ms() != last_message_time_ms) {
             last_message_time_ms = gps.last_message_time_ms();
             const Location &loc = gps.location();
-            cliSerial->printf("Lat: %ld, Lon %ld, Alt: %ldm, #sats: %d\n",
-                    static_cast<int64_t>(loc.lat),
-                    static_cast<int64_t>(loc.lng),
-                    static_cast<int64_t>(loc.alt/100),
-                    static_cast<int32_t>(gps.num_sats()));
+            cliSerial->printf("Lat: %d, Lon %d, Alt: %dm, #sats: %d\n",
+                    (loc.lat),
+                    (loc.lng),
+                    (loc.alt/100),
+                    (gps.num_sats()));
         } else {
             cliSerial->printf(".");
         }