ardupilot/libraries/AP_Motors/AP_MotorsMatrixTS.cpp

172 lines
6.5 KiB
C++

/*
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 + _roll_in_ff) * compensation_gain;
pitch_thrust = (_pitch_in + _pitch_in_ff) * 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;
}