diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 0cbaa9f934..ef9e8c195e 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -110,6 +110,9 @@ static void init_arm_motors() // disable cpu failsafe because initialising everything takes a while failsafe_disable(); + + motors.enable(); + motors.output_unsafe(); #if LOGGING_ENABLED == ENABLED // start dataflash @@ -178,9 +181,6 @@ static void init_arm_motors() piezo_beep_twice(); #endif - // enable output to motors - output_min(); - // finally actually arm the motors motors.armed(true); diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp index 6490703eb7..55477b9366 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.cpp +++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp @@ -453,6 +453,19 @@ void AP_MotorsMatrix::output_disarmed() output_min(); } +void AP_MotorsMatrix::output_unsafe() +{ + int8_t i; + + // fill the motor_out[] array for HIL use and send minimum value to each motor + for( i=0; iradio_min + constrain_int16(_throttle_unsafe,0,AP_MOTORS_THROTTLE_UNSAFE_MAX);; + hal.rcout->write(_motor_to_channel_map[i], motor_out[i]); + } + } +} + // output_disarmed - sends commands to the motors void AP_MotorsMatrix::output_test() { diff --git a/libraries/AP_Motors/AP_MotorsMatrix.h b/libraries/AP_Motors/AP_MotorsMatrix.h index 9ca3007528..e64620bc9e 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.h +++ b/libraries/AP_Motors/AP_MotorsMatrix.h @@ -50,6 +50,8 @@ public: // add_motor using just position and yaw_factor (or prop direction) void add_motor(int8_t motor_num, float angle_degrees, float yaw_factor, uint8_t testing_order); + + void output_unsafe(); // remove_motor void remove_motor(int8_t motor_num); diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index 195751c28a..5d7bdd18ef 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -85,6 +85,8 @@ public: // enable - starts allowing signals to be sent to motors virtual void enable() = 0; + + virtual void output_unsafe() = 0; // arm, disarm or check status status of motors bool armed() { return _armed; };