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             : #include <stdlib.h>
      17             : #include <AP_HAL/AP_HAL.h>
      18             : #include <GCS_MAVLink/GCS.h>
      19             : #include "AP_MotorsHeli_Quad.h"
      20             : 
      21             : extern const AP_HAL::HAL& hal;
      22             : 
      23             : const AP_Param::GroupInfo AP_MotorsHeli_Quad::var_info[] = {
      24             :     AP_NESTEDGROUPINFO(AP_MotorsHeli, 0),
      25             : 
      26             :     // Indices 1-3 were used by RSC_PWM_MIN, RSC_PWM_MAX and RSC_PWM_REV and should not be used
      27             : 
      28             :     AP_GROUPEND
      29             : };
      30             : 
      31             : #define QUAD_SERVO_MAX_ANGLE 4500
      32             : 
      33             : // set update rate to motors - a value in hertz
      34           6 : void AP_MotorsHeli_Quad::set_update_rate( uint16_t speed_hz )
      35             : {
      36             :     // record requested speed
      37           6 :     _speed_hz = speed_hz;
      38             : 
      39             :     // setup fast channels
      40           6 :     uint32_t mask = 0;
      41          30 :     for (uint8_t i=0; i<AP_MOTORS_HELI_QUAD_NUM_MOTORS; i++) {
      42          24 :         mask |= 1U << (AP_MOTORS_MOT_1+i);
      43             :     }
      44             : 
      45           6 :     rc_set_freq(mask, _speed_hz);
      46           6 : }
      47             : 
      48             : // init_outputs
      49       52615 : void AP_MotorsHeli_Quad::init_outputs()
      50             : {
      51       52615 :     if (initialised_ok()) {
      52       52612 :         return;
      53             :     }
      54             : 
      55          15 :     for (uint8_t i=0; i<AP_MOTORS_HELI_QUAD_NUM_MOTORS; i++) {
      56          12 :         add_motor_num(CH_1+i);
      57          12 :         SRV_Channels::set_angle(SRV_Channels::get_motor_function(i), QUAD_SERVO_MAX_ANGLE);
      58             :     }
      59             : 
      60             :     // set rotor servo range
      61           3 :     _main_rotor.initialize();
      62             : 
      63           3 :     set_initialised_ok(_frame_class == MOTOR_FRAME_HELI_QUAD);
      64             : }
      65             : 
      66             : // calculate_scalars
      67       52615 : void AP_MotorsHeli_Quad::calculate_scalars()
      68             : {
      69             :     // range check collective min, max and mid
      70       52615 :     if( _collective_min >= _collective_max ) {
      71           0 :         _collective_min.set(AP_MOTORS_HELI_COLLECTIVE_MIN);
      72           0 :         _collective_max.set(AP_MOTORS_HELI_COLLECTIVE_MAX);
      73             :     }
      74             : 
      75       52615 :     _collective_zero_thrust_deg.set(constrain_float(_collective_zero_thrust_deg, _collective_min_deg, _collective_max_deg));
      76             : 
      77       52615 :     _collective_land_min_deg.set(constrain_float(_collective_land_min_deg, _collective_min_deg, _collective_max_deg));
      78             : 
      79       52615 :     if (!is_equal((float)_collective_max_deg, (float)_collective_min_deg)) {
      80             :         // calculate collective zero thrust point as a number from 0 to 1
      81       52615 :         _collective_zero_thrust_pct = (_collective_zero_thrust_deg-_collective_min_deg)/(_collective_max_deg-_collective_min_deg);
      82             : 
      83             :         // calculate collective land min point as a number from 0 to 1
      84       52615 :         _collective_land_min_pct = (_collective_land_min_deg-_collective_min_deg)/(_collective_max_deg-_collective_min_deg);
      85             :     } else {
      86           0 :         _collective_zero_thrust_pct = 0.0f;
      87           0 :         _collective_land_min_pct = 0.0f;
      88             :     }
      89             : 
      90             :     // calculate factors based on swash type and servo position
      91       52615 :     calculate_roll_pitch_collective_factors();
      92             : 
      93             :     // configure main rotor and update scalars
      94       52615 :     _main_rotor.configure();
      95             : 
      96             :     // calculate armed scalars for rotors
      97       52615 :     calculate_armed_scalars();
      98       52615 : }
      99             : 
     100             : // calculate_swash_factors - calculate factors based on swash type and servo position
     101       52615 : void AP_MotorsHeli_Quad::calculate_roll_pitch_collective_factors()
     102             : {
     103             :     // assume X quad layout, with motors at 45, 135, 225 and 315 degrees
     104             :     // order FrontRight, RearLeft, FrontLeft, RearRight
     105       52615 :     const float angles[AP_MOTORS_HELI_QUAD_NUM_MOTORS] = { 45, 225, 315, 135 };
     106       52615 :     const bool x_clockwise[AP_MOTORS_HELI_QUAD_NUM_MOTORS] = { false, false, true, true };
     107       52615 :     const float cos45 = cosf(radians(45));
     108             : 
     109      263075 :     for (uint8_t i=0; i<AP_MOTORS_HELI_QUAD_NUM_MOTORS; i++) {
     110      210460 :         bool clockwise = x_clockwise[i];
     111      210460 :         if (_frame_type == MOTOR_FRAME_TYPE_H) {
     112             :             // reverse yaw for H frame
     113           0 :             clockwise = !clockwise;
     114             :         }
     115      210460 :         _rollFactor[CH_1+i]       = -0.25*sinf(radians(angles[i]))/cos45;
     116      210460 :         _pitchFactor[CH_1+i]      =  0.25*cosf(radians(angles[i]))/cos45;
     117      210460 :         _yawFactor[CH_1+i]        = clockwise?-0.25:0.25;
     118      210460 :         _collectiveFactor[CH_1+i] = 1;
     119             :     }
     120       52615 : }
     121             : 
     122             : // update_motor_controls - sends commands to motor controllers
     123      181722 : void  AP_MotorsHeli_Quad::update_motor_control(AP_MotorsHeli_RSC::DesiredRSCSpoolState state)
     124             : {
     125             :     // Send state update to motors
     126      181722 :     _main_rotor.update(_dt_s);
     127             : 
     128      181722 :     if (state == AP_MotorsHeli_RSC::DesiredRSCSpoolState::SHUT_DOWN) {
     129             :         // set engine run enable aux output to not run position to kill engine when disarmed
     130       52618 :         SRV_Channels::set_output_limit(SRV_Channel::k_engine_run_enable, SRV_Channel::Limit::MIN);
     131             :     } else {
     132             :         // else if armed, set engine run enable output to run position
     133      129104 :         SRV_Channels::set_output_limit(SRV_Channel::k_engine_run_enable, SRV_Channel::Limit::MAX);
     134             :     }
     135             : 
     136      181722 : }
     137             : 
     138             : // update_spool_state - updates the spool state based on the desired state
     139      181722 : AP_Motors::SpoolState AP_MotorsHeli_Quad::update_spool_state(AP_MotorsHeli_RSC::DesiredRSCSpoolState state)
     140             : {
     141             : 
     142      181722 :     SpoolState main_rotor_state = _main_rotor.update_spool_state(state, _dt_s);
     143             : 
     144             :     // Check if main rotor is run-up complete.  Tail rotor run-up is not included in check because currently
     145             :     // the tail rotor in DDVP uses the same ramp and runup time as the main rotor.  This may need changed if 
     146             :     // the RSC is used for DDFP tail rotors.
     147      181722 :     set_rotor_runup_complete(main_rotor_state == AP_MotorsHeli_RSC::RSCSpoolState::THROTTLE_UNLIMITED);
     148             : 
     149      181722 :     return main_rotor_state;
     150             : 
     151             : }
     152             : 
     153             : //
     154             : // move_actuators - moves swash plate to attitude of parameters passed in
     155             : //                - expected ranges:
     156             : //                       roll : -1 ~ +1
     157             : //                       pitch: -1 ~ +1
     158             : //                       collective: 0 ~ 1
     159             : //                       yaw:   -1 ~ +1
     160             : //
     161      181722 : void AP_MotorsHeli_Quad::move_actuators(float roll_out, float pitch_out, float collective_in, float yaw_out)
     162             : {
     163             :     // constrain collective input
     164      181722 :     float collective_out = collective_in;
     165      181722 :     if (collective_out <= 0.0f) {
     166       78082 :         collective_out = 0.0f;
     167       78082 :         limit.throttle_lower = true;
     168             :     }
     169      181722 :     if (collective_out >= 1.0f) {
     170         222 :         collective_out = 1.0f;
     171         222 :         limit.throttle_upper = true;
     172             :     }
     173             : 
     174             :     // ensure not below landed/landing collective
     175      181722 :     if (_heliflags.landing_collective && collective_out < _collective_land_min_pct) {
     176       64163 :         collective_out = _collective_land_min_pct;
     177       64163 :         limit.throttle_lower = true;
     178             :     }
     179             : 
     180             :     // updates below land min collective flag
     181      181722 :     _heliflags.below_land_min_coll = !is_positive(collective_out - _collective_land_min_pct);
     182             : 
     183             :     // updates takeoff collective flag based on 50% hover collective
     184      181722 :     update_takeoff_collective_flag(collective_out);
     185             : 
     186      181722 :     float collective_range = (_collective_max - _collective_min) * 0.001f;
     187             : 
     188             :     // feed power estimate into main rotor controller
     189      181722 :     _main_rotor.set_collective(fabsf(collective_out));
     190             : 
     191             :     // rescale collective for overhead calc
     192      181722 :     collective_out -= _collective_zero_thrust_pct;
     193             : 
     194             :     // reserve some collective for attitude control
     195      181722 :     collective_out *= collective_range;
     196             : 
     197      908610 :     for (uint8_t i=0; i<AP_MOTORS_HELI_QUAD_NUM_MOTORS; i++) {
     198      726888 :         _out[i] =
     199      726888 :             _rollFactor[CH_1+i] * roll_out +
     200      726888 :             _pitchFactor[CH_1+i] * pitch_out +
     201      726888 :             _collectiveFactor[CH_1+i] * collective_out;
     202             :     }
     203             : 
     204             :     // see if we need to scale down yaw_out
     205      908610 :     for (uint8_t i=0; i<AP_MOTORS_HELI_QUAD_NUM_MOTORS; i++) {
     206      726888 :         float y = _yawFactor[CH_1+i] * yaw_out;
     207      726888 :         if (_out[i] < 0.0f) {
     208             :             // the slope of the yaw effect changes at zero collective
     209      403824 :             y = -y;
     210             :         }
     211      726888 :         if (_out[i] * (_out[i] + y) < 0.0f) {
     212             :             // applying this yaw demand would change the sign of the
     213             :             // collective, which means the yaw would not be applied
     214             :             // evenly. We scale down the overall yaw demand to prevent
     215             :             // it crossing over zero
     216          12 :             float s = -(_out[i] / y);
     217          12 :             yaw_out *= s;
     218             :         }
     219             :     }
     220             : 
     221             :     // now apply the yaw correction
     222      908610 :     for (uint8_t i=0; i<AP_MOTORS_HELI_QUAD_NUM_MOTORS; i++) {
     223      726888 :         float y = _yawFactor[CH_1+i] * yaw_out;
     224      726888 :         if (_out[i] < 0.0f) {
     225             :             // the slope of the yaw effect changes at zero collective
     226      403824 :             y = -y;
     227             :         }
     228      726888 :         _out[i] += y;
     229             :     }
     230             : 
     231      908610 :     for (uint8_t i=0; i<AP_MOTORS_HELI_QUAD_NUM_MOTORS; i++) {
     232             :         // scale output to 0 to 1
     233      726888 :         _out[i] += _collective_zero_thrust_pct;
     234             :         // scale output to -1 to 1 for servo output
     235      726888 :         _out[i] = _out[i] * 2.0f - 1.0f;
     236             :     }
     237      181722 : }
     238             : 
     239      181722 : void AP_MotorsHeli_Quad::output_to_motors()
     240             : {
     241      181722 :     if (!initialised_ok()) {
     242           0 :         return;
     243             :     }
     244             : 
     245             :     // move the servos
     246      908610 :     for (uint8_t i=0; i<AP_MOTORS_HELI_QUAD_NUM_MOTORS; i++) {
     247      726888 :         rc_write_angle(AP_MOTORS_MOT_1+i, _out[i] * QUAD_SERVO_MAX_ANGLE);
     248             :     }
     249             : 
     250      181722 :     _main_rotor.output_to_servo();
     251             : 
     252             : }
     253             : 
     254             : // servo_test - move servos through full range of movement
     255           0 : void AP_MotorsHeli_Quad::servo_test()
     256             : {
     257             :     // not implemented
     258           0 : }
     259             : 
     260             : #if HAL_LOGGING_ENABLED
     261             : // heli motors logging - called at 10 Hz
     262        4541 : void AP_MotorsHeli_Quad::Log_Write(void)
     263             : {
     264        4541 :     _main_rotor.write_log();
     265        4541 : }
     266             : #endif