/* 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 <http://www.gnu.org/licenses/>. */ /* * AP_MotorsMatrixTS.cpp - tailsitters with multicopter motor configuration */ #include <AP_BattMonitor/AP_BattMonitor.h> #include <AP_HAL/AP_HAL.h> #include "AP_MotorsMatrixTS.h" extern const AP_HAL::HAL& hal; #define SERVO_OUTPUT_RANGE 4500 // output a thrust to all motors that match a given motor mask. This // is used to control motors enabled for forward flight. Thrust is in // the range 0 to 1 void AP_MotorsMatrixTS::output_motor_mask(float thrust, uint8_t mask, float rudder_dt) { const int16_t pwm_min = get_pwm_output_min(); const int16_t pwm_range = get_pwm_output_max() - pwm_min; for (uint8_t i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) { if (motor_enabled[i]) { int16_t motor_out; if (mask & (1U<<i)) { /* apply rudder mixing differential thrust copter frame roll is plane frame yaw (this is only used by tiltrotors and tailsitters) */ float diff_thrust = get_roll_factor(i) * rudder_dt * 0.5f; motor_out = pwm_min + pwm_range * constrain_float(thrust + diff_thrust, 0.0f, 1.0f); } else { motor_out = pwm_min; } rc_write(i, motor_out); } } } void AP_MotorsMatrixTS::output_to_motors() { // calls calc_thrust_to_pwm(_thrust_rpyt_out[i]) for each enabled motor AP_MotorsMatrix::output_to_motors(); // also actuate control surfaces 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); } // output_armed - sends commands to the motors // includes new scaling stability patch void AP_MotorsMatrixTS::output_armed_stabilizing() { float roll_thrust; // roll thrust input value, +/- 1.0 float pitch_thrust; // pitch thrust input value, +/- 1.0 float throttle_thrust; // throttle thrust input value, 0.0 - 1.0 float thrust_max = 0.0f; // 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(); // compensation for battery voltage and altitude roll_thrust = _roll_in * compensation_gain; pitch_thrust = _pitch_in * compensation_gain; throttle_thrust = get_throttle() * compensation_gain; // sanity check throttle is above zero and below current limited throttle if (throttle_thrust <= 0.0f) { throttle_thrust = 0.0f; limit.throttle_lower = true; } if (throttle_thrust >= _throttle_thrust_max) { throttle_thrust = _throttle_thrust_max; limit.throttle_upper = true; } thrust_max = 0.0f; for (int i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) { if (motor_enabled[i]) { // calculate the thrust outputs for roll and pitch _thrust_rpyt_out[i] = throttle_thrust + roll_thrust * _roll_factor[i] + pitch_thrust * _pitch_factor[i]; if (thrust_max < _thrust_rpyt_out[i]) { thrust_max = _thrust_rpyt_out[i]; } } } // if max thrust is more than one reduce average throttle if (thrust_max > 1.0f) { thr_adj = 1.0f - thrust_max; limit.throttle_upper = true; limit.roll_pitch = true; for (int i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) { if (motor_enabled[i]) { // calculate the thrust outputs for roll and pitch _thrust_rpyt_out[i] += thr_adj; } } } } void AP_MotorsMatrixTS::setup_motors(motor_frame_class frame_class, motor_frame_type frame_type) { // remove existing motors for (int8_t i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) { remove_motor(i); } bool success = false; switch (frame_class) { case MOTOR_FRAME_TRI: // frame_type ignored since only one frame type is currently supported add_motor(AP_MOTORS_MOT_1, 90, 0, 2); add_motor(AP_MOTORS_MOT_2, -90, 0, 4); add_motor(AP_MOTORS_MOT_4, 180, 0, 3); success = true; break; case MOTOR_FRAME_QUAD: switch (frame_type) { case MOTOR_FRAME_TYPE_PLUS: // motors 1,2 on wings, motors 3,4 on vertical tail/subfin // motors 1,2 are counter-rotating, as are motors 3,4 // left wing motor is CW (looking from front) // don't think it matters which of 3,4 is CW add_motor(AP_MOTORS_MOT_1, 90, 0, 2); add_motor(AP_MOTORS_MOT_2, -90, 0, 4); add_motor(AP_MOTORS_MOT_3, 0, 0, 1); add_motor(AP_MOTORS_MOT_4, 180, 0, 3); success = true; break; case MOTOR_FRAME_TYPE_X: // PLUS_TS layout rotated 45 degrees about X axis add_motor(AP_MOTORS_MOT_1, 45, 0, 1); add_motor(AP_MOTORS_MOT_2, -135, 0, 3); add_motor(AP_MOTORS_MOT_3, -45, 0, 4); add_motor(AP_MOTORS_MOT_4, 135, 0, 2); success = true; break; default: // matrixTS doesn't support the configured frame_type break; } break; default: // matrixTS doesn't support the configured frame_class break; } // switch frame_class // normalise factors to magnitude 0.5 normalise_rpy_factors(); _flags.initialised_ok = success; }