Copter: compassmot format fixes

ArduCopter/compassmot.cpp

@@ -29,7 +29,7 @@ MAV_RESULT Copter::mavlink_compassmot(mavlink_channel_t chan)
     if (ap.compass_mot) {
         // ignore restart messages
         return MAV_RESULT_TEMPORARILY_REJECTED;
-    }else{
+    } else {
         ap.compass_mot = true;
     }
 
@@ -88,7 +88,7 @@ MAV_RESULT Copter::mavlink_compassmot(mavlink_channel_t chan)
     // default compensation type to use current if possible
     if (battery.has_current()) {
         comp_type = AP_COMPASS_MOT_COMP_CURRENT;
-    }else{
+    } else {
         comp_type = AP_COMPASS_MOT_COMP_THROTTLE;
     }
 
@@ -104,7 +104,7 @@ MAV_RESULT Copter::mavlink_compassmot(mavlink_channel_t chan)
     // inform what type of compensation we are attempting
     if (comp_type == AP_COMPASS_MOT_COMP_CURRENT) {
         gcs_chan.send_text(MAV_SEVERITY_INFO, "Current");
-    } else{
+    } else {
         gcs_chan.send_text(MAV_SEVERITY_INFO, "Throttle");
     }
 
@@ -153,18 +153,18 @@ MAV_RESULT Copter::mavlink_compassmot(mavlink_channel_t chan)
 
         // read radio input
         read_radio();
-        
+
         // pass through throttle to motors
         SRV_Channels::cork();
         motors->set_throttle_passthrough_for_esc_calibration(channel_throttle->get_control_in() / 1000.0f);
         SRV_Channels::push();
-        
+
         // read some compass values
         compass.read();
-        
+
         // read current
         battery.read();
-        
+
         // calculate scaling for throttle
         throttle_pct = (float)channel_throttle->get_control_in() / 1000.0f;
         throttle_pct = constrain_float(throttle_pct,0.0f,1.0f);
@@ -174,8 +174,6 @@ MAV_RESULT Copter::mavlink_compassmot(mavlink_channel_t chan)
             for (uint8_t i=0; i<compass.get_count(); i++) {
                 compass_base[i] = compass_base[i] * 0.99f + compass.get_field(i) * 0.01f;
             }
-
-            // causing printing to happen as soon as throttle is lifted
         } else {
 
             // calculate diff from compass base and scale with throttle
@@ -192,14 +190,13 @@ MAV_RESULT Copter::mavlink_compassmot(mavlink_channel_t chan)
                     // adjust the motor compensation to negate the impact
                     motor_compensation[i] = motor_compensation[i] * 0.99f - motor_impact_scaled[i] * 0.01f;
                 }
-
                 updated = true;
             } else {
                 // for each compass
                 for (uint8_t i=0; i<compass.get_count(); i++) {
                     // current based compensation if more than 3amps being drawn
                     motor_impact_scaled[i] = motor_impact[i] / battery.current_amps();
-                
+
                     // adjust the motor compensation to negate the impact if drawing over 3amps
                     if (battery.current_amps() >= 3.0f) {
                         motor_compensation[i] = motor_compensation[i] * 0.99f - motor_impact_scaled[i] * 0.01f;
@@ -214,7 +211,7 @@ MAV_RESULT Copter::mavlink_compassmot(mavlink_channel_t chan)
                     // interference is impact@fullthrottle / mag field * 100
                     interference_pct[i] = motor_compensation[i].length() / (float)arming.compass_magfield_expected() * 100.0f;
                 }
-            }else{
+            } else {
                 for (uint8_t i=0; i<compass.get_count(); i++) {
                     // interference is impact/amp * (max current seen / max throttle seen) / mag field * 100
                     interference_pct[i] = motor_compensation[i].length() * (current_amps_max/throttle_pct_max) / (float)arming.compass_magfield_expected() * 100.0f;
@@ -224,8 +221,8 @@ MAV_RESULT Copter::mavlink_compassmot(mavlink_channel_t chan)
             // record maximum throttle and current
             throttle_pct_max = MAX(throttle_pct_max, throttle_pct);
             current_amps_max = MAX(current_amps_max, battery.current_amps());
-
         }
+
         if (AP_HAL::millis() - last_send_time > 500) {
             last_send_time = AP_HAL::millis();
             mavlink_msg_compassmot_status_send(chan, 
@@ -275,4 +272,3 @@ MAV_RESULT Copter::mavlink_compassmot(mavlink_channel_t chan)
     return MAV_RESULT_ACCEPTED;
 #endif  // FRAME_CONFIG != HELI_FRAME
 }
-