From 7df9b2eb8c9358d9dffa269cc08587d91e875348 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Wed, 20 Jan 2016 12:54:01 +0900 Subject: [PATCH] AP_MotorsCoax: support 4 servo outputs --- libraries/AP_Motors/AP_MotorsCoax.cpp | 67 ++++++++++++++++++++------- libraries/AP_Motors/AP_MotorsCoax.h | 30 ++++++++++-- 2 files changed, 76 insertions(+), 21 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsCoax.cpp b/libraries/AP_Motors/AP_MotorsCoax.cpp index c23592df2e..8e336e958b 100644 --- a/libraries/AP_Motors/AP_MotorsCoax.cpp +++ b/libraries/AP_Motors/AP_MotorsCoax.cpp @@ -64,18 +64,25 @@ const AP_Param::GroupInfo AP_MotorsCoax::var_info[] = { // init void AP_MotorsCoax::Init() { - // set update rate for the 2 motors (but not the servo on channel 1&2) + // set update rate for the 3 motors (but not the servo on channel 7) set_update_rate(_speed_hz); - // set the motor_enabled flag so that the ESCs can be calibrated like other frame types - motor_enabled[AP_MOTORS_MOT_3] = true; - motor_enabled[AP_MOTORS_MOT_4] = true; + // set the motor_enabled flag so that the main ESC can be calibrated like other frame types + motor_enabled[AP_MOTORS_MOT_5] = true; + motor_enabled[AP_MOTORS_MOT_6] = true; - // set ranges for fin servos + // we set four servos to angle _servo1.set_type(RC_CHANNEL_TYPE_ANGLE); _servo2.set_type(RC_CHANNEL_TYPE_ANGLE); + _servo3.set_type(RC_CHANNEL_TYPE_ANGLE); + _servo4.set_type(RC_CHANNEL_TYPE_ANGLE); _servo1.set_angle(AP_MOTORS_COAX_SERVO_INPUT_RANGE); _servo2.set_angle(AP_MOTORS_COAX_SERVO_INPUT_RANGE); + _servo3.set_angle(AP_MOTORS_COAX_SERVO_INPUT_RANGE); + _servo4.set_angle(AP_MOTORS_COAX_SERVO_INPUT_RANGE); + + // disable CH7 from being used as an aux output (i.e. for camera gimbal, etc) + RC_Channel_aux::disable_aux_channel(CH_7); } // set update rate to motors - a value in hertz @@ -84,17 +91,17 @@ void AP_MotorsCoax::set_update_rate( uint16_t speed_hz ) // record requested speed _speed_hz = speed_hz; - // set update rate for the two motors - uint32_t mask2 = + // set update rate for the 4 servos and 2 motors + uint32_t mask = + 1U << AP_MOTORS_MOT_1 | + 1U << AP_MOTORS_MOT_2 | 1U << AP_MOTORS_MOT_3 | 1U << AP_MOTORS_MOT_4 ; - rc_set_freq(mask2, _speed_hz); - - // set update rate for the two servos - uint32_t mask = - 1U << AP_MOTORS_MOT_1 | - 1U << AP_MOTORS_MOT_2 ; rc_set_freq(mask, _servo_speed); + uint32_t mask2 = + 1U << AP_MOTORS_MOT_5 | + 1U << AP_MOTORS_MOT_6 ; + rc_set_freq(mask2, _speed_hz); } // enable - starts allowing signals to be sent to motors @@ -105,6 +112,8 @@ void AP_MotorsCoax::enable() 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); } // output_min - sends minimum values out to the motor and trim values to the servos @@ -114,11 +123,27 @@ void AP_MotorsCoax::output_min() hal.rcout->cork(); 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); + rc_write(AP_MOTORS_MOT_3, _servo3.radio_trim); + rc_write(AP_MOTORS_MOT_4, _servo4.radio_trim); + rc_write(AP_MOTORS_MOT_5, _throttle_radio_min); + rc_write(AP_MOTORS_MOT_6, _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) +// this can be used to ensure other pwm outputs (i.e. for servos) do not conflict +uint16_t AP_MotorsCoax::get_motor_mask() +{ + uint32_t mask = + 1U << AP_MOTORS_MOT_1 | + 1U << AP_MOTORS_MOT_2 | + 1U << AP_MOTORS_MOT_3 | + 1U << AP_MOTORS_MOT_4 | + 1U << AP_MOTORS_MOT_5 | + 1U << AP_MOTORS_MOT_6; + return rc_map_mask(mask); +} + // sends commands to the motors void AP_MotorsCoax::output_armed_stabilizing() { @@ -199,13 +224,21 @@ void AP_MotorsCoax::output_test(uint8_t motor_seq, int16_t pwm) rc_write(AP_MOTORS_MOT_2, pwm); break; case 3: - // motor 1 + // flap servo 3 rc_write(AP_MOTORS_MOT_3, pwm); break; case 4: - // motor 2 + // flap servo 4 rc_write(AP_MOTORS_MOT_4, pwm); break; + case 5: + // motor 1 + rc_write(AP_MOTORS_MOT_5, pwm); + break; + case 6: + // motor 2 + rc_write(AP_MOTORS_MOT_6, pwm); + break; default: // do nothing break; diff --git a/libraries/AP_Motors/AP_MotorsCoax.h b/libraries/AP_Motors/AP_MotorsCoax.h index 67ed3ec441..89209ba85e 100644 --- a/libraries/AP_Motors/AP_MotorsCoax.h +++ b/libraries/AP_Motors/AP_MotorsCoax.h @@ -23,10 +23,12 @@ class AP_MotorsCoax : public AP_MotorsMulticopter { public: /// Constructor - AP_MotorsCoax(RC_Channel& servo1, RC_Channel& servo2, uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : + AP_MotorsCoax(RC_Channel& servo1, RC_Channel& servo2, RC_Channel& servo3, RC_Channel& servo4, uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsMulticopter(loop_rate, speed_hz), _servo1(servo1), - _servo2(servo2) + _servo2(servo2), + _servo3(servo3), + _servo4(servo4) { AP_Param::setup_object_defaults(this, var_info); }; @@ -50,8 +52,7 @@ public: // get_motor_mask - returns a bitmask of which outputs are being used for motors or servos (1 means being used) // this can be used to ensure other pwm outputs (i.e. for servos) do not conflict - // for coax copter, output channels 1 to 4 are used - virtual uint16_t get_motor_mask() { return 0x000F; } + virtual uint16_t get_motor_mask(); // var_info for holding Parameter information static const struct AP_Param::GroupInfo var_info[]; @@ -65,6 +66,27 @@ protected: AP_Int8 _pitch_reverse; // Reverse pitch output AP_Int8 _yaw_reverse; // Reverse yaw output AP_Int16 _servo_speed; // servo speed + // Allow the use of a 4 servo output to make it easy to test coax and single using same airframe RC_Channel& _servo1; RC_Channel& _servo2; + RC_Channel& _servo3; + RC_Channel& _servo4; + + AP_Int8 _servo_1_reverse; // Roll servo signal reversing + AP_Int16 _servo_1_trim; // Trim or center position of roll servo + AP_Int16 _servo_1_min; // Minimum angle limit of roll servo + AP_Int16 _servo_1_max; // Maximum angle limit of roll servo + AP_Int8 _servo_2_reverse; // Pitch servo signal reversing + AP_Int16 _servo_2_trim; // Trim or center position of pitch servo + AP_Int16 _servo_2_min; // Minimum angle limit of pitch servo + AP_Int16 _servo_2_max; // Maximum angle limit of pitch servo + AP_Int8 _servo_3_reverse; // Pitch servo signal reversing + AP_Int16 _servo_3_trim; // Trim or center position of pitch servo + AP_Int16 _servo_3_min; // Minimum angle limit of pitch servo + AP_Int16 _servo_3_max; // Maximum angle limit of pitch servo + AP_Int8 _servo_4_reverse; // Pitch servo signal reversing + AP_Int16 _servo_4_trim; // Trim or center position of pitch servo + AP_Int16 _servo_4_min; // Minimum angle limit of pitch servo + AP_Int16 _servo_4_max; // Maximum angle limit of pitch servo + };