Line data    Source code

       1             : #include "Copter.h"
       2             : 
       3             : // Traditional helicopter variables and functions
       4             : 
       5             : #if FRAME_CONFIG == HELI_FRAME
       6             : 
       7             : #ifndef HELI_DYNAMIC_FLIGHT_SPEED_MIN
       8             : #define HELI_DYNAMIC_FLIGHT_SPEED_MIN      250     // we are in "dynamic flight" when the speed is over 2.5m/s for 2 seconds
       9             : #endif
      10             : 
      11             : // counter to control dynamic flight profile
      12             : static int8_t heli_dynamic_flight_counter;
      13             : 
      14             : // heli_init - perform any special initialisation required for the tradheli
      15          83 : void Copter::heli_init()
      16             : {
      17             :     // pre-load stab col values as mode is initialized as Stabilize, but stabilize_init() function is not run on start-up.
      18          83 :     input_manager.set_use_stab_col(true);
      19          83 :     input_manager.set_collective_ramp(0);
      20          83 : }
      21             : 
      22             : // heli_check_dynamic_flight - updates the dynamic_flight flag based on our horizontal velocity
      23             : // should be called at 50hz
      24      404223 : void Copter::check_dynamic_flight(void)
      25             : {
      26      653676 :     if ((motors->get_spool_state() != AP_Motors::SpoolState::THROTTLE_UNLIMITED && !motors->in_autorotation()) ||
      27      249453 :         flightmode->is_landing()) {
      28      187891 :         heli_dynamic_flight_counter = 0;
      29      187891 :         heli_flags.dynamic_flight = false;
      30      187891 :         return;
      31             :     }
      32             : 
      33      216332 :     bool moving = false;
      34             : 
      35             :     // with GPS lock use inertial nav to determine if we are moving
      36      216332 :     Vector3f vel_ned_ms;
      37      216332 :     if (AP::ahrs().get_velocity_NED(vel_ned_ms)) {
      38             :         // get horizontal speed
      39      216332 :         moving = (vel_ned_ms.xy().length() * 100.0 >= HELI_DYNAMIC_FLIGHT_SPEED_MIN);
      40             :     } else {
      41             :         // with no GPS lock base it on throttle and forward lean angle
      42           0 :         moving = (motors->get_throttle() > 0.8f || ahrs.get_pitch_deg() < -15);
      43             :     }
      44             : 
      45             : #if AP_RANGEFINDER_ENABLED
      46      216332 :     if (!moving && rangefinder_state.enabled && rangefinder.status_orient(ROTATION_PITCH_270) == RangeFinder::Status::Good) {
      47             :         // when we are more than 2m from the ground with good
      48             :         // rangefinder lock consider it to be dynamic flight
      49           0 :         moving = (rangefinder.distance_orient(ROTATION_PITCH_270) > 2);
      50             :     }
      51             : #endif
      52             : 
      53      216332 :     if (moving) {
      54             :         // if moving for 2 seconds, set the dynamic flight flag
      55       46122 :         if (!heli_flags.dynamic_flight) {
      56        5018 :             heli_dynamic_flight_counter++;
      57        5018 :             if (heli_dynamic_flight_counter >= 100) {
      58          49 :                 heli_flags.dynamic_flight = true;
      59          49 :                 heli_dynamic_flight_counter = 100;
      60             :             }
      61             :         }
      62             :     } else {
      63             :         // if not moving for 2 seconds, clear the dynamic flight flag
      64      170210 :         if (heli_flags.dynamic_flight) {
      65        4866 :             if (heli_dynamic_flight_counter > 0) {
      66        4819 :                 heli_dynamic_flight_counter--;
      67             :             } else {
      68          47 :                 heli_flags.dynamic_flight = false;
      69             :             }
      70             :         }
      71             :     }
      72             : }
      73             : 
      74             : // update_heli_control_dynamics - pushes several important factors up into AP_MotorsHeli.
      75             : // should be run between the rate controller and the servo updates.
      76     3233979 : void Copter::update_heli_control_dynamics(void)
      77             : {
      78             : 
      79     3233979 :     if (!motors->using_leaky_integrator()) {
      80             :         //turn off leaky_I
      81           0 :         attitude_control->use_leaky_i(false);
      82           0 :         if (ap.land_complete || ap.land_complete_maybe) {
      83           0 :             motors->set_land_complete(true);
      84             :         } else {
      85           0 :             motors->set_land_complete(false);
      86             :         }
      87             :     } else {
      88             :         // Use Leaky_I if we are not moving fast
      89     3233979 :         attitude_control->use_leaky_i(!heli_flags.dynamic_flight);
      90     3233979 :         motors->set_land_complete(false);
      91             :     }
      92             : 
      93     3233979 :     if (ap.land_complete || (is_zero(motors->get_desired_rotor_speed()))) {
      94             :         // if we are landed or there is no rotor power demanded, decrement slew scalar
      95     1420065 :         hover_roll_trim_scalar_slew--;
      96             :     } else {
      97             :         // if we are not landed and motor power is demanded, increment slew scalar
      98     1813914 :         hover_roll_trim_scalar_slew++;
      99             :     }
     100     3233979 :     hover_roll_trim_scalar_slew = constrain_int16(hover_roll_trim_scalar_slew, 0, scheduler.get_loop_rate_hz());
     101             : 
     102             :     // set hover roll trim scalar, will ramp from 0 to 1 over 1 second after we think helicopter has taken off
     103     3233979 :     attitude_control->set_hover_roll_trim_scalar((float) hover_roll_trim_scalar_slew/(float) scheduler.get_loop_rate_hz());
     104             : 
     105             :     // set manual collective mode in motors to aid in runup complete determination in RSC
     106             :     // if copter is set to a manual throttle flight mode, then heli is using a manual collective mode
     107     3233979 :     motors->set_using_manual_collective_mode(flightmode->has_manual_throttle());
     108     3233979 : }
     109             : 
     110      404223 : bool Copter::should_use_landing_swash() const
     111             : {
     112      676082 :     if (flightmode->has_manual_throttle() ||
     113      676082 :         flightmode->mode_number() == Mode::Number::DRIFT ||
     114      269469 :         attitude_control->get_inverted_flight()) {
     115             :         // manual modes or modes using inverted flight uses full swash range
     116      135984 :         return false;
     117             :     }
     118      268239 :     if (flightmode->is_landing()) {
     119             :         // landing with non-manual throttle mode always uses limit swash range
     120       41465 :         return true;
     121             :     }
     122      226774 :     if (ap.land_complete) {
     123             :         // when landed in non-manual throttle mode limit swash range
     124       48423 :         return true;
     125             :     }
     126      178351 :     if (!ap.auto_armed) {
     127             :         // when waiting to takeoff in non-manual throttle mode limit swash range
     128           0 :         return true;
     129             :     }
     130      178351 :     if (!heli_flags.dynamic_flight) {
     131             :         // Just in case we are unsure of being in non-manual throttle
     132             :         // mode, limit swash range in low speed and hovering flight.
     133             :         // This will catch any non-manual throttle mode attempting a
     134             :         // landing and driving the collective too low before the land
     135             :         // complete flag is set.
     136      138953 :         return true;
     137             :     }
     138       39398 :     return false;
     139             : }
     140             : 
     141             : // heli_update_landing_swash - sets swash plate flag so higher minimum is used when landed or landing
     142             : // should be called soon after update_land_detector in main code
     143      404223 : void Copter::heli_update_landing_swash()
     144             : {
     145      404223 :     motors->set_collective_for_landing(should_use_landing_swash());
     146      404223 :     update_collective_low_flag(channel_throttle->get_control_in());
     147      404223 : }
     148             : 
     149             : // convert motor interlock switch's position to desired rotor speed expressed as a value from 0 to 1
     150             : // returns zero if motor interlock auxiliary switch hasn't been defined
     151           0 : float Copter::get_pilot_desired_rotor_speed() const
     152             : {
     153           0 :     RC_Channel *rc_ptr = rc().find_channel_for_option(RC_Channel::AUX_FUNC::MOTOR_INTERLOCK);
     154           0 :     if (rc_ptr != nullptr) {
     155           0 :         rc_ptr->set_range(1000);
     156           0 :         return (float)rc_ptr->get_control_in() * 0.001f;
     157             :     }
     158           0 :     return 0.0f;
     159             : }
     160             : 
     161             : // heli_update_rotor_speed_targets - reads pilot input and passes new rotor speed targets to heli motors object
     162      404223 : void Copter::heli_update_rotor_speed_targets()
     163             : {
     164             :     // get rotor control method
     165      404223 :     uint8_t rsc_control_mode = motors->get_rsc_mode();
     166             : 
     167      404223 :     if (rsc_control_mode == ROTOR_CONTROL_MODE_PASSTHROUGH) {
     168             :         // pass through pilot desired rotor speed from the RC
     169           0 :         if (get_pilot_desired_rotor_speed() > 0.01) {
     170           0 :             ap.motor_interlock_switch = true;
     171           0 :             motors->set_desired_rotor_speed(get_pilot_desired_rotor_speed());
     172             :         } else {
     173           0 :             ap.motor_interlock_switch = false;
     174           0 :             motors->set_desired_rotor_speed(0.0f);
     175             :         }
     176             :     }
     177             : 
     178      404223 : }
     179             : 
     180             : 
     181             : // heli_update_autorotation - determines if aircraft is in autorotation and sets motors flag and switches
     182             : // to autorotation flight mode if manual collective is not being used.
     183     3233979 : void Copter::heli_update_autorotation()
     184             : {
     185     3233979 :     bool in_autorotation_mode = false;
     186             : #if MODE_AUTOROTATE_ENABLED
     187     3233979 :     in_autorotation_mode = flightmode == &mode_autorotate;
     188             : #endif
     189             : 
     190             :     // If we have landed then we do not want to be in autorotation and we do not want to via the bailout state
     191     3233979 :     if (ap.land_complete || ap.land_complete_maybe) {
     192     1430458 :         motors->force_deactivate_autorotation();
     193     1430458 :         return;
     194             :     }
     195             : 
     196             :     // if we got this far we are flying, check for conditions to set autorotation state
     197     1803521 :     if (!motors->get_interlock() && (flightmode->has_manual_throttle() || in_autorotation_mode)) {
     198             :         // set state in motors to facilitate manual and assisted autorotations
     199       27897 :         motors->set_autorotation_active(true);
     200             :     } else {
     201             :         // deactivate the autorotation state via the bailout case
     202     1775624 :         motors->set_autorotation_active(false);
     203             :     }
     204             : }
     205             : 
     206             : // update collective low flag.  Use a debounce time of 400 milliseconds.
     207      404223 : void Copter::update_collective_low_flag(int16_t throttle_control)
     208             : {
     209             :     static uint32_t last_nonzero_collective_ms = 0;
     210      404223 :     uint32_t tnow_ms = millis();
     211             : 
     212      404223 :     if (throttle_control > 0) {
     213      196953 :         last_nonzero_collective_ms = tnow_ms;
     214      196953 :         heli_flags.coll_stk_low = false;
     215      207270 :     } else if (tnow_ms - last_nonzero_collective_ms > 400) {
     216      206552 :         heli_flags.coll_stk_low = true;
     217             :     }
     218      404223 : }
     219             : 
     220             : #endif  // FRAME_CONFIG == HELI_FRAME