/* * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #include #include #include #include "AP_MotorsHeli_Quad.h" extern const AP_HAL::HAL& hal; const AP_Param::GroupInfo AP_MotorsHeli_Quad::var_info[] = { AP_NESTEDGROUPINFO(AP_MotorsHeli, 0), // Indices 1-3 were used by RSC_PWM_MIN, RSC_PWM_MAX and RSC_PWM_REV and should not be used AP_GROUPEND }; #define QUAD_SERVO_MAX_ANGLE 4500 // set update rate to motors - a value in hertz void AP_MotorsHeli_Quad::set_update_rate( uint16_t speed_hz ) { // record requested speed _speed_hz = speed_hz; // setup fast channels uint32_t mask = 0; for (uint8_t i=0; i= _collective_max ) { _collective_min.set(AP_MOTORS_HELI_COLLECTIVE_MIN); _collective_max.set(AP_MOTORS_HELI_COLLECTIVE_MAX); } _collective_zero_thrust_deg.set(constrain_float(_collective_zero_thrust_deg, _collective_min_deg, _collective_max_deg)); _collective_land_min_deg.set(constrain_float(_collective_land_min_deg, _collective_min_deg, _collective_max_deg)); if (!is_equal((float)_collective_max_deg, (float)_collective_min_deg)) { // calculate collective zero thrust point as a number from 0 to 1 _collective_zero_thrust_pct = (_collective_zero_thrust_deg-_collective_min_deg)/(_collective_max_deg-_collective_min_deg); // calculate collective land min point as a number from 0 to 1 _collective_land_min_pct = (_collective_land_min_deg-_collective_min_deg)/(_collective_max_deg-_collective_min_deg); } else { _collective_zero_thrust_pct = 0.0f; _collective_land_min_pct = 0.0f; } // calculate factors based on swash type and servo position calculate_roll_pitch_collective_factors(); // set mode of main rotor controller and trigger recalculation of scalars _main_rotor.set_control_mode(static_cast(_main_rotor._rsc_mode.get())); calculate_armed_scalars(); } // calculate_swash_factors - calculate factors based on swash type and servo position void AP_MotorsHeli_Quad::calculate_roll_pitch_collective_factors() { // assume X quad layout, with motors at 45, 135, 225 and 315 degrees // order FrontRight, RearLeft, FrontLeft, RearLeft const float angles[AP_MOTORS_HELI_QUAD_NUM_MOTORS] = { 45, 225, 315, 135 }; const bool x_clockwise[AP_MOTORS_HELI_QUAD_NUM_MOTORS] = { false, false, true, true }; const float cos45 = cosf(radians(45)); for (uint8_t i=0; i= 1.0f) { collective_out = 1.0f; limit.throttle_upper = true; } // ensure not below landed/landing collective if (_heliflags.landing_collective && collective_out < _collective_land_min_pct) { collective_out = _collective_land_min_pct; limit.throttle_lower = true; } // updates below land min collective flag if (collective_out <= _collective_land_min_pct) { _heliflags.below_land_min_coll = true; } else { _heliflags.below_land_min_coll = false; } // updates takeoff collective flag based on 50% hover collective update_takeoff_collective_flag(collective_out); float collective_range = (_collective_max - _collective_min) * 0.001f; // feed power estimate into main rotor controller _main_rotor.set_collective(fabsf(collective_out)); // rescale collective for overhead calc collective_out -= _collective_zero_thrust_pct; // reserve some collective for attitude control collective_out *= collective_range; for (uint8_t i=0; i