diff --git a/libraries/AP_Motors/AP_MotorsCoax.cpp b/libraries/AP_Motors/AP_MotorsCoax.cpp index bd735eda6e..f2595a9279 100644 --- a/libraries/AP_Motors/AP_MotorsCoax.cpp +++ b/libraries/AP_Motors/AP_MotorsCoax.cpp @@ -112,10 +112,10 @@ 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); + rc_write(AP_MOTORS_MOT_1, _servo1.radio_trim); + rc_write(AP_MOTORS_MOT_2, _servo2.radio_trim); + rc_write(AP_MOTORS_MOT_3, _throttle_radio_min); + rc_write(AP_MOTORS_MOT_4, _throttle_radio_min); hal.rcout->push(); } @@ -157,10 +157,10 @@ void AP_MotorsCoax::output_armed_not_stabilizing() } 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); + rc_write(AP_MOTORS_MOT_1, _servo1.radio_out); + rc_write(AP_MOTORS_MOT_2, _servo2.radio_out); + rc_write(AP_MOTORS_MOT_3, motor_out); + rc_write(AP_MOTORS_MOT_4, motor_out); hal.rcout->push(); } @@ -217,10 +217,10 @@ void AP_MotorsCoax::output_armed_stabilizing() // 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]); + rc_write(AP_MOTORS_MOT_1, _servo1.radio_out); + rc_write(AP_MOTORS_MOT_2, _servo2.radio_out); + rc_write(AP_MOTORS_MOT_3, motor_out[AP_MOTORS_MOT_3]); + rc_write(AP_MOTORS_MOT_4, motor_out[AP_MOTORS_MOT_4]); hal.rcout->push(); } @@ -245,19 +245,19 @@ void AP_MotorsCoax::output_test(uint8_t motor_seq, int16_t pwm) switch (motor_seq) { case 1: // flap servo 1 - hal.rcout->write(AP_MOTORS_MOT_1, pwm); + rc_write(AP_MOTORS_MOT_1, pwm); break; case 2: // flap servo 2 - hal.rcout->write(AP_MOTORS_MOT_2, pwm); + rc_write(AP_MOTORS_MOT_2, pwm); break; case 3: // motor 1 - hal.rcout->write(AP_MOTORS_MOT_3, pwm); + rc_write(AP_MOTORS_MOT_3, pwm); break; case 4: // motor 2 - hal.rcout->write(AP_MOTORS_MOT_4, pwm); + rc_write(AP_MOTORS_MOT_4, pwm); break; default: // do nothing diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp index bc885d4339..ab0bfed3c1 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp @@ -175,15 +175,15 @@ void AP_MotorsHeli_Single::output_test(uint8_t motor_seq, int16_t pwm) switch (motor_seq) { case 1: // swash servo 1 - hal.rcout->write(AP_MOTORS_MOT_1, pwm); + rc_write(AP_MOTORS_MOT_1, pwm); break; case 2: // swash servo 2 - hal.rcout->write(AP_MOTORS_MOT_2, pwm); + rc_write(AP_MOTORS_MOT_2, pwm); break; case 3: // swash servo 3 - hal.rcout->write(AP_MOTORS_MOT_3, pwm); + rc_write(AP_MOTORS_MOT_3, pwm); break; case 4: // external gyro & tail servo @@ -194,11 +194,11 @@ void AP_MotorsHeli_Single::output_test(uint8_t motor_seq, int16_t pwm) write_aux(_ext_gyro_gain_std); } } - hal.rcout->write(AP_MOTORS_MOT_4, pwm); + rc_write(AP_MOTORS_MOT_4, pwm); break; case 5: // main rotor - hal.rcout->write(AP_MOTORS_HELI_SINGLE_RSC, pwm); + rc_write(AP_MOTORS_HELI_SINGLE_RSC, pwm); break; default: // do nothing @@ -438,9 +438,9 @@ void AP_MotorsHeli_Single::move_actuators(int16_t roll_out, int16_t pitch_out, i 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); + rc_write(AP_MOTORS_MOT_1, _swash_servo_1.radio_out); + rc_write(AP_MOTORS_MOT_2, _swash_servo_2.radio_out); + rc_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); @@ -459,7 +459,7 @@ void AP_MotorsHeli_Single::move_yaw(int16_t yaw_out) _yaw_servo.calc_pwm(); - hal.rcout->write(AP_MOTORS_MOT_4, _yaw_servo.radio_out); + rc_write(AP_MOTORS_MOT_4, _yaw_servo.radio_out); if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) { // output gain to exernal gyro @@ -480,7 +480,7 @@ void AP_MotorsHeli_Single::write_aux(int16_t servo_out) { _servo_aux.servo_out = servo_out; _servo_aux.calc_pwm(); - hal.rcout->write(AP_MOTORS_HELI_SINGLE_AUX, _servo_aux.radio_out); + rc_write(AP_MOTORS_HELI_SINGLE_AUX, _servo_aux.radio_out); } // servo_test - move servos through full range of movement diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp index 5147f64c94..aa2748082f 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.cpp +++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp @@ -98,7 +98,7 @@ void AP_MotorsMatrix::output_min() hal.rcout->cork(); for( i=0; iwrite(i, _throttle_radio_min); + rc_write(i, _throttle_radio_min); } } hal.rcout->push(); @@ -164,7 +164,7 @@ void AP_MotorsMatrix::output_armed_not_stabilizing() hal.rcout->cork(); for( i=0; iwrite(i, motor_out[i]); + rc_write(i, motor_out[i]); } } hal.rcout->push(); @@ -362,7 +362,7 @@ void AP_MotorsMatrix::output_armed_stabilizing() hal.rcout->cork(); for( i=0; iwrite(i, motor_out[i]); + rc_write(i, motor_out[i]); } } hal.rcout->push(); @@ -390,7 +390,7 @@ void AP_MotorsMatrix::output_test(uint8_t motor_seq, int16_t pwm) for (uint8_t i=0; iwrite(i, pwm); + rc_write(i, pwm); } } hal.rcout->push(); diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.cpp b/libraries/AP_Motors/AP_MotorsMulticopter.cpp index 4263732277..3cac7c2786 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.cpp +++ b/libraries/AP_Motors/AP_MotorsMulticopter.cpp @@ -381,7 +381,7 @@ void AP_MotorsMulticopter::throttle_pass_through(int16_t pwm) hal.rcout->cork(); for (uint16_t i=0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) { if (motor_enabled[i]) { - hal.rcout->write(i, pwm); + rc_write(i, pwm); } } hal.rcout->push(); diff --git a/libraries/AP_Motors/AP_MotorsSingle.cpp b/libraries/AP_Motors/AP_MotorsSingle.cpp index dfe7c7faa2..8705088ba9 100644 --- a/libraries/AP_Motors/AP_MotorsSingle.cpp +++ b/libraries/AP_Motors/AP_MotorsSingle.cpp @@ -117,11 +117,11 @@ 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); + rc_write(AP_MOTORS_MOT_1, _servo1.radio_trim); + rc_write(AP_MOTORS_MOT_2, _servo2.radio_trim); + rc_write(AP_MOTORS_MOT_3, _servo3.radio_trim); + rc_write(AP_MOTORS_MOT_4, _servo4.radio_trim); + rc_write(AP_MOTORS_MOT_7, _throttle_radio_min); hal.rcout->push(); } @@ -175,11 +175,11 @@ void AP_MotorsSingle::output_armed_not_stabilizing() } 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); + rc_write(AP_MOTORS_MOT_1, _servo1.radio_out); + rc_write(AP_MOTORS_MOT_2, _servo2.radio_out); + rc_write(AP_MOTORS_MOT_3, _servo3.radio_out); + rc_write(AP_MOTORS_MOT_4, _servo4.radio_out); + rc_write(AP_MOTORS_MOT_7, throttle_radio_output); hal.rcout->push(); } @@ -234,11 +234,11 @@ void AP_MotorsSingle::output_armed_stabilizing() // 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); + rc_write(AP_MOTORS_MOT_1, _servo1.radio_out); + rc_write(AP_MOTORS_MOT_2, _servo2.radio_out); + rc_write(AP_MOTORS_MOT_3, _servo3.radio_out); + rc_write(AP_MOTORS_MOT_4, _servo4.radio_out); + rc_write(AP_MOTORS_MOT_7, throttle_radio_output); hal.rcout->push(); } @@ -263,23 +263,23 @@ void AP_MotorsSingle::output_test(uint8_t motor_seq, int16_t pwm) switch (motor_seq) { case 1: // flap servo 1 - hal.rcout->write(AP_MOTORS_MOT_1, pwm); + rc_write(AP_MOTORS_MOT_1, pwm); break; case 2: // flap servo 2 - hal.rcout->write(AP_MOTORS_MOT_2, pwm); + rc_write(AP_MOTORS_MOT_2, pwm); break; case 3: // flap servo 3 - hal.rcout->write(AP_MOTORS_MOT_3, pwm); + rc_write(AP_MOTORS_MOT_3, pwm); break; case 4: // flap servo 4 - hal.rcout->write(AP_MOTORS_MOT_4, pwm); + rc_write(AP_MOTORS_MOT_4, pwm); break; case 5: // spin main motor - hal.rcout->write(AP_MOTORS_MOT_7, pwm); + rc_write(AP_MOTORS_MOT_7, pwm); break; default: // do nothing diff --git a/libraries/AP_Motors/AP_MotorsTri.cpp b/libraries/AP_Motors/AP_MotorsTri.cpp index 3e63bad44c..8edec89246 100644 --- a/libraries/AP_Motors/AP_MotorsTri.cpp +++ b/libraries/AP_Motors/AP_MotorsTri.cpp @@ -118,10 +118,10 @@ void AP_MotorsTri::output_min() // 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); + rc_write(AP_MOTORS_MOT_1, _throttle_radio_min); + rc_write(AP_MOTORS_MOT_2, _throttle_radio_min); + rc_write(AP_MOTORS_MOT_4, _throttle_radio_min); + rc_write(AP_MOTORS_CH_TRI_YAW, _yaw_servo_trim); hal.rcout->push(); } @@ -176,12 +176,12 @@ void AP_MotorsTri::output_armed_not_stabilizing() 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]); - hal.rcout->write(AP_MOTORS_MOT_4, motor_out[AP_MOTORS_MOT_4]); + rc_write(AP_MOTORS_MOT_1, motor_out[AP_MOTORS_MOT_1]); + rc_write(AP_MOTORS_MOT_2, motor_out[AP_MOTORS_MOT_2]); + rc_write(AP_MOTORS_MOT_4, motor_out[AP_MOTORS_MOT_4]); // send centering signal to yaw servo - hal.rcout->write(AP_MOTORS_CH_TRI_YAW, _yaw_servo_trim); + rc_write(AP_MOTORS_CH_TRI_YAW, _yaw_servo_trim); hal.rcout->push(); } @@ -293,12 +293,12 @@ void AP_MotorsTri::output_armed_stabilizing() 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]); - hal.rcout->write(AP_MOTORS_MOT_4, motor_out[AP_MOTORS_MOT_4]); + rc_write(AP_MOTORS_MOT_1, motor_out[AP_MOTORS_MOT_1]); + rc_write(AP_MOTORS_MOT_2, motor_out[AP_MOTORS_MOT_2]); + rc_write(AP_MOTORS_MOT_4, motor_out[AP_MOTORS_MOT_4]); // send out to yaw command to tail servo - hal.rcout->write(AP_MOTORS_CH_TRI_YAW, yaw_radio_output); + rc_write(AP_MOTORS_CH_TRI_YAW, yaw_radio_output); hal.rcout->push(); } @@ -324,19 +324,19 @@ void AP_MotorsTri::output_test(uint8_t motor_seq, int16_t pwm) switch (motor_seq) { case 1: // front right motor - hal.rcout->write(AP_MOTORS_MOT_1, pwm); + rc_write(AP_MOTORS_MOT_1, pwm); break; case 2: // back motor - hal.rcout->write(AP_MOTORS_MOT_4, pwm); + rc_write(AP_MOTORS_MOT_4, pwm); break; case 3: // back servo - hal.rcout->write(AP_MOTORS_CH_TRI_YAW, pwm); + rc_write(AP_MOTORS_CH_TRI_YAW, pwm); break; case 4: // front left motor - hal.rcout->write(AP_MOTORS_MOT_2, pwm); + rc_write(AP_MOTORS_MOT_2, pwm); break; default: // do nothing diff --git a/libraries/AP_Motors/AP_Motors_Class.cpp b/libraries/AP_Motors/AP_Motors_Class.cpp index e7622cc078..26587b735b 100644 --- a/libraries/AP_Motors/AP_Motors_Class.cpp +++ b/libraries/AP_Motors/AP_Motors_Class.cpp @@ -64,3 +64,11 @@ void AP_Motors::armed(bool arm) _flags.armed = arm; AP_Notify::flags.armed = arm; }; + +/* + write to an output channel + */ +void AP_Motors::rc_write(uint8_t chan, uint16_t pwm) +{ + hal.rcout->write(chan, pwm); +} diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index 84751b6de9..fd723f4a49 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -129,7 +129,8 @@ protected: virtual void output_armed_not_stabilizing()=0; virtual void output_armed_zero_throttle() { output_min(); } virtual void output_disarmed()=0; - + virtual void rc_write(uint8_t chan, uint16_t pwm); + // update the throttle input filter virtual void update_throttle_filter() = 0;