Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : 
      16             : /*
      17             :  *       AP_MotorsTailsitter.cpp - ArduCopter motors library for tailsitters and bicopters
      18             :  *
      19             :  */
      20             : 
      21             : #include <AP_HAL/AP_HAL.h>
      22             : #include <AP_Math/AP_Math.h>
      23             : #include "AP_MotorsTailsitter.h"
      24             : #include <GCS_MAVLink/GCS.h>
      25             : #include <SRV_Channel/SRV_Channel.h>
      26             : 
      27             : extern const AP_HAL::HAL& hal;
      28             : 
      29             : #define SERVO_OUTPUT_RANGE  4500
      30             : 
      31             : // init
      32           1 : void AP_MotorsTailsitter::init(motor_frame_class frame_class, motor_frame_type frame_type)
      33             : {
      34             :     // setup default motor and servo mappings
      35           1 :     _has_diff_thrust = SRV_Channels::function_assigned(SRV_Channel::k_throttleRight) || SRV_Channels::function_assigned(SRV_Channel::k_throttleLeft);
      36             : 
      37             :     // right throttle defaults to servo output 1
      38           1 :     SRV_Channels::set_aux_channel_default(SRV_Channel::k_throttleRight, CH_1);
      39             : 
      40             :     // left throttle defaults to servo output 2
      41           1 :     SRV_Channels::set_aux_channel_default(SRV_Channel::k_throttleLeft, CH_2);
      42             : 
      43             :     // right servo defaults to servo output 3
      44           1 :     SRV_Channels::set_aux_channel_default(SRV_Channel::k_tiltMotorRight, CH_3);
      45           1 :     SRV_Channels::set_angle(SRV_Channel::k_tiltMotorRight, SERVO_OUTPUT_RANGE);
      46             : 
      47             :     // left servo defaults to servo output 4
      48           1 :     SRV_Channels::set_aux_channel_default(SRV_Channel::k_tiltMotorLeft, CH_4);
      49           1 :     SRV_Channels::set_angle(SRV_Channel::k_tiltMotorLeft, SERVO_OUTPUT_RANGE);
      50             : 
      51           1 :     _mav_type = MAV_TYPE_VTOL_DUOROTOR;
      52             : 
      53             :     // record successful initialisation if what we setup was the desired frame_class
      54           1 :     set_initialised_ok(frame_class == MOTOR_FRAME_TAILSITTER);
      55           1 : }
      56             : 
      57             : 
      58             : /// Constructor
      59           1 : AP_MotorsTailsitter::AP_MotorsTailsitter(uint16_t speed_hz) :
      60           1 :     AP_MotorsMulticopter(speed_hz)
      61             : {
      62           1 :     set_update_rate(speed_hz);
      63           1 : }
      64             : 
      65             : 
      66             : // set update rate to motors - a value in hertz
      67           2 : void AP_MotorsTailsitter::set_update_rate(uint16_t speed_hz)
      68             : {
      69             :     // record requested speed
      70           2 :     _speed_hz = speed_hz;
      71             : 
      72           2 :     SRV_Channels::set_rc_frequency(SRV_Channel::k_throttleLeft, speed_hz);
      73           2 :     SRV_Channels::set_rc_frequency(SRV_Channel::k_throttleRight, speed_hz);
      74           2 : }
      75             : 
      76       62688 : void AP_MotorsTailsitter::output_to_motors()
      77             : {
      78       62688 :     if (!initialised_ok()) {
      79           0 :         return;
      80             :     }
      81             : 
      82       62688 :     switch (_spool_state) {
      83       62688 :         case SpoolState::SHUT_DOWN:
      84       62688 :             _actuator[0] = 0.0f;
      85       62688 :             _actuator[1] = 0.0f;
      86       62688 :             _actuator[2] = 0.0f;
      87       62688 :             _external_min_throttle = 0.0;
      88       62688 :             break;
      89           0 :         case SpoolState::GROUND_IDLE:
      90           0 :             set_actuator_with_slew(_actuator[0], actuator_spin_up_to_ground_idle());
      91           0 :             set_actuator_with_slew(_actuator[1], actuator_spin_up_to_ground_idle());
      92           0 :             set_actuator_with_slew(_actuator[2], actuator_spin_up_to_ground_idle());
      93           0 :             _external_min_throttle = 0.0;
      94           0 :             break;
      95           0 :         case SpoolState::SPOOLING_UP:
      96             :         case SpoolState::THROTTLE_UNLIMITED:
      97             :         case SpoolState::SPOOLING_DOWN:
      98           0 :             set_actuator_with_slew(_actuator[0], thr_lin.thrust_to_actuator(_thrust_left));
      99           0 :             set_actuator_with_slew(_actuator[1], thr_lin.thrust_to_actuator(_thrust_right));
     100           0 :             set_actuator_with_slew(_actuator[2], thr_lin.thrust_to_actuator(_throttle));
     101           0 :             break;
     102             :     }
     103             : 
     104       62688 :     SRV_Channels::set_output_pwm(SRV_Channel::k_throttleLeft, output_to_pwm(_actuator[0]));
     105       62688 :     SRV_Channels::set_output_pwm(SRV_Channel::k_throttleRight, output_to_pwm(_actuator[1]));
     106             : 
     107             :     // use set scaled to allow a different PWM range on plane forward throttle, throttle range is 0 to 100
     108       62688 :     SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, _actuator[2]*100);
     109             : 
     110       62688 :     SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, _tilt_left*SERVO_OUTPUT_RANGE);
     111       62688 :     SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, _tilt_right*SERVO_OUTPUT_RANGE);
     112             : 
     113             : }
     114             : 
     115             : // get_motor_mask - returns a bitmask of which outputs are being used for motors (1 means being used)
     116             : //  this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
     117           3 : uint32_t AP_MotorsTailsitter::get_motor_mask()
     118             : {
     119           3 :     uint32_t motor_mask = 0;
     120             :     uint8_t chan;
     121           3 :     if (SRV_Channels::find_channel(SRV_Channel::k_throttleLeft, chan)) {
     122           0 :         motor_mask |= 1U << chan;
     123             :     }
     124           3 :     if (SRV_Channels::find_channel(SRV_Channel::k_throttleRight, chan)) {
     125           0 :         motor_mask |= 1U << chan;
     126             :     }
     127             : 
     128             :     // add parent's mask
     129           3 :     motor_mask |= AP_MotorsMulticopter::get_motor_mask();
     130             : 
     131           3 :     return motor_mask;
     132             : }
     133             : 
     134             : // calculate outputs to the motors
     135       62688 : void AP_MotorsTailsitter::output_armed_stabilizing()
     136             : {
     137             :     float   roll_thrust;                // roll thrust input value, +/- 1.0
     138             :     float   pitch_thrust;               // pitch thrust input value, +/- 1.0
     139             :     float   yaw_thrust;                 // yaw thrust input value, +/- 1.0
     140             :     float   throttle_thrust;            // throttle thrust input value, 0.0 - 1.0
     141             :     float   thrust_max;                 // highest motor value
     142             :     float   thrust_min;                 // lowest motor value
     143       62688 :     float   thr_adj = 0.0f;             // the difference between the pilot's desired throttle and throttle_thrust_best_rpy
     144             : 
     145             :     // apply voltage and air pressure compensation
     146       62688 :     const float compensation_gain = thr_lin.get_compensation_gain();
     147       62688 :     roll_thrust = (_roll_in + _roll_in_ff) * compensation_gain;
     148       62688 :     pitch_thrust = _pitch_in + _pitch_in_ff;
     149       62688 :     yaw_thrust = _yaw_in + _yaw_in_ff;
     150       62688 :     throttle_thrust = get_throttle() * compensation_gain;
     151       62688 :     const float max_boost_throttle = _throttle_avg_max * compensation_gain;
     152             : 
     153             :     // never boost above max, derived from throttle mix params
     154       62688 :     const float min_throttle_out = MIN(_external_min_throttle, max_boost_throttle);
     155       62688 :     const float max_throttle_out = _throttle_thrust_max * compensation_gain;
     156             : 
     157             :     // sanity check throttle is above min and below current limited throttle
     158       62688 :     if (throttle_thrust <= min_throttle_out) {
     159       62688 :         throttle_thrust = min_throttle_out;
     160       62688 :         limit.throttle_lower = true;
     161             :     }
     162       62688 :     if (throttle_thrust >= max_throttle_out) {
     163       62688 :         throttle_thrust = max_throttle_out;
     164       62688 :         limit.throttle_upper = true;
     165             :     }
     166             : 
     167       62688 :     if (roll_thrust >= 1.0) {
     168             :         // cannot split motor outputs by more than 1
     169           0 :         roll_thrust = 1;
     170           0 :         limit.roll = true;
     171             :     }
     172             : 
     173             :     // calculate left and right throttle outputs
     174       62688 :     _thrust_left  = throttle_thrust + roll_thrust * 0.5f;
     175       62688 :     _thrust_right = throttle_thrust - roll_thrust * 0.5f;
     176             : 
     177       62688 :     thrust_max = MAX(_thrust_right,_thrust_left);
     178       62688 :     thrust_min = MIN(_thrust_right,_thrust_left);
     179       62688 :     if (thrust_max > 1.0f) {
     180             :         // if max thrust is more than one reduce average throttle
     181           0 :         thr_adj = 1.0f - thrust_max;
     182           0 :         limit.throttle_upper = true;
     183       62688 :     } else if (thrust_min < 0.0) {
     184             :         // if min thrust is less than 0 increase average throttle
     185             :         // but never above max boost
     186           0 :         thr_adj = -thrust_min;
     187           0 :         if ((throttle_thrust + thr_adj) > max_boost_throttle) {
     188           0 :             thr_adj = MAX(max_boost_throttle - throttle_thrust, 0.0);
     189             :             // in this case we throw away some roll output, it will be uneven
     190             :             // constraining the lower motor more than the upper
     191             :             // this unbalances torque, but motor torque should have significantly less control power than tilts / control surfaces
     192             :             // so its worth keeping the higher roll control power at a minor cost to yaw
     193           0 :             limit.roll = true;
     194             :         }
     195           0 :         limit.throttle_lower = true;
     196             :     }
     197             : 
     198             :     // Add adjustment to reduce average throttle
     199       62688 :     _thrust_left  = constrain_float(_thrust_left  + thr_adj, 0.0f, 1.0f);
     200       62688 :     _thrust_right = constrain_float(_thrust_right + thr_adj, 0.0f, 1.0f);
     201             : 
     202       62688 :     _throttle = throttle_thrust;
     203             : 
     204             :     // compensation_gain can never be zero
     205             :     // ensure accurate representation of average throttle output, this value is used for notch tracking and control surface scaling
     206       62688 :     if (_has_diff_thrust) {
     207           0 :         _throttle_out = (throttle_thrust + thr_adj) / compensation_gain;
     208             :     } else {
     209       62688 :         _throttle_out = throttle_thrust / compensation_gain;
     210             :     }
     211             : 
     212             :     // thrust vectoring
     213       62688 :     _tilt_left  = pitch_thrust - yaw_thrust;
     214       62688 :     _tilt_right = pitch_thrust + yaw_thrust;
     215       62688 : }
     216             : 
     217             : // output_test_seq - spin a motor at the pwm value specified
     218             : //  motor_seq is the motor's sequence number from 1 to the number of motors on the frame
     219             : //  pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
     220           0 : void AP_MotorsTailsitter::_output_test_seq(uint8_t motor_seq, int16_t pwm)
     221             : {
     222             :     // output to motors and servos
     223           0 :     switch (motor_seq) {
     224           0 :         case 1:
     225             :             // right throttle
     226           0 :             SRV_Channels::set_output_pwm(SRV_Channel::k_throttleRight, pwm);
     227           0 :             break;
     228           0 :         case 2:
     229             :             // right tilt servo
     230           0 :             SRV_Channels::set_output_pwm(SRV_Channel::k_tiltMotorRight, pwm);
     231           0 :             break;
     232           0 :         case 3:
     233             :             // left throttle
     234           0 :             SRV_Channels::set_output_pwm(SRV_Channel::k_throttleLeft, pwm);
     235           0 :             break;
     236           0 :         case 4:
     237             :             // left tilt servo
     238           0 :             SRV_Channels::set_output_pwm(SRV_Channel::k_tiltMotorLeft, pwm);
     239           0 :             break;
     240           0 :         default:
     241             :             // do nothing
     242           0 :             break;
     243             :     }
     244           0 : }