APMRover2: correct some style

APMrover2/APMrover2.cpp

@@ -102,7 +102,7 @@ void Rover::setup()
     AP_Param::setup_sketch_defaults();
 
     in_auto_reverse = false;
-    
+
     rssi.init();
 
     init_ardupilot();
@@ -436,7 +436,7 @@ void Rover::update_GPS_10Hz(void)
 
 void Rover::update_current_mode(void)
 {
-    switch (control_mode){
+    switch (control_mode) {
     case AUTO:
     case RTL:
         if (!in_auto_reverse) {
@@ -451,7 +451,7 @@ void Rover::update_current_mode(void)
         if (!in_auto_reverse) {
             set_reverse(false);
         }
-        switch (guided_mode){
+        switch (guided_mode) {
         case Guided_Angle:
             nav_set_yaw_speed();
             break;
@@ -462,8 +462,8 @@ void Rover::update_current_mode(void)
                 if (SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) != g.throttle_min.get()) {
                     gcs_send_mission_item_reached_message(0);
                 }
-                SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,g.throttle_min.get());
+                SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, g.throttle_min.get());
-                SRV_Channels::set_output_scaled(SRV_Channel::k_steering,0);
+                SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0);
                 lateral_acceleration = 0;
             } else {
                 calc_lateral_acceleration();
@@ -518,9 +518,8 @@ void Rover::update_current_mode(void)
           we set the exact value in set_servos(), but it helps for
           logging
          */
-        in_auto_reverse = false;
+        SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, channel_throttle->get_control_in());
-        SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,channel_throttle->get_control_in());
+        SRV_Channels::set_output_scaled(SRV_Channel::k_steering, channel_steer->get_control_in());
-        SRV_Channels::set_output_scaled(SRV_Channel::k_steering,channel_steer->get_control_in());
 
         // mark us as in_reverse when using a negative throttle to
         // stop AHRS getting off
@@ -529,8 +528,8 @@ void Rover::update_current_mode(void)
 
     case HOLD:
         // hold position - stop motors and center steering
-        SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,0);
+        SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
-        SRV_Channels::set_output_scaled(SRV_Channel::k_steering,0);
+        SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0);
         if (!in_auto_reverse) {
             set_reverse(false);
         }
@@ -560,13 +559,13 @@ void Rover::update_navigation()
         calc_lateral_acceleration();
         calc_nav_steer();
         if (verify_RTL()) {
-            SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,g.throttle_min.get());
+            SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, g.throttle_min.get());
             set_mode(HOLD);
         }
         break;
 
     case GUIDED:
-        switch (guided_mode){
+        switch (guided_mode) {
         case Guided_Angle:
             nav_set_yaw_speed();
             break;
@@ -577,8 +576,8 @@ void Rover::update_navigation()
             calc_nav_steer();
             if (rtl_complete || verify_RTL()) {
                 // we have reached destination so stop where we are
-                SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,g.throttle_min.get());
+                SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, g.throttle_min.get());
-                SRV_Channels::set_output_scaled(SRV_Channel::k_steering,0);
+                SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0);
                 lateral_acceleration = 0;
             }
             break;

APMrover2/GCS_Mavlink.cpp

@@ -180,6 +180,7 @@ void Rover::send_vfr_hud(mavlink_channel_t chan)
         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
+        current_loc.alt / 100.0f,
         0);
 }
 
@@ -195,7 +196,7 @@ void Rover::send_hwstatus(mavlink_channel_t chan)
 {
     mavlink_msg_hwstatus_send(
         chan,
-        hal.analogin->board_voltage()*1000,
+        hal.analogin->board_voltage() * 1000,
         0);
 }
 
@@ -978,7 +979,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
             break;
 
         case MAV_CMD_DO_REPEAT_SERVO:
-            if (rover.ServoRelayEvents.do_repeat_servo(packet.param1, packet.param2, packet.param3, packet.param4*1000)) {
+            if (rover.ServoRelayEvents.do_repeat_servo(packet.param1, packet.param2, packet.param3, packet.param4 * 1000)) {
                 result = MAV_RESULT_ACCEPTED;
             }
             break;
@@ -990,7 +991,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
             break;
 
         case MAV_CMD_DO_REPEAT_RELAY:
-            if (rover.ServoRelayEvents.do_repeat_relay(packet.param1, packet.param2, packet.param3*1000)) {
+            if (rover.ServoRelayEvents.do_repeat_relay(packet.param1, packet.param2, packet.param3 * 1000)) {
                 result = MAV_RESULT_ACCEPTED;
             }
             break;

APMrover2/GCS_Mavlink.h

@@ -7,7 +7,6 @@
 
 class GCS_MAVLINK_Rover : public GCS_MAVLINK
 {
-
 public:
 
     void data_stream_send(void) override;
@@ -23,5 +22,4 @@ private:
     bool handle_guided_request(AP_Mission::Mission_Command &cmd) override;
     void handle_change_alt_request(AP_Mission::Mission_Command &cmd) override;
     bool try_send_message(enum ap_message id) override;
-
 };

APMrover2/Log.cpp

@@ -279,7 +279,7 @@ void Rover::Log_Write_Nav_Tuning()
 // Write an attitude packet
 void Rover::Log_Write_Attitude()
 {
-    Vector3f targets(0, 0, 0);       // Rover does not have attitude targets, use place-holder for commonality with Dataflash Log_Write_Attitude message
+    Vector3f targets(0.0f, 0.0f, 0.0f);  // Rover does not have attitude targets, use place-holder for commonality with Dataflash Log_Write_Attitude message
 
     DataFlash.Log_Write_Attitude(ahrs, targets);
 

APMrover2/Parameters.cpp

@@ -599,15 +599,15 @@ void Rover::load_parameters(void)
         cliSerial->printf("done.\n");
     }
 
-    unsigned long before = micros();
+    uint32_t before = micros();
     // Load all auto-loaded EEPROM variables
     AP_Param::load_all();
 
     AP_Param::set_frame_type_flags(AP_PARAM_FRAME_ROVER);
-    
+
     SRV_Channels::set_default_function(CH_1, SRV_Channel::k_steering);
     SRV_Channels::set_default_function(CH_3, SRV_Channel::k_throttle);
-    
+
     const uint8_t old_rc_keys[14] = { Parameters::k_param_rc_1_old,  Parameters::k_param_rc_2_old,
                                       Parameters::k_param_rc_3_old,  Parameters::k_param_rc_4_old,
                                       Parameters::k_param_rc_5_old,  Parameters::k_param_rc_6_old,

APMrover2/Parameters.h

@@ -288,7 +288,7 @@ public:
     Parameters() :
         // PID controller    initial P        initial I        initial D        initial imax
         //-----------------------------------------------------------------------------------
-        pidSpeedThrottle    (0.7,             0.2,             0.2,             4000)
+        pidSpeedThrottle    (0.7f,            0.2f,            0.2f,            4000)
         {}
 };
 
@@ -310,7 +310,7 @@ public:
 
     // RC input channels
     RC_Channels rc_channels;
-    
+
     // control over servo output ranges
     SRV_Channels servo_channels;
 
@@ -318,7 +318,6 @@ public:
     // advanced failsafe library
     AP_AdvancedFailsafe_Rover afs;
 #endif
-
 };
 
 extern const AP_Param::Info var_info[];

APMrover2/Rover.cpp

@@ -49,6 +49,6 @@ Rover::Rover(void) :
     frsky_telemetry(ahrs, battery, sonar),
 #endif
     home(ahrs.get_home()),
-    G_Dt(0.02)
+    G_Dt(0.02f)
 {
 }

APMrover2/Rover.h

@@ -186,7 +186,7 @@ private:
     AP_SerialManager serial_manager;
     const uint8_t num_gcs;
     GCS_MAVLINK_Rover gcs_chan[MAVLINK_COMM_NUM_BUFFERS];
-    GCS _gcs; // avoid using this; use gcs()
+    GCS _gcs;  // avoid using this; use gcs()
     GCS &gcs() { return _gcs; }
 
     // relay support

APMrover2/Steering.cpp

@@ -172,7 +172,7 @@ void Rover::calc_throttle(float target_speed) {
     }
 
     if (use_pivot_steering()) {
-        SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,0);
+        SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
     }
 }
 

APMrover2/commands_logic.cpp

@@ -305,7 +305,7 @@ bool Rover::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
 
         // check if distance to the WP has changed and output new message if it has
         float dist_to_wp = get_distance(current_loc, next_WP);
-        if ((uint32_t)distance_past_wp != (uint32_t)dist_to_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",
                     static_cast<uint32_t>(cmd.index),
@@ -370,14 +370,14 @@ void Rover::nav_set_yaw_speed()
     // if we haven't received a MAV_CMD_NAV_SET_YAW_SPEED message within the last 3 seconds bring the rover to a halt
     if ((millis() - guided_yaw_speed.msg_time_ms) > 3000) {
         gcs_send_text(MAV_SEVERITY_WARNING, "NAV_SET_YAW_SPEED not recvd last 3secs, stopping");
-        SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,g.throttle_min.get());
+        SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, g.throttle_min.get());
-        SRV_Channels::set_output_scaled(SRV_Channel::k_steering,0);
+        SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0);
         lateral_acceleration = 0;
         return;
     }
 
     int32_t steering = steerController.get_steering_out_angle_error(guided_yaw_speed.turn_angle);
-    SRV_Channels::set_output_scaled(SRV_Channel::k_steering,steering);
+    SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steering);
 
     // speed param in the message gives speed as a proportion of cruise speed.
     // 0.5 would set speed to the cruise speed
@@ -401,7 +401,7 @@ void Rover::do_wait_delay(const AP_Mission::Mission_Command& cmd)
 
 void Rover::do_within_distance(const AP_Mission::Mission_Command& cmd)
 {
-    condition_value  = cmd.content.distance.meters;
+    condition_value = cmd.content.distance.meters;
 }
 
 /********************************************************************************/
@@ -432,12 +432,12 @@ bool Rover::verify_within_distance()
 
 void Rover::do_change_speed(const AP_Mission::Mission_Command& cmd)
 {
-    if (cmd.content.speed.target_ms > 0) {
+    if (cmd.content.speed.target_ms > 0.0f) {
         g.speed_cruise.set(cmd.content.speed.target_ms);
         gcs_send_text_fmt(MAV_SEVERITY_INFO, "Cruise speed: %.1f m/s", static_cast<double>(g.speed_cruise.get()));
     }
 
-    if (cmd.content.speed.throttle_pct > 0 && cmd.content.speed.throttle_pct <= 100) {
+    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());
     }

APMrover2/navigation.cpp

@@ -11,7 +11,7 @@ void Rover::navigate()
         return;
     }
 
-    if ((next_WP.lat == 0 && next_WP.lng == 0) || (home_is_set == HOME_UNSET)){
+    if ((next_WP.lat == 0 && next_WP.lng == 0) || (home_is_set == HOME_UNSET)) {
         return;
     }
 

APMrover2/radio.cpp

@@ -18,9 +18,9 @@ void Rover::set_control_channels(void)
 
     // For a rover safety is TRIM throttle
     if (!arming.is_armed() && arming.arming_required() == AP_Arming::YES_MIN_PWM) {
-        hal.rcout->set_safety_pwm(1UL<<(rcmap.throttle()-1), channel_throttle->get_radio_trim());
+        hal.rcout->set_safety_pwm(1UL << (rcmap.throttle() - 1), channel_throttle->get_radio_trim());
         if (g.skid_steer_out) {
-            hal.rcout->set_safety_pwm(1UL<<(rcmap.roll()-1),  channel_steer->get_radio_trim());
+            hal.rcout->set_safety_pwm(1UL << (rcmap.roll() - 1),  channel_steer->get_radio_trim());
         }
     }
     // setup correct scaling for ESCs like the UAVCAN PX4ESC which
@@ -50,9 +50,9 @@ void Rover::init_rc_out()
     // For Rover's no throttle means TRIM as rovers can go backwards i.e. MIN throttle is
     // full speed backward.
     if (arming.arming_required() == AP_Arming::YES_MIN_PWM) {
-        hal.rcout->set_safety_pwm(1UL<<(rcmap.throttle()-1),  channel_throttle->get_radio_trim());
+        hal.rcout->set_safety_pwm(1UL << (rcmap.throttle() - 1),  channel_throttle->get_radio_trim());
         if (g.skid_steer_out) {
-            hal.rcout->set_safety_pwm(1UL<<(rcmap.roll()-1),  channel_steer->get_radio_trim());
+            hal.rcout->set_safety_pwm(1UL << (rcmap.roll() - 1),  channel_steer->get_radio_trim());
         }
     }
 }
@@ -131,7 +131,7 @@ void Rover::read_radio()
 
     control_failsafe(channel_throttle->get_radio_in());
 
-    SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,channel_throttle->get_control_in());
+    SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, channel_throttle->get_control_in());
 
     // Check if the throttle value is above 50% and we need to nudge
     // Make sure its above 50% in the direction we are travelling
@@ -139,7 +139,7 @@ void Rover::read_radio()
         (((SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) < 0) && in_reverse) ||
          ((SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) > 0) && !in_reverse))) {
             throttle_nudge = (g.throttle_max - g.throttle_cruise) *
-                ((fabsf(channel_throttle->norm_input())-0.5f) / 0.5f);
+                ((fabsf(channel_throttle->norm_input()) - 0.5f) / 0.5f);
     } else {
         throttle_nudge = 0;
     }
@@ -157,18 +157,19 @@ void Rover::read_radio()
         float motor1 = channel_steer->norm_input();
         float motor2 = channel_throttle->norm_input();
         float steering_scaled = motor1 - motor2;
-        float throttle_scaled = 0.5f*(motor1 + motor2);
+        float throttle_scaled = 0.5f * (motor1 + motor2);
+
         int16_t steer = channel_steer->get_radio_trim();
         int16_t thr   = channel_throttle->get_radio_trim();
         if (steering_scaled > 0.0f) {
-            steer += steering_scaled*(channel_steer->get_radio_max()-channel_steer->get_radio_trim());
+            steer += steering_scaled * (channel_steer->get_radio_max()-channel_steer->get_radio_trim());
         } else {
-            steer += steering_scaled*(channel_steer->get_radio_trim()-channel_steer->get_radio_min());
+            steer += steering_scaled * (channel_steer->get_radio_trim()-channel_steer->get_radio_min());
         }
         if (throttle_scaled > 0.0f) {
-            thr += throttle_scaled*(channel_throttle->get_radio_max()-channel_throttle->get_radio_trim());
+            thr += throttle_scaled * (channel_throttle->get_radio_max()-channel_throttle->get_radio_trim());
         } else {
-            thr += throttle_scaled*(channel_throttle->get_radio_trim()-channel_throttle->get_radio_min());
+            thr += throttle_scaled * (channel_throttle->get_radio_trim()-channel_throttle->get_radio_min());
         }
         channel_steer->set_pwm(steer);
         channel_throttle->set_pwm(thr);

APMrover2/system.cpp

@@ -268,7 +268,7 @@ void Rover::set_reverse(bool reverse)
 
 void Rover::set_mode(enum mode mode)
 {
-    if (control_mode == mode){
+    if (control_mode == mode) {
         // don't switch modes if we are already in the correct mode.
         return;
     }

APMrover2/test.cpp

@@ -58,7 +58,7 @@ int8_t Rover::test_passthru(uint8_t argc, const Menu::arg *argv)
             }
             cliSerial->printf("\n");
         }
-        if (cliSerial->available() > 0){
+        if (cliSerial->available() > 0) {
             return (0);
         }
     }
@@ -96,7 +96,7 @@ int8_t Rover::test_failsafe(uint8_t argc, const Menu::arg *argv)
             fail_test++;
         }
 
-        if (oldSwitchPosition != readSwitch()){
+        if (oldSwitchPosition != readSwitch()) {
             cliSerial->printf("CONTROL MODE CHANGED: ");
             print_mode(cliSerial, readSwitch());
             cliSerial->printf("\n");
@@ -183,7 +183,7 @@ int8_t Rover::test_modeswitch(uint8_t argc, const Menu::arg *argv)
     while (1) {
         delay(20);
         uint8_t switchPosition = readSwitch();
-        if (oldSwitchPosition != switchPosition){
+        if (oldSwitchPosition != switchPosition) {
             cliSerial->printf("Position %d\n",  switchPosition);
             oldSwitchPosition = switchPosition;
         }