diff --git a/libraries/AP_Motors/AP_MotorsTailsitter.cpp b/libraries/AP_Motors/AP_MotorsTailsitter.cpp index 4113be1f5e..24110a9ab2 100644 --- a/libraries/AP_Motors/AP_MotorsTailsitter.cpp +++ b/libraries/AP_Motors/AP_MotorsTailsitter.cpp @@ -26,20 +26,32 @@ extern const AP_HAL::HAL& hal; #define SERVO_OUTPUT_RANGE 4500 -#define THROTTLE_RANGE 100 // init void AP_MotorsTailsitter::init(motor_frame_class frame_class, motor_frame_type frame_type) { - // make sure 4 output channels are mapped - add_motor_num(AP_MOTORS_THROTTLE_LEFT); - add_motor_num(AP_MOTORS_THROTTLE_RIGHT); - add_motor_num(AP_MOTORS_TILT_LEFT); - add_motor_num(AP_MOTORS_TILT_RIGHT); + // setup default motor and servo mappings + uint8_t chan; - // set the motor_enabled flag so that the main ESC can be calibrated like other frame types - motor_enabled[AP_MOTORS_THROTTLE_LEFT] = true; - motor_enabled[AP_MOTORS_THROTTLE_RIGHT] = true; + // right throttle defaults to servo output 1 + SRV_Channels::set_aux_channel_default(SRV_Channel::k_throttleRight, CH_1); + if (SRV_Channels::find_channel(SRV_Channel::k_throttleRight, chan)) { + motor_enabled[chan] = true; + } + + // left throttle defaults to servo output 2 + SRV_Channels::set_aux_channel_default(SRV_Channel::k_throttleLeft, CH_2); + if (SRV_Channels::find_channel(SRV_Channel::k_throttleLeft, chan)) { + motor_enabled[chan] = true; + } + + // right servo defaults to servo output 3 + SRV_Channels::set_aux_channel_default(SRV_Channel::k_tiltMotorRight, CH_3); + SRV_Channels::set_angle(SRV_Channel::k_tiltMotorRight, SERVO_OUTPUT_RANGE); + + // left servo defaults to servo output 4 + SRV_Channels::set_aux_channel_default(SRV_Channel::k_tiltMotorLeft, CH_4); + SRV_Channels::set_angle(SRV_Channel::k_tiltMotorLeft, SERVO_OUTPUT_RANGE); // record successful initialisation if what we setup was the desired frame_class _flags.initialised_ok = (frame_class == MOTOR_FRAME_TAILSITTER); @@ -50,8 +62,7 @@ void AP_MotorsTailsitter::init(motor_frame_class frame_class, motor_frame_type f AP_MotorsTailsitter::AP_MotorsTailsitter(uint16_t loop_rate, uint16_t speed_hz) : AP_MotorsMulticopter(loop_rate, speed_hz) { - SRV_Channels::set_rc_frequency(SRV_Channel::k_throttleLeft, speed_hz); - SRV_Channels::set_rc_frequency(SRV_Channel::k_throttleRight, speed_hz); + set_update_rate(speed_hz); } @@ -61,10 +72,8 @@ void AP_MotorsTailsitter::set_update_rate(uint16_t speed_hz) // record requested speed _speed_hz = speed_hz; - uint32_t mask = - 1U << AP_MOTORS_THROTTLE_LEFT | - 1U << AP_MOTORS_THROTTLE_RIGHT; - rc_set_freq(mask, _speed_hz); + SRV_Channels::set_rc_frequency(SRV_Channel::k_throttleLeft, speed_hz); + SRV_Channels::set_rc_frequency(SRV_Channel::k_throttleRight, speed_hz); } void AP_MotorsTailsitter::output_to_motors() @@ -72,59 +81,56 @@ void AP_MotorsTailsitter::output_to_motors() if (!_flags.initialised_ok) { return; } - float throttle = 0.0f; + float throttle_pwm = 0.0f; switch (_spool_mode) { case SHUT_DOWN: - throttle = get_pwm_output_min(); - rc_write(AP_MOTORS_THROTTLE_LEFT, get_pwm_output_min()); - rc_write(AP_MOTORS_THROTTLE_RIGHT, get_pwm_output_min()); + throttle_pwm = get_pwm_output_min(); + SRV_Channels::set_output_pwm(SRV_Channel::k_throttleLeft, get_pwm_output_min()); + SRV_Channels::set_output_pwm(SRV_Channel::k_throttleRight, get_pwm_output_min()); break; case SPIN_WHEN_ARMED: - throttle = constrain_float(_spin_up_ratio, 0.0f, 1.0f) * _spin_min; - rc_write(AP_MOTORS_THROTTLE_LEFT, calc_spin_up_to_pwm()); - rc_write(AP_MOTORS_THROTTLE_RIGHT, calc_spin_up_to_pwm()); + throttle_pwm = calc_spin_up_to_pwm(); + SRV_Channels::set_output_pwm(SRV_Channel::k_throttleLeft, calc_spin_up_to_pwm()); + SRV_Channels::set_output_pwm(SRV_Channel::k_throttleRight, calc_spin_up_to_pwm()); break; case SPOOL_UP: case THROTTLE_UNLIMITED: case SPOOL_DOWN: - throttle = calc_thrust_to_pwm(_throttle); - rc_write(AP_MOTORS_THROTTLE_LEFT, calc_thrust_to_pwm(_thrust_left)); - rc_write(AP_MOTORS_THROTTLE_RIGHT, calc_thrust_to_pwm(_thrust_right)); + throttle_pwm = calc_thrust_to_pwm(_throttle); + SRV_Channels::set_output_pwm(SRV_Channel::k_throttleLeft, calc_thrust_to_pwm(_thrust_left)); + SRV_Channels::set_output_pwm(SRV_Channel::k_throttleRight, calc_thrust_to_pwm(_thrust_right)); break; } - // Always output to tilts - rc_write_angle(AP_MOTORS_TILT_LEFT, _tilt_left*SERVO_OUTPUT_RANGE); - rc_write_angle(AP_MOTORS_TILT_RIGHT, _tilt_right*SERVO_OUTPUT_RANGE); + // Always output to tilt servos + SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft, _tilt_left*SERVO_OUTPUT_RANGE); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, _tilt_right*SERVO_OUTPUT_RANGE); // plane outputs for Qmodes are setup here, and written to the HAL by the plane servos loop SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, -_yaw_in*SERVO_OUTPUT_RANGE); SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, _pitch_in*SERVO_OUTPUT_RANGE); SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, _roll_in*SERVO_OUTPUT_RANGE); - SRV_Channels::set_output_pwm(SRV_Channel::k_throttle, throttle); - -#if APM_BUILD_TYPE(APM_BUILD_ArduCopter) - SRV_Channels::calc_pwm(); - SRV_Channels::output_ch_all(); -#endif + SRV_Channels::set_output_pwm(SRV_Channel::k_throttle, throttle_pwm); } -// get_motor_mask - returns a bitmask of which outputs are being used for motors or servos (1 means being used) +// get_motor_mask - returns a bitmask of which outputs are being used for motors (1 means being used) // this can be used to ensure other pwm outputs (i.e. for servos) do not conflict uint16_t AP_MotorsTailsitter::get_motor_mask() { - uint32_t motor_mask = - 1U << AP_MOTORS_THROTTLE_LEFT | - 1U << AP_MOTORS_THROTTLE_RIGHT | - 1U << AP_MOTORS_TILT_LEFT | - 1U << AP_MOTORS_TILT_RIGHT; - uint16_t mask = rc_map_mask(motor_mask); + uint32_t motor_mask = 0; + uint8_t chan; + if (SRV_Channels::find_channel(SRV_Channel::k_throttleLeft, chan)) { + motor_mask |= 1U << chan; + } + if (SRV_Channels::find_channel(SRV_Channel::k_throttleRight, chan)) { + motor_mask |= 1U << chan; + } // add parent's mask - mask |= AP_MotorsMulticopter::get_motor_mask(); + motor_mask |= AP_MotorsMulticopter::get_motor_mask(); - return mask; + return motor_mask; } // calculate outputs to the motors @@ -189,20 +195,20 @@ void AP_MotorsTailsitter::output_test_seq(uint8_t motor_seq, int16_t pwm) // output to motors and servos switch (motor_seq) { case 1: - // throttle left - rc_write(AP_MOTORS_THROTTLE_LEFT, pwm); + // right throttle + SRV_Channels::set_output_pwm(SRV_Channel::k_throttleRight, pwm); break; case 2: - // throttle right - rc_write(AP_MOTORS_THROTTLE_RIGHT, pwm); + // right tilt servo + SRV_Channels::set_output_pwm(SRV_Channel::k_tiltMotorRight, pwm); break; case 3: - // tilt left - rc_write(AP_MOTORS_TILT_LEFT, pwm); + // left throttle + SRV_Channels::set_output_pwm(SRV_Channel::k_throttleLeft, pwm); break; case 4: - // tilt right - rc_write(AP_MOTORS_TILT_RIGHT, pwm); + // left tilt servo + SRV_Channels::set_output_pwm(SRV_Channel::k_tiltMotorLeft, pwm); break; default: // do nothing