From 260006dcb3f6de4b68a41dcbaf0bd3e583cdfb10 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Wed, 20 Jan 2016 21:11:39 +0900 Subject: [PATCH] AP_MotorsCoax: move servo objects into Coax class --- libraries/AP_Motors/AP_MotorsCoax.cpp | 49 ++++++++++++++++++--------- libraries/AP_Motors/AP_MotorsCoax.h | 37 +++++--------------- 2 files changed, 42 insertions(+), 44 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsCoax.cpp b/libraries/AP_Motors/AP_MotorsCoax.cpp index 458e880ce9..e476f74fa3 100644 --- a/libraries/AP_Motors/AP_MotorsCoax.cpp +++ b/libraries/AP_Motors/AP_MotorsCoax.cpp @@ -59,6 +59,19 @@ const AP_Param::GroupInfo AP_MotorsCoax::var_info[] = { // @Units: Hz AP_GROUPINFO("SV_SPEED", 43, AP_MotorsCoax, _servo_speed, AP_MOTORS_SINGLE_SPEED_DIGITAL_SERVOS), + // @Group: SV1_ + // @Path: ../RC_Channel/RC_Channel.cpp + AP_SUBGROUPINFO(_servo1, "SV1_", 44, AP_MotorsCoax, RC_Channel), + // @Group: SV2_ + // @Path: ../RC_Channel/RC_Channel.cpp + AP_SUBGROUPINFO(_servo2, "SV2_", 45, AP_MotorsCoax, RC_Channel), + // @Group: SV3_ + // @Path: ../RC_Channel/RC_Channel.cpp + AP_SUBGROUPINFO(_servo3, "SV3_", 46, AP_MotorsCoax, RC_Channel), + // @Group: SV4_ + // @Path: ../RC_Channel/RC_Channel.cpp + AP_SUBGROUPINFO(_servo4, "SV4_", 47, AP_MotorsCoax, RC_Channel), + AP_GROUPEND }; // init @@ -136,10 +149,10 @@ void AP_MotorsCoax::output_to_motors() case SHUT_DOWN: // sends minimum values out to the motors hal.rcout->cork(); - rc_write(AP_MOTORS_MOT_1, calc_pivot_radio_output(_roll_radio_passthrough*_servo_1_reverse, _servo_1_min, _servo_1_trim, _servo_1_max)); - rc_write(AP_MOTORS_MOT_2, calc_pivot_radio_output(_pitch_radio_passthrough *_servo_2_reverse, _servo_2_min, _servo_2_trim, _servo_2_max)); - rc_write(AP_MOTORS_MOT_3, calc_pivot_radio_output(_roll_radio_passthrough*_servo_3_reverse, _servo_3_min, _servo_3_trim, _servo_3_max)); - rc_write(AP_MOTORS_MOT_4, calc_pivot_radio_output(_pitch_radio_passthrough*_servo_4_reverse, _servo_4_min, _servo_4_trim, _servo_4_max)); + rc_write(AP_MOTORS_MOT_1, calc_pivot_radio_output(_roll_radio_passthrough, _servo1)); + rc_write(AP_MOTORS_MOT_2, calc_pivot_radio_output(_pitch_radio_passthrough, _servo2)); + rc_write(AP_MOTORS_MOT_3, calc_pivot_radio_output(_roll_radio_passthrough, _servo3)); + rc_write(AP_MOTORS_MOT_4, calc_pivot_radio_output(_pitch_radio_passthrough, _servo4)); rc_write(AP_MOTORS_MOT_5, _throttle_radio_min); rc_write(AP_MOTORS_MOT_6, _throttle_radio_min); hal.rcout->push(); @@ -147,10 +160,10 @@ void AP_MotorsCoax::output_to_motors() case SPIN_WHEN_ARMED: // sends output to motors when armed but not flying hal.rcout->cork(); - rc_write(AP_MOTORS_MOT_1, calc_pivot_radio_output(_throttle_low_end_pct * _actuator_out[0]*_servo_1_reverse, _servo_1_min, _servo_1_trim, _servo_1_max)); - rc_write(AP_MOTORS_MOT_2, calc_pivot_radio_output(_throttle_low_end_pct * _actuator_out[1]*_servo_2_reverse, _servo_2_min, _servo_2_trim, _servo_2_max)); - rc_write(AP_MOTORS_MOT_3, calc_pivot_radio_output(_throttle_low_end_pct * _actuator_out[2]*_servo_3_reverse, _servo_3_min, _servo_3_trim, _servo_3_max)); - rc_write(AP_MOTORS_MOT_4, calc_pivot_radio_output(_throttle_low_end_pct * _actuator_out[3]*_servo_4_reverse, _servo_4_min, _servo_4_trim, _servo_4_max)); + rc_write(AP_MOTORS_MOT_1, calc_pivot_radio_output(_throttle_low_end_pct * _actuator_out[0], _servo1)); + rc_write(AP_MOTORS_MOT_2, calc_pivot_radio_output(_throttle_low_end_pct * _actuator_out[1], _servo2)); + rc_write(AP_MOTORS_MOT_3, calc_pivot_radio_output(_throttle_low_end_pct * _actuator_out[2], _servo3)); + rc_write(AP_MOTORS_MOT_4, calc_pivot_radio_output(_throttle_low_end_pct * _actuator_out[3], _servo4)); rc_write(AP_MOTORS_MOT_5, constrain_int16(_throttle_radio_min + _throttle_low_end_pct * _min_throttle, _throttle_radio_min, _throttle_radio_min + _min_throttle)); rc_write(AP_MOTORS_MOT_6, constrain_int16(_throttle_radio_min + _throttle_low_end_pct * _min_throttle, _throttle_radio_min, _throttle_radio_min + _min_throttle)); hal.rcout->push(); @@ -160,10 +173,10 @@ void AP_MotorsCoax::output_to_motors() case SPOOL_DOWN: // set motor output based on thrust requests hal.rcout->cork(); - rc_write(AP_MOTORS_MOT_1, calc_pivot_radio_output(_actuator_out[0]*_servo_1_reverse, _servo_1_min, _servo_1_trim, _servo_1_max)); - rc_write(AP_MOTORS_MOT_2, calc_pivot_radio_output(_actuator_out[1]*_servo_2_reverse, _servo_2_min, _servo_2_trim, _servo_2_max)); - rc_write(AP_MOTORS_MOT_3, calc_pivot_radio_output(_actuator_out[2]*_servo_3_reverse, _servo_3_min, _servo_3_trim, _servo_3_max)); - rc_write(AP_MOTORS_MOT_4, calc_pivot_radio_output(_actuator_out[3]*_servo_4_reverse, _servo_4_min, _servo_4_trim, _servo_4_max)); + rc_write(AP_MOTORS_MOT_1, calc_pivot_radio_output(_actuator_out[0], _servo1)); + rc_write(AP_MOTORS_MOT_2, calc_pivot_radio_output(_actuator_out[1], _servo2)); + rc_write(AP_MOTORS_MOT_3, calc_pivot_radio_output(_actuator_out[2], _servo3)); + rc_write(AP_MOTORS_MOT_4, calc_pivot_radio_output(_actuator_out[3], _servo4)); rc_write(AP_MOTORS_MOT_5, calc_thrust_to_pwm(_thrust_yt_ccw)); rc_write(AP_MOTORS_MOT_6, calc_thrust_to_pwm(_thrust_yt_cw)); hal.rcout->push(); @@ -338,14 +351,18 @@ void AP_MotorsCoax::output_test(uint8_t motor_seq, int16_t pwm) } // calc_yaw_radio_output - calculate final radio output for yaw channel -int16_t AP_MotorsCoax::calc_pivot_radio_output(float yaw_input, int16_t servo_min, int16_t servo_trim, int16_t servo_max) +int16_t AP_MotorsCoax::calc_pivot_radio_output(float yaw_input, const RC_Channel& servo) { int16_t ret; - if (yaw_input >= 0){ - ret = ((yaw_input * (servo_max - servo_trim)) + servo_trim); + if (servo.get_reverse()) { + yaw_input = -yaw_input; + } + + if (yaw_input >= 0.0f){ + ret = ((yaw_input * (servo.radio_max - servo.radio_trim)) + servo.radio_trim); } else { - ret = ((yaw_input * (servo_trim - servo_min)) + servo_trim); + ret = ((yaw_input * (servo.radio_trim - servo.radio_min)) + servo.radio_trim); } return ret; diff --git a/libraries/AP_Motors/AP_MotorsCoax.h b/libraries/AP_Motors/AP_MotorsCoax.h index 1b5d3d58eb..7cbfcb4d52 100644 --- a/libraries/AP_Motors/AP_MotorsCoax.h +++ b/libraries/AP_Motors/AP_MotorsCoax.h @@ -25,12 +25,9 @@ class AP_MotorsCoax : public AP_MotorsMulticopter { public: /// Constructor - 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_MotorsCoax(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : AP_MotorsMulticopter(loop_rate, speed_hz), - _servo1(servo1), - _servo2(servo2), - _servo3(servo3), - _servo4(servo4) + _servo1(CH_1), _servo2(CH_2), _servo3(CH_3), _servo4(CH_4) { AP_Param::setup_object_defaults(this, var_info); }; @@ -66,36 +63,20 @@ protected: // output - sends commands to the motors void output_armed_stabilizing(); - // calc_yaw_radio_output - calculate final radio output for yaw channel - int16_t calc_pivot_radio_output(float yaw_input, int16_t servo_min, int16_t servo_trim, int16_t servo_max); // calculate radio output for yaw servo, typically in range of 1100-1900 + // calc_yaw_radio_output - calculate final pwm output for yaw channel + int16_t calc_pivot_radio_output(float yaw_input, const RC_Channel& servo); // We shouldn't need roll, pitch, and yaw reversing with servo reversing. AP_Int8 _roll_reverse; // Reverse roll output 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 + RC_Channel _servo1; + RC_Channel _servo2; + RC_Channel _servo3; + RC_Channel _servo4; float _actuator_out[NUM_ACTUATORS]; // combined roll, pitch, yaw and throttle outputs to motors in 0~1 range float _thrust_yt_ccw;