From 9e66b555cb0bef04f5adc4c3477774aa614eaec0 Mon Sep 17 00:00:00 2001 From: rmackay9 Date: Thu, 13 Sep 2012 21:31:13 +0900 Subject: [PATCH] ArduCopter: removed INSTANT_PWM from ArduCopter and AP_Motors library --- ArduCopter/ArduCopter.pde | 12 -------- ArduCopter/config.h | 7 +---- ArduCopter/radio.pde | 5 ---- libraries/AP_Motors/AP_Motors.h | 5 ++-- libraries/AP_Motors/AP_MotorsHeli.cpp | 12 ++------ libraries/AP_Motors/AP_MotorsHeli.h | 2 +- libraries/AP_Motors/AP_MotorsMatrix.cpp | 39 ++----------------------- libraries/AP_Motors/AP_MotorsMatrix.h | 10 +------ libraries/AP_Motors/AP_MotorsTri.cpp | 18 ++---------- libraries/AP_Motors/AP_MotorsTri.h | 2 +- 10 files changed, 12 insertions(+), 100 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 720fbdaa64..bc13a85ea8 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -433,23 +433,11 @@ static byte oldSwitchPosition; #endif #if FRAME_CONFIG == HELI_FRAME // helicopter constructor requires more arguments - #if INSTANT_PWM == 1 -MOTOR_CLASS motors(CONFIG_APM_HARDWARE, &APM_RC, &g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, &g.rc_8, &g.heli_servo_1, &g.heli_servo_2, &g.heli_servo_3, &g.heli_servo_4, AP_MOTORS_SPEED_INSTANT_PWM); // this hardware definition is slightly bad because it assumes APM_HARDWARE_APM2 == AP_MOTORS_APM2 - #else MOTOR_CLASS motors(CONFIG_APM_HARDWARE, &APM_RC, &g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, &g.rc_8, &g.heli_servo_1, &g.heli_servo_2, &g.heli_servo_3, &g.heli_servo_4); - #endif #elif FRAME_CONFIG == TRI_FRAME // tri constructor requires additional rc_7 argument to allow tail servo reversing - #if INSTANT_PWM == 1 -MOTOR_CLASS motors(CONFIG_APM_HARDWARE, &APM_RC, &g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, &g.rc_7, AP_MOTORS_SPEED_INSTANT_PWM); // this hardware definition is slightly bad because it assumes APM_HARDWARE_APM2 == AP_MOTORS_APM2 - #else MOTOR_CLASS motors(CONFIG_APM_HARDWARE, &APM_RC, &g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, &g.rc_7); - #endif #else - #if INSTANT_PWM == 1 -MOTOR_CLASS motors(CONFIG_APM_HARDWARE, &APM_RC, &g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, AP_MOTORS_SPEED_INSTANT_PWM); // this hardware definition is slightly bad because it assumes APM_HARDWARE_APM2 == AP_MOTORS_APM2 - #else MOTOR_CLASS motors(CONFIG_APM_HARDWARE, &APM_RC, &g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4); - #endif #endif //////////////////////////////////////////////////////////////////////////////// diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 8e022c43e8..9a5a33c144 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -123,12 +123,7 @@ ////////////////////////////////////////////////////////////////////////////// // PWM control -// -#ifndef INSTANT_PWM - # define INSTANT_PWM DISABLED -#endif - -// default RC speed in Hz if INSTANT_PWM is not used +// default RC speed in Hz #ifndef RC_FAST_SPEED # if FRAME_CONFIG == HELI_FRAME # define RC_FAST_SPEED 125 diff --git a/ArduCopter/radio.pde b/ArduCopter/radio.pde index 2c78c68e6c..9417446784 100644 --- a/ArduCopter/radio.pde +++ b/ArduCopter/radio.pde @@ -64,12 +64,7 @@ static void init_rc_out() { APM_RC.Init( &isr_registry ); // APM Radio initialization -#if INSTANT_PWM == 1 - motors.set_update_rate(AP_MOTORS_SPEED_INSTANT_PWM); -#else motors.set_update_rate(g.rc_speed); -#endif - motors.set_frame_orientation(g.frame_orientation); motors.Init(); // motor initialisation motors.set_min_throttle(g.throttle_min); diff --git a/libraries/AP_Motors/AP_Motors.h b/libraries/AP_Motors/AP_Motors.h index a3a1918391..c1e0c3b9aa 100644 --- a/libraries/AP_Motors/AP_Motors.h +++ b/libraries/AP_Motors/AP_Motors.h @@ -39,9 +39,8 @@ #define AP_MOTORS_X_FRAME 1 #define AP_MOTORS_V_FRAME 2 -// motor update rates +// motor update rate #define AP_MOTORS_SPEED_DEFAULT 490 -#define AP_MOTORS_SPEED_INSTANT_PWM 0 // top-bottom ratio (for Y6) #define AP_MOTORS_TOP_BOTTOM_RATIO 1.0 @@ -69,7 +68,7 @@ public: _motor_to_channel_map[AP_MOTORS_MOT_8] = mot_8; } - // set update rate to motors - a value in hertz or AP_MOTORS_SPEED_INSTANT_PWM for instant pwm + // set update rate to motors - a value in hertz virtual void set_update_rate( uint16_t speed_hz ) { _speed_hz = speed_hz; }; diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index 0b6a4576e3..203794b221 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -170,16 +170,14 @@ void AP_MotorsHeli::Init() set_update_rate(_speed_hz); } -// set update rate to motors - a value in hertz or AP_MOTORS_SPEED_INSTANT_PWM for instant pwm +// set update rate to motors - a value in hertz void AP_MotorsHeli::set_update_rate( uint16_t speed_hz ) { // record requested speed _speed_hz = speed_hz; // setup fast channels - if( _speed_hz != AP_MOTORS_SPEED_INSTANT_PWM ) { - _rc->SetFastOutputChannels(_BV(_motor_to_channel_map[AP_MOTORS_MOT_1]) | _BV(_motor_to_channel_map[AP_MOTORS_MOT_2]) | _BV(_motor_to_channel_map[AP_MOTORS_MOT_3]) | _BV(_motor_to_channel_map[AP_MOTORS_MOT_4]), _speed_hz); - } + _rc->SetFastOutputChannels(_BV(_motor_to_channel_map[AP_MOTORS_MOT_1]) | _BV(_motor_to_channel_map[AP_MOTORS_MOT_2]) | _BV(_motor_to_channel_map[AP_MOTORS_MOT_3]) | _BV(_motor_to_channel_map[AP_MOTORS_MOT_4]), _speed_hz); } // enable - starts allowing signals to be sent to motors @@ -498,12 +496,6 @@ void AP_MotorsHeli::move_swash(int16_t roll_out, int16_t pitch_out, int16_t coll if( ext_gyro_enabled ) { _rc->OutputCh(AP_MOTORS_HELI_EXT_GYRO, ext_gyro_gain); } - - // InstantPWM - if( _speed_hz == AP_MOTORS_SPEED_INSTANT_PWM ) { - _rc->Force_Out0_Out1(); - _rc->Force_Out2_Out3(); - } } void AP_MotorsHeli::rsc_control() diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index f80c5918a7..c5f0430a96 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -106,7 +106,7 @@ public: // init void Init(); - // set update rate to motors - a value in hertz or AP_MOTORS_SPEED_INSTANT_PWM for instant pwm + // set update rate to motors - a value in hertz // you must have setup_motors before calling this void set_update_rate( uint16_t speed_hz ); diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp index a73712434a..a28bbd0f32 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.cpp +++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp @@ -28,7 +28,7 @@ void AP_MotorsMatrix::Init() set_update_rate(_speed_hz); } -// set update rate to motors - a value in hertz or AP_MOTORS_SPEED_INSTANT_PWM for instant pwm +// set update rate to motors - a value in hertz void AP_MotorsMatrix::set_update_rate( uint16_t speed_hz ) { uint32_t fast_channel_mask = 0; @@ -37,31 +37,16 @@ void AP_MotorsMatrix::set_update_rate( uint16_t speed_hz ) // record requested speed _speed_hz = speed_hz; - // initialise instant_pwm booleans. they will be set again below - instant_pwm_force01 = false; - instant_pwm_force23 = false; - instant_pwm_force67 = false; - // check each enabled motor for( i=0; iSetFastOutputChannels(fast_channel_mask, _speed_hz); - } + _rc->SetFastOutputChannels(fast_channel_mask, _speed_hz); } // set frame orientation (normally + or X) @@ -107,16 +92,6 @@ void AP_MotorsMatrix::output_min() _rc->OutputCh(_motor_to_channel_map[i], motor_out[i]); } } - - // Force output if instant pwm - if( _speed_hz == AP_MOTORS_SPEED_INSTANT_PWM ) { - if( instant_pwm_force01 ) - _rc->Force_Out0_Out1(); - if( instant_pwm_force23 ) - _rc->Force_Out2_Out3(); - if( instant_pwm_force67 ) - _rc->Force_Out6_Out7(); - } } // output_armed - sends commands to the motors @@ -189,16 +164,6 @@ void AP_MotorsMatrix::output_armed() _rc->OutputCh(_motor_to_channel_map[i], motor_out[i]); } } - - // InstantPWM - if( _speed_hz == AP_MOTORS_SPEED_INSTANT_PWM ) { - if( instant_pwm_force01 ) - _rc->Force_Out0_Out1(); - if( instant_pwm_force23 ) - _rc->Force_Out2_Out3(); - if( instant_pwm_force67 ) - _rc->Force_Out6_Out7(); - } } // output_disarmed - sends commands to the motors diff --git a/libraries/AP_Motors/AP_MotorsMatrix.h b/libraries/AP_Motors/AP_MotorsMatrix.h index dc0671a753..37baaf5518 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.h +++ b/libraries/AP_Motors/AP_MotorsMatrix.h @@ -26,16 +26,13 @@ public: /// Constructor AP_MotorsMatrix( uint8_t APM_version, APM_RC_Class* rc_out, RC_Channel* rc_roll, RC_Channel* rc_pitch, RC_Channel* rc_throttle, RC_Channel* rc_yaw, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_Motors(APM_version, rc_out, rc_roll, rc_pitch, rc_throttle, rc_yaw, speed_hz), - instant_pwm_force01(false), - instant_pwm_force23(false), - instant_pwm_force67(false), _num_motors(0) { }; // init virtual void Init(); - // set update rate to motors - a value in hertz or AP_MOTORS_SPEED_INSTANT_PWM for instant pwm + // set update rate to motors - a value in hertz // you must have setup_motors before calling this virtual void set_update_rate( uint16_t speed_hz ); @@ -74,11 +71,6 @@ public: AP_Int8 opposite_motor[AP_MOTORS_MAX_NUM_MOTORS]; // motor number of the opposite motor AP_Int8 test_order[AP_MOTORS_MAX_NUM_MOTORS]; // order of the motors in the test sequence - // used for instant_pwm only - bool instant_pwm_force01; - bool instant_pwm_force23; - bool instant_pwm_force67; - protected: // output - sends commands to the motors virtual void output_armed(); diff --git a/libraries/AP_Motors/AP_MotorsTri.cpp b/libraries/AP_Motors/AP_MotorsTri.cpp index 00158f464e..cfca645103 100644 --- a/libraries/AP_Motors/AP_MotorsTri.cpp +++ b/libraries/AP_Motors/AP_MotorsTri.cpp @@ -17,16 +17,14 @@ void AP_MotorsTri::Init() set_update_rate(_speed_hz); } -// set update rate to motors - a value in hertz or AP_MOTORS_SPEED_INSTANT_PWM for instant pwm +// set update rate to motors - a value in hertz void AP_MotorsTri::set_update_rate( uint16_t speed_hz ) { // record requested speed _speed_hz = speed_hz; // set update rate for the 3 motors (but not the servo on channel 7) - if( _speed_hz != AP_MOTORS_SPEED_INSTANT_PWM ) { - _rc->SetFastOutputChannels(_BV(_motor_to_channel_map[AP_MOTORS_MOT_1]) | _BV(_motor_to_channel_map[AP_MOTORS_MOT_2]) | _BV(_motor_to_channel_map[AP_MOTORS_MOT_4]), _speed_hz); - } + _rc->SetFastOutputChannels(_BV(_motor_to_channel_map[AP_MOTORS_MOT_1]) | _BV(_motor_to_channel_map[AP_MOTORS_MOT_2]) | _BV(_motor_to_channel_map[AP_MOTORS_MOT_4]), _speed_hz); } // enable - starts allowing signals to be sent to motors @@ -52,12 +50,6 @@ void AP_MotorsTri::output_min() _rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_2], _rc_throttle->radio_min); _rc->OutputCh(_motor_to_channel_map[AP_MOTORS_MOT_4], _rc_throttle->radio_min); _rc->OutputCh(_motor_to_channel_map[AP_MOTORS_CH_TRI_YAW], _rc_yaw->radio_trim); - - // InstantPWM - if( _speed_hz == AP_MOTORS_SPEED_INSTANT_PWM ) { - _rc->Force_Out0_Out1(); - _rc->Force_Out2_Out3(); - } } // output_armed - sends commands to the motors @@ -135,12 +127,6 @@ void AP_MotorsTri::output_armed() }else{ _rc->OutputCh(AP_MOTORS_CH_TRI_YAW, _rc_yaw->radio_out); } - - // InstantPWM - if( _speed_hz == AP_MOTORS_SPEED_INSTANT_PWM ) { - _rc->Force_Out0_Out1(); - _rc->Force_Out2_Out3(); - } } // output_disarmed - sends commands to the motors diff --git a/libraries/AP_Motors/AP_MotorsTri.h b/libraries/AP_Motors/AP_MotorsTri.h index a7981f02ed..e4c6a4e24c 100644 --- a/libraries/AP_Motors/AP_MotorsTri.h +++ b/libraries/AP_Motors/AP_MotorsTri.h @@ -29,7 +29,7 @@ public: // init virtual void Init(); - // set update rate to motors - a value in hertz or AP_MOTORS_SPEED_INSTANT_PWM for instant pwm + // 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