diff --git a/libraries/AP_Motors/AP_MotorsTailsitter.cpp b/libraries/AP_Motors/AP_MotorsTailsitter.cpp index 8e213d6fce..3f22e9f8d3 100644 --- a/libraries/AP_Motors/AP_MotorsTailsitter.cpp +++ b/libraries/AP_Motors/AP_MotorsTailsitter.cpp @@ -14,7 +14,7 @@ */ /* - * AP_MotorsTailsitter.cpp - ArduCopter motors library for tailsitters + * AP_MotorsTailsitter.cpp - ArduCopter motors library for tailsitters and bicopters * */ @@ -31,6 +31,16 @@ extern const AP_HAL::HAL& hal; // 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); + + // 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; + // record successful initialisation if what we setup was the desired frame_class _flags.initialised_ok = (frame_class == MOTOR_FRAME_TAILSITTER); } @@ -44,56 +54,55 @@ AP_MotorsTailsitter::AP_MotorsTailsitter(uint16_t loop_rate, uint16_t speed_hz) SRV_Channels::set_rc_frequency(SRV_Channel::k_throttleRight, speed_hz); } + +// set update rate to motors - a value in hertz +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); +} + void AP_MotorsTailsitter::output_to_motors() { if (!_flags.initialised_ok) { return; } - float throttle = _throttle; - float throttle_left = 0; - float throttle_right = 0; - + float throttle = 0.0f; + switch (_spool_mode) { case SHUT_DOWN: - throttle = 0; - // set limits flags - limit.roll_pitch = true; - limit.yaw = true; - limit.throttle_lower = true; - limit.throttle_upper = true; + 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()); break; case SPIN_WHEN_ARMED: - // sends output to motors when armed but not flying throttle = constrain_float(_spin_up_ratio, 0.0f, 1.0f) * _spin_min; - // set limits flags - limit.roll_pitch = true; - limit.yaw = true; - limit.throttle_lower = true; - limit.throttle_upper = true; + rc_write(AP_MOTORS_THROTTLE_LEFT, calc_spin_up_to_pwm()); + rc_write(AP_MOTORS_THROTTLE_RIGHT, calc_spin_up_to_pwm()); break; case SPOOL_UP: case THROTTLE_UNLIMITED: - case SPOOL_DOWN: { - throttle = _spin_min + throttle * (1 - _spin_min); - throttle_left = constrain_float(throttle + _rudder*0.5, _spin_min, 1); - throttle_right = constrain_float(throttle - _rudder*0.5, _spin_min, 1); - // initialize limits flags - limit.roll_pitch = false; - limit.yaw = false; - limit.throttle_lower = false; - limit.throttle_upper = false; + 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)); break; - } } - // outputs are setup here, and written to the HAL by the plane servos loop - SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, _aileron*SERVO_OUTPUT_RANGE); - SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, _elevator*SERVO_OUTPUT_RANGE); - SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, _rudder*SERVO_OUTPUT_RANGE); - SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, throttle*THROTTLE_RANGE); - // also support differential roll with twin motors - SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft, throttle_left*THROTTLE_RANGE); - SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, throttle_right*THROTTLE_RANGE); + // 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); + + // 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(); @@ -101,24 +110,103 @@ void AP_MotorsTailsitter::output_to_motors() #endif } +// 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_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); + + // add parent's mask + mask |= AP_MotorsMulticopter::get_motor_mask(); + + return mask; +} + // calculate outputs to the motors void AP_MotorsTailsitter::output_armed_stabilizing() { - _aileron = -_yaw_in; - _elevator = _pitch_in; - _rudder = _roll_in; - _throttle = get_throttle(); + float roll_thrust; // roll thrust input value, +/- 1.0 + float pitch_thrust; // pitch thrust input value, +/- 1.0 + float yaw_thrust; // yaw thrust input value, +/- 1.0 + float throttle_thrust; // throttle thrust input value, 0.0 - 1.0 + float thrust_max; // highest motor value + float thr_adj = 0.0f; // the difference between the pilot's desired throttle and throttle_thrust_best_rpy + + // apply voltage and air pressure compensation + const float compensation_gain = get_compensation_gain(); + roll_thrust = _roll_in * compensation_gain; + pitch_thrust = _pitch_in * compensation_gain; + yaw_thrust = _yaw_in * compensation_gain; + throttle_thrust = get_throttle() * compensation_gain; + // sanity check throttle is above zero and below current limited throttle - if (_throttle <= 0.0f) { - _throttle = 0.0f; + if (throttle_thrust <= 0.0f) { + throttle_thrust = 0.0f; limit.throttle_lower = true; } - if (_throttle >= _throttle_thrust_max) { - _throttle = _throttle_thrust_max; + if (throttle_thrust >= _throttle_thrust_max) { + throttle_thrust = _throttle_thrust_max; limit.throttle_upper = true; } - _throttle = constrain_float(_throttle, 0.1, 1); + // caculate left and right throttle outputs + _thrust_left = throttle_thrust + roll_thrust*0.5; + _thrust_right = throttle_thrust - roll_thrust*0.5; + + // if max thrust is more than one reduce average throttle + thrust_max = MAX(_thrust_right,_thrust_left); + if (thrust_max > 1.0f) { + thr_adj = 1.0f - thrust_max; + limit.throttle_upper = true; + limit.roll_pitch = true; + } + + // Add ajustment to reduce average throttle + _thrust_left = constrain_float(_thrust_left + thr_adj, 0.0f, 1.0f); + _thrust_right = constrain_float(_thrust_right + thr_adj, 0.0f, 1.0f); + _throttle = throttle_thrust + thr_adj; + + // thrust vectoring + _tilt_left = pitch_thrust - yaw_thrust; + _tilt_right = pitch_thrust + yaw_thrust; } +// output_test_seq - spin a motor at the pwm value specified +// motor_seq is the motor's sequence number from 1 to the number of motors on the frame +// pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000 +void AP_MotorsTailsitter::output_test_seq(uint8_t motor_seq, int16_t pwm) +{ + // exit immediately if not armed + if (!armed()) { + return; + } + + // output to motors and servos + switch (motor_seq) { + case 1: + // throttle left + rc_write(AP_MOTORS_THROTTLE_LEFT, pwm); + break; + case 2: + // throttle right + rc_write(AP_MOTORS_THROTTLE_RIGHT, pwm); + break; + case 3: + // tilt left + rc_write(AP_MOTORS_TILT_LEFT, pwm); + break; + case 4: + // tilt right + rc_write(AP_MOTORS_TILT_RIGHT, pwm); + break; + default: + // do nothing + break; + } +} diff --git a/libraries/AP_Motors/AP_MotorsTailsitter.h b/libraries/AP_Motors/AP_MotorsTailsitter.h index 1220a6b967..22cb718a88 100644 --- a/libraries/AP_Motors/AP_MotorsTailsitter.h +++ b/libraries/AP_Motors/AP_MotorsTailsitter.h @@ -1,5 +1,5 @@ /// @file AP_MotorsTailsitter.h -/// @brief Motor control class for tailsitters +/// @brief Motor control class for tailsitters and bicopters #pragma once #include @@ -19,23 +19,27 @@ public: // set frame class (i.e. quad, hexa, heli) and type (i.e. x, plus) void set_frame_class_and_type(motor_frame_class frame_class, motor_frame_type frame_type) override {} - void set_update_rate( uint16_t speed_hz ) override {} - virtual void output_test_seq(uint8_t motor_seq, int16_t pwm) override {} + // set update rate to motors - a value in hertz + void set_update_rate( uint16_t speed_hz ) override; + + virtual void output_test_seq(uint8_t motor_seq, int16_t pwm) override; // output_to_motors - sends output to named servos void output_to_motors() override; - // return 0 motor mask - uint16_t get_motor_mask() override { return 0; } + // 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 get_motor_mask() override; protected: // calculate motor outputs void output_armed_stabilizing() override; // calculated outputs - float _aileron; // -1..1 - float _elevator; // -1..1 - float _rudder; // -1..1 float _throttle; // 0..1 + float _tilt_left; // -1..1 + float _tilt_right; // -1..1 + float _thrust_left; // 0..1 + float _thrust_right; // 0..1 };