aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Motors: remove AP_MotorsMatrixTS

Browse Source
This commit is contained in:
Mark Whitehorn 2020-08-09 08:23:03 -06:00 committed by Andrew Tridgell
parent dba3158446
commit d3c1838fbe
4 changed files with 15 additions and 182 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/AP_Motors/AP_Motors.h
View File

@ -11,4 +11,3 @@
#include "AP_MotorsCoax.h"
#include "AP_MotorsTailsitter.h"
#include "AP_Motors6DOF.h"
#include "AP_MotorsMatrixTS.h"

15
libraries/AP_Motors/AP_MotorsMatrix.cpp
View File

@ -15,6 +15,7 @@
#include <AP_HAL/AP_HAL.h>
#include "AP_MotorsMatrix.h"
#include <AP_Vehicle/AP_Vehicle.h>
extern const AP_HAL::HAL& hal;
@ -504,6 +505,20 @@ void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_ty
add_motor(AP_MOTORS_MOT_3, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4);
add_motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2);
break;
#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
case MOTOR_FRAME_TYPE_NYT_PLUS:
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);
break;
case MOTOR_FRAME_TYPE_NYT_X:
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);
break;
#endif
case MOTOR_FRAME_TYPE_BF_X:
// betaflight quad X order
// see: https://fpvfrenzy.com/betaflight-motor-order/

153
libraries/AP_Motors/AP_MotorsMatrixTS.cpp
View File

@ -1,153 +0,0 @@
/*
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_armed - sends commands to the motors
// includes new scaling stability patch
void AP_MotorsMatrixTS::output_armed_stabilizing()
{
if (use_standard_matrix) {
AP_MotorsMatrix::output_armed_stabilizing();
return;
}
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 = true;
limit.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;
}
}
}
// compensation_gain can never be zero
_throttle_out = (throttle_thrust + thr_adj) / compensation_gain;
}
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;
use_standard_matrix = true;
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_NYT_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);
use_standard_matrix = false;
success = true;
break;
case MOTOR_FRAME_TYPE_NYT_X:
// PLUS_TS layout rotated 45 degrees about X axis
// no differential torque for yaw: wing and fin motors counter-rotating
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);
use_standard_matrix = false;
success = true;
break;
default:
// matrixTS doesn't support the configured frame_type
// try to use normal Matrix
AP_MotorsMatrix::setup_motors(frame_class, frame_type);
return;
}
break;
default:
// matrixTS doesn't support the configured frame_class
// try to use normal Matrix
AP_MotorsMatrix::setup_motors(frame_class, frame_type);
return;
} // switch frame_class
// normalise factors to magnitude 0.5
normalise_rpy_factors();
_flags.initialised_ok = success;
}

28
libraries/AP_Motors/AP_MotorsMatrixTS.h
View File

@ -1,28 +0,0 @@
/// @file AP_MotorsMatrixTS.h
/// @brief Motor control class for tailsitters with multicopter motor configurations
#pragma once
#include <AP_Common/AP_Common.h>
#include <AP_Math/AP_Math.h>
#include <SRV_Channel/SRV_Channel.h>
#include "AP_MotorsMatrix.h"
/// @class AP_MotorsMatrix
class AP_MotorsMatrixTS : public AP_MotorsMatrix {
public:
AP_MotorsMatrixTS(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) :
AP_MotorsMatrix(loop_rate, speed_hz) {
AP_Param::setup_object_defaults(this, var_info);
};
protected:
bool use_standard_matrix; // True to use normal matrix mixers with yaw torque
// configures the motors for the defined frame_class and frame_type
virtual void setup_motors(motor_frame_class frame_class, motor_frame_type frame_type) override;
// calculate motor outputs
void output_armed_stabilizing() override;
};