diff --git a/libraries/AP_Motors/AP_MotorsCoax.cpp b/libraries/AP_Motors/AP_MotorsCoax.cpp index ea18c7dc2a..1c1f7ca18c 100644 --- a/libraries/AP_Motors/AP_MotorsCoax.cpp +++ b/libraries/AP_Motors/AP_MotorsCoax.cpp @@ -72,18 +72,6 @@ void AP_MotorsCoax::set_update_rate( uint16_t speed_hz ) rc_set_freq(mask, _speed_hz); } -// enable - starts allowing signals to be sent to motors -void AP_MotorsCoax::enable() -{ - // enable output channels - rc_enable_ch(AP_MOTORS_MOT_1); - rc_enable_ch(AP_MOTORS_MOT_2); - rc_enable_ch(AP_MOTORS_MOT_3); - rc_enable_ch(AP_MOTORS_MOT_4); - rc_enable_ch(AP_MOTORS_MOT_5); - rc_enable_ch(AP_MOTORS_MOT_6); -} - void AP_MotorsCoax::output_to_motors() { switch (_spool_mode) { diff --git a/libraries/AP_Motors/AP_MotorsCoax.h b/libraries/AP_Motors/AP_MotorsCoax.h index 98f7ddc1df..b4ba1efd52 100644 --- a/libraries/AP_Motors/AP_MotorsCoax.h +++ b/libraries/AP_Motors/AP_MotorsCoax.h @@ -37,9 +37,6 @@ public: // set update rate to motors - a value in hertz void set_update_rate( uint16_t speed_hz ); - // enable - starts allowing signals to be sent to motors - virtual void enable(); - // output_test - spin a motor at the pwm value specified // motor_seq is the motor's sequence number from 1 to the number of motors on the frame // pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000 diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index fff7f40032..101e68a459 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -68,9 +68,6 @@ public: // set update rate to motors - a value in hertz virtual void set_update_rate( uint16_t speed_hz ) = 0; - // enable - starts allowing signals to be sent to motors - virtual void enable() = 0; - // output_min - sets servos to neutral point with motors stopped void output_min(); diff --git a/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp b/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp index 3b60b5d6ca..493710fd45 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp @@ -193,20 +193,6 @@ void AP_MotorsHeli_Dual::set_update_rate( uint16_t speed_hz ) rc_set_freq(mask, _speed_hz); } -// enable - starts allowing signals to be sent to motors -void AP_MotorsHeli_Dual::enable() -{ - // enable output channels - rc_enable_ch(AP_MOTORS_MOT_1); - rc_enable_ch(AP_MOTORS_MOT_2); - rc_enable_ch(AP_MOTORS_MOT_3); - rc_enable_ch(AP_MOTORS_MOT_4); - rc_enable_ch(AP_MOTORS_MOT_5); - rc_enable_ch(AP_MOTORS_MOT_6); - - rc_enable_ch(AP_MOTORS_HELI_DUAL_RSC); -} - // init_outputs bool AP_MotorsHeli_Dual::init_outputs() { diff --git a/libraries/AP_Motors/AP_MotorsHeli_Dual.h b/libraries/AP_Motors/AP_MotorsHeli_Dual.h index 87c62696e1..a83f8c1602 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Dual.h +++ b/libraries/AP_Motors/AP_MotorsHeli_Dual.h @@ -54,9 +54,6 @@ public: // set_update_rate - set update rate to motors void set_update_rate( uint16_t speed_hz ) override; - // enable - starts allowing signals to be sent to motors - void enable() override; - // output_test - spin a motor at the pwm value specified void output_test(uint8_t motor_seq, int16_t pwm) override; diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp index 7c7bc69945..b80ef7b2da 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp @@ -157,18 +157,6 @@ void AP_MotorsHeli_Single::set_update_rate( uint16_t speed_hz ) rc_set_freq(mask, _speed_hz); } -// enable - starts allowing signals to be sent to motors and servos -void AP_MotorsHeli_Single::enable() -{ - // enable output channels - rc_enable_ch(AP_MOTORS_MOT_1); // swash servo 1 - rc_enable_ch(AP_MOTORS_MOT_2); // swash servo 2 - rc_enable_ch(AP_MOTORS_MOT_3); // swash servo 3 - rc_enable_ch(AP_MOTORS_MOT_4); // yaw - rc_enable_ch(AP_MOTORS_HELI_SINGLE_AUX); // output for gyro gain or direct drive variable pitch tail motor - rc_enable_ch(AP_MOTORS_HELI_SINGLE_RSC); // output for main rotor esc -} - // init_outputs - initialise Servo/PWM ranges and endpoints bool AP_MotorsHeli_Single::init_outputs() { diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.h b/libraries/AP_Motors/AP_MotorsHeli_Single.h index 04befe2b4c..004c9e2c81 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.h +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.h @@ -57,9 +57,6 @@ public: // set update rate to motors - a value in hertz void set_update_rate(uint16_t speed_hz) override; - // enable - starts allowing signals to be sent to motors and servos - void enable() override; - // output_test - spin a motor at the pwm value specified // motor_seq is the motor's sequence number from 1 to the number of motors on the frame // pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000 diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp index 0d79b069f7..eab36c53aa 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.cpp +++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp @@ -72,19 +72,6 @@ void AP_MotorsMatrix::set_frame_class_and_type(motor_frame_class frame_class, mo set_update_rate(_speed_hz); } -// enable - starts allowing signals to be sent to motors -void AP_MotorsMatrix::enable() -{ - int8_t i; - - // enable output channels - for( i=0; ienable_ch(chan); -} - /* map an internal motor mask to real motor mask */ diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index 8738245d95..3ed5dffb38 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -141,9 +141,6 @@ public: // set frame class (i.e. quad, hexa, heli) and type (i.e. x, plus) virtual void set_frame_class_and_type(motor_frame_class frame_class, motor_frame_type frame_type) = 0; - // enable - starts allowing signals to be sent to motors - virtual void enable() = 0; - // output - sends commands to the motors virtual void output() = 0; @@ -173,7 +170,6 @@ protected: virtual void output_armed_stabilizing()=0; virtual void rc_write(uint8_t chan, uint16_t pwm); virtual void rc_set_freq(uint32_t mask, uint16_t freq_hz); - virtual void rc_enable_ch(uint8_t chan); virtual uint32_t rc_map_mask(uint32_t mask) const; // add a motor to the motor map diff --git a/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp b/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp index de3c206387..1a9a7c0af9 100644 --- a/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp +++ b/libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp @@ -29,6 +29,7 @@ #include #include #include +#include const AP_HAL::HAL& hal = AP_HAL::get_HAL(); @@ -64,7 +65,6 @@ void setup() motors.set_throttle_range(1000,2000); motors.set_throttle_avg_max(0.5f); #endif - motors.enable(); motors.output_min(); // setup radio @@ -138,6 +138,7 @@ void stability_test() // arm motors motors.armed(true); motors.set_interlock(true); + SRV_Channels::enable_aux_servos(); #if NUM_OUTPUTS <= 4 hal.console->printf("Roll,Pitch,Yaw,Thr,Mot1,Mot2,Mot3,Mot4,AvgOut,LimRP,LimY,LimThD,LimThU\n"); // quad