diff --git a/ArduCopter/compassmot.cpp b/ArduCopter/compassmot.cpp index dffdb84a26..8be9a6fa29 100644 --- a/ArduCopter/compassmot.cpp +++ b/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= 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 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 } -