AP_Motors: Group writes to motors

Surround calls to rcout->write() with rcout->cork() and rcout->push().
If the RCOutput implementation allows the writes are grouped and only
sent together to the underlying hardware.

libraries/AP_Motors/AP_MotorsCoax.cpp

@@ -111,10 +111,12 @@ void AP_MotorsCoax::enable()
 void AP_MotorsCoax::output_min()
 {
     // send minimum value to each motor
+    hal.rcout->cork();
     hal.rcout->write(AP_MOTORS_MOT_1, _servo1.radio_trim);
     hal.rcout->write(AP_MOTORS_MOT_2, _servo2.radio_trim);
     hal.rcout->write(AP_MOTORS_MOT_3, _throttle_radio_min);
     hal.rcout->write(AP_MOTORS_MOT_4, _throttle_radio_min);
+    hal.rcout->push();
 }
 
 void AP_MotorsCoax::output_armed_not_stabilizing()
@@ -154,10 +156,12 @@ void AP_MotorsCoax::output_armed_not_stabilizing()
         motor_out = apply_thrust_curve_and_volt_scaling(motor_out, out_min, _throttle_radio_max);
     }
 
+    hal.rcout->cork();
     hal.rcout->write(AP_MOTORS_MOT_1, _servo1.radio_out);
     hal.rcout->write(AP_MOTORS_MOT_2, _servo2.radio_out);
     hal.rcout->write(AP_MOTORS_MOT_3, motor_out);
     hal.rcout->write(AP_MOTORS_MOT_4, motor_out);
+    hal.rcout->push();
 }
 
 // sends commands to the motors
@@ -212,10 +216,12 @@ void AP_MotorsCoax::output_armed_stabilizing()
     motor_out[AP_MOTORS_MOT_4] = max(motor_out[AP_MOTORS_MOT_4],    out_min);
 
     // send output to each motor
+    hal.rcout->cork();
     hal.rcout->write(AP_MOTORS_MOT_1, _servo1.radio_out);
     hal.rcout->write(AP_MOTORS_MOT_2, _servo2.radio_out);
     hal.rcout->write(AP_MOTORS_MOT_3, motor_out[AP_MOTORS_MOT_3]);
     hal.rcout->write(AP_MOTORS_MOT_4, motor_out[AP_MOTORS_MOT_4]);
+    hal.rcout->push();
 }
 
 // output_disarmed - sends commands to the motors

libraries/AP_Motors/AP_MotorsHeli_Single.cpp

@@ -445,13 +445,17 @@ void AP_MotorsHeli_Single::move_actuators(int16_t roll_out, int16_t pitch_out, i
     _swash_servo_2.calc_pwm();
     _swash_servo_3.calc_pwm();
 
+    hal.rcout->cork();
+
     // actually move the servos
     hal.rcout->write(AP_MOTORS_MOT_1, _swash_servo_1.radio_out);
     hal.rcout->write(AP_MOTORS_MOT_2, _swash_servo_2.radio_out);
     hal.rcout->write(AP_MOTORS_MOT_3, _swash_servo_3.radio_out);
- 
+
     // update the yaw rate using the tail rotor/servo
     move_yaw(yaw_out + yaw_offset);
+
+    hal.rcout->push();
 }
 
 // move_yaw

libraries/AP_Motors/AP_MotorsMatrix.cpp

@@ -95,11 +95,13 @@ void AP_MotorsMatrix::output_min()
     limit.throttle_upper = false;
 
     // fill the motor_out[] array for HIL use and send minimum value to each motor
+    hal.rcout->cork();
     for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
         if( motor_enabled[i] ) {
             hal.rcout->write(i, _throttle_radio_min);
         }
     }
+    hal.rcout->push();
 }
 
 // get_motor_mask - returns a bitmask of which outputs are being used for motors (1 means being used)
@@ -159,11 +161,13 @@ void AP_MotorsMatrix::output_armed_not_stabilizing()
     }
 
     // send output to each motor
+    hal.rcout->cork();
     for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
         if( motor_enabled[i] ) {
             hal.rcout->write(i, motor_out[i]);
         }
     }
+    hal.rcout->push();
 }
 
 // output_armed - sends commands to the motors
@@ -355,11 +359,13 @@ void AP_MotorsMatrix::output_armed_stabilizing()
     }
 
     // send output to each motor
+    hal.rcout->cork();
     for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
         if( motor_enabled[i] ) {
             hal.rcout->write(i, motor_out[i]);
         }
     }
+    hal.rcout->push();
 }
 
 // output_disarmed - sends commands to the motors
@@ -380,12 +386,14 @@ void AP_MotorsMatrix::output_test(uint8_t motor_seq, int16_t pwm)
     }
 
     // loop through all the possible orders spinning any motors that match that description
+    hal.rcout->cork();
     for (uint8_t i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
         if (motor_enabled[i] && _test_order[i] == motor_seq) {
             // turn on this motor
             hal.rcout->write(i, pwm);
         }
     }
+    hal.rcout->push();
 }
 
 // add_motor

libraries/AP_Motors/AP_MotorsMulticopter.cpp

@@ -378,10 +378,12 @@ void AP_MotorsMulticopter::throttle_pass_through(int16_t pwm)
 {
     if (armed()) {
         // send the pilot's input directly to each enabled motor
+        hal.rcout->cork();
         for (uint16_t i=0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
             if (motor_enabled[i]) {
                 hal.rcout->write(i, pwm);
             }
         }
+        hal.rcout->push();
     }
 }

libraries/AP_Motors/AP_MotorsSingle.cpp

@@ -116,11 +116,13 @@ void AP_MotorsSingle::enable()
 void AP_MotorsSingle::output_min()
 {
     // send minimum value to each motor
+    hal.rcout->cork();
     hal.rcout->write(AP_MOTORS_MOT_1, _servo1.radio_trim);
     hal.rcout->write(AP_MOTORS_MOT_2, _servo2.radio_trim);
     hal.rcout->write(AP_MOTORS_MOT_3, _servo3.radio_trim);
     hal.rcout->write(AP_MOTORS_MOT_4, _servo4.radio_trim);
     hal.rcout->write(AP_MOTORS_MOT_7, _throttle_radio_min);
+    hal.rcout->push();
 }
 
 // get_motor_mask - returns a bitmask of which outputs are being used for motors or servos (1 means being used)
@@ -172,11 +174,13 @@ void AP_MotorsSingle::output_armed_not_stabilizing()
         throttle_radio_output = apply_thrust_curve_and_volt_scaling(throttle_radio_output, out_min, _throttle_radio_max);
     }
 
+    hal.rcout->cork();
     hal.rcout->write(AP_MOTORS_MOT_1, _servo1.radio_out);
     hal.rcout->write(AP_MOTORS_MOT_2, _servo2.radio_out);
     hal.rcout->write(AP_MOTORS_MOT_3, _servo3.radio_out);
     hal.rcout->write(AP_MOTORS_MOT_4, _servo4.radio_out);
     hal.rcout->write(AP_MOTORS_MOT_7, throttle_radio_output);
+    hal.rcout->push();
 }
 
 // sends commands to the motors
@@ -229,11 +233,13 @@ void AP_MotorsSingle::output_armed_stabilizing()
     _servo4.calc_pwm();
 
     // send output to each motor
+    hal.rcout->cork();
     hal.rcout->write(AP_MOTORS_MOT_1, _servo1.radio_out);
     hal.rcout->write(AP_MOTORS_MOT_2, _servo2.radio_out);
     hal.rcout->write(AP_MOTORS_MOT_3, _servo3.radio_out);
     hal.rcout->write(AP_MOTORS_MOT_4, _servo4.radio_out);
     hal.rcout->write(AP_MOTORS_MOT_7, throttle_radio_output);
+    hal.rcout->push();
 }
 
 // output_disarmed - sends commands to the motors

libraries/AP_Motors/AP_MotorsTri.cpp

@@ -117,10 +117,12 @@ void AP_MotorsTri::output_min()
     limit.throttle_lower = true;
 
     // send minimum value to each motor
+    hal.rcout->cork();
     hal.rcout->write(AP_MOTORS_MOT_1, _throttle_radio_min);
     hal.rcout->write(AP_MOTORS_MOT_2, _throttle_radio_min);
     hal.rcout->write(AP_MOTORS_MOT_4, _throttle_radio_min);
     hal.rcout->write(AP_MOTORS_CH_TRI_YAW, _yaw_servo_trim);
+    hal.rcout->push();
 }
 
 // get_motor_mask - returns a bitmask of which outputs are being used for motors or servos (1 means being used)
@@ -171,6 +173,8 @@ void AP_MotorsTri::output_armed_not_stabilizing()
         motor_out[AP_MOTORS_MOT_4] = apply_thrust_curve_and_volt_scaling(motor_out[AP_MOTORS_MOT_4], out_min, out_max);
     }
 
+    hal.rcout->cork();
+
     // send output to each motor
     hal.rcout->write(AP_MOTORS_MOT_1, motor_out[AP_MOTORS_MOT_1]);
     hal.rcout->write(AP_MOTORS_MOT_2, motor_out[AP_MOTORS_MOT_2]);
@@ -178,6 +182,8 @@ void AP_MotorsTri::output_armed_not_stabilizing()
 
     // send centering signal to yaw servo
     hal.rcout->write(AP_MOTORS_CH_TRI_YAW, _yaw_servo_trim);
+
+    hal.rcout->push();
 }
 
 // sends commands to the motors
@@ -284,6 +290,8 @@ void AP_MotorsTri::output_armed_stabilizing()
         motor_out[AP_MOTORS_MOT_4] = max(motor_out[AP_MOTORS_MOT_4],    out_min);
     }
 
+    hal.rcout->cork();
+
     // send output to each motor
     hal.rcout->write(AP_MOTORS_MOT_1, motor_out[AP_MOTORS_MOT_1]);
     hal.rcout->write(AP_MOTORS_MOT_2, motor_out[AP_MOTORS_MOT_2]);
@@ -291,6 +299,8 @@ void AP_MotorsTri::output_armed_stabilizing()
 
     // send out to yaw command to tail servo
     hal.rcout->write(AP_MOTORS_CH_TRI_YAW, yaw_radio_output);
+
+    hal.rcout->push();
 }
 
 // output_disarmed - sends commands to the motors