AP_Motors: consolidate matrix multicopters
AP_MOTORS_x_FRAME definitions moved to motor_frame_type enum init function and now accepts frame-class and type to perform initial motor setup set_frame_class_and_type allows real-time changing of motor setup initialised_ok flag and accessor records whether setup was successful
This commit is contained in:
Randy Mackay
parent
41189758b8
commit
7be0f437a9
@ -4,11 +4,6 @@
|
||||
#include "AP_MotorsMulticopter.h"
|
||||
#include "AP_MotorsMatrix.h"
|
||||
#include "AP_MotorsTri.h"
|
||||
#include "AP_MotorsQuad.h"
|
||||
#include "AP_MotorsHexa.h"
|
||||
#include "AP_MotorsY6.h"
|
||||
#include "AP_MotorsOcta.h"
|
||||
#include "AP_MotorsOctaQuad.h"
|
||||
#include "AP_MotorsHeli_Single.h"
|
||||
#include "AP_MotorsSingle.h"
|
||||
#include "AP_MotorsCoax.h"
|
||||
|
||||
@ -60,7 +60,7 @@ const AP_Param::GroupInfo AP_MotorsCoax::var_info[] = {
|
||||
AP_GROUPEND
|
||||
};
|
||||
// init
|
||||
void AP_MotorsCoax::Init()
|
||||
void AP_MotorsCoax::init(motor_frame_class frame_class, motor_frame_type frame_type)
|
||||
{
|
||||
// set update rate for the 3 motors (but not the servo on channel 7)
|
||||
set_update_rate(_speed_hz);
|
||||
@ -78,6 +78,15 @@ void AP_MotorsCoax::Init()
|
||||
_servo2.set_angle(AP_MOTORS_COAX_SERVO_INPUT_RANGE);
|
||||
_servo3.set_angle(AP_MOTORS_COAX_SERVO_INPUT_RANGE);
|
||||
_servo4.set_angle(AP_MOTORS_COAX_SERVO_INPUT_RANGE);
|
||||
|
||||
// record successful initialisation if what we setup was the desired frame_class
|
||||
_flags.initialised_ok = (frame_class == MOTOR_FRAME_COAX);
|
||||
}
|
||||
|
||||
// set frame class (i.e. quad, hexa, heli) and type (i.e. x, plus)
|
||||
void AP_MotorsCoax::set_frame_class_and_type(motor_frame_class frame_class, motor_frame_type frame_type)
|
||||
{
|
||||
_flags.initialised_ok = (frame_class == MOTOR_FRAME_COAX);
|
||||
}
|
||||
|
||||
// set update rate to motors - a value in hertz
|
||||
|
||||
@ -31,7 +31,10 @@ public:
|
||||
};
|
||||
|
||||
// init
|
||||
virtual void Init();
|
||||
void init(motor_frame_class frame_class, motor_frame_type frame_type);
|
||||
|
||||
// set frame class (i.e. quad, hexa, heli) and type (i.e. x, plus)
|
||||
void set_frame_class_and_type(motor_frame_class frame_class, motor_frame_type frame_type);
|
||||
|
||||
// set update rate to motors - a value in hertz
|
||||
void set_update_rate( uint16_t speed_hz );
|
||||
|
||||
@ -179,7 +179,7 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] = {
|
||||
//
|
||||
|
||||
// init
|
||||
void AP_MotorsHeli::Init()
|
||||
void AP_MotorsHeli::init(motor_frame_class frame_class, motor_frame_type frame_type)
|
||||
{
|
||||
// set update rate
|
||||
set_update_rate(_speed_hz);
|
||||
@ -198,6 +198,15 @@ void AP_MotorsHeli::Init()
|
||||
|
||||
// calculate all scalars
|
||||
calculate_scalars();
|
||||
|
||||
// record successful initialisation if what we setup was the desired frame_class
|
||||
_flags.initialised_ok = (frame_class == MOTOR_FRAME_HELI);
|
||||
}
|
||||
|
||||
// set frame class (i.e. quad, hexa, heli) and type (i.e. x, plus)
|
||||
void AP_MotorsHeli::set_frame_class_and_type(motor_frame_class frame_class, motor_frame_type frame_type)
|
||||
{
|
||||
_flags.initialised_ok = (frame_class == MOTOR_FRAME_HELI);
|
||||
}
|
||||
|
||||
// output_min - sets servos to neutral point with motors stopped
|
||||
|
||||
@ -62,7 +62,10 @@ public:
|
||||
};
|
||||
|
||||
// init
|
||||
void Init();
|
||||
void init(motor_frame_class frame_class, motor_frame_type frame_type);
|
||||
|
||||
// set frame class (i.e. quad, hexa, heli) and type (i.e. x, plus)
|
||||
void set_frame_class_and_type(motor_frame_class frame_class, motor_frame_type frame_type);
|
||||
|
||||
// set update rate to motors - a value in hertz
|
||||
virtual void set_update_rate( uint16_t speed_hz ) = 0;
|
||||
|
||||
@ -1,51 +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_MotorsHexa.cpp - ArduCopter motors library
|
||||
* Code by RandyMackay. DIYDrones.com
|
||||
*
|
||||
*/
|
||||
|
||||
#include "AP_MotorsHexa.h"
|
||||
|
||||
// setup_motors - configures the motors for a hexa
|
||||
void AP_MotorsHexa::setup_motors()
|
||||
{
|
||||
// call parent
|
||||
AP_MotorsMatrix::setup_motors();
|
||||
|
||||
// hard coded config for supported frames
|
||||
if( _flags.frame_orientation == AP_MOTORS_PLUS_FRAME ) {
|
||||
// plus frame set-up
|
||||
add_motor(AP_MOTORS_MOT_1, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
|
||||
add_motor(AP_MOTORS_MOT_2, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
|
||||
add_motor(AP_MOTORS_MOT_3,-120, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5);
|
||||
add_motor(AP_MOTORS_MOT_4, 60, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
|
||||
add_motor(AP_MOTORS_MOT_5, -60, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6);
|
||||
add_motor(AP_MOTORS_MOT_6, 120, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
|
||||
}else{
|
||||
// X frame set-up
|
||||
add_motor(AP_MOTORS_MOT_1, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2);
|
||||
add_motor(AP_MOTORS_MOT_2, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5);
|
||||
add_motor(AP_MOTORS_MOT_3, -30, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6);
|
||||
add_motor(AP_MOTORS_MOT_4, 150, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
|
||||
add_motor(AP_MOTORS_MOT_5, 30, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1);
|
||||
add_motor(AP_MOTORS_MOT_6,-150, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4);
|
||||
}
|
||||
|
||||
// normalise factors to magnitude 0.5
|
||||
normalise_rpy_factors();
|
||||
}
|
||||
@ -1,24 +0,0 @@
|
||||
/// @file AP_MotorsHexa.h
|
||||
/// @brief Motor control class for Hexacopters
|
||||
#pragma once
|
||||
|
||||
#include <AP_Common/AP_Common.h>
|
||||
#include <AP_Math/AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
|
||||
#include <RC_Channel/RC_Channel.h> // RC Channel Library
|
||||
#include "AP_MotorsMatrix.h" // Parent Motors Matrix library
|
||||
|
||||
/// @class AP_MotorsHexa
|
||||
class AP_MotorsHexa : public AP_MotorsMatrix {
|
||||
public:
|
||||
|
||||
/// Constructor
|
||||
AP_MotorsHexa(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) :
|
||||
AP_MotorsMatrix(loop_rate, speed_hz)
|
||||
{ };
|
||||
|
||||
// setup_motors - configures the motors for a hexa
|
||||
virtual void setup_motors();
|
||||
|
||||
protected:
|
||||
|
||||
};
|
||||
@ -24,10 +24,10 @@
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
// Init
|
||||
void AP_MotorsMatrix::Init()
|
||||
void AP_MotorsMatrix::init(motor_frame_class frame_class, motor_frame_type frame_type)
|
||||
{
|
||||
// setup the motors
|
||||
setup_motors();
|
||||
setup_motors(frame_class, frame_type);
|
||||
|
||||
// enable fast channels or instant pwm
|
||||
set_update_rate(_speed_hz);
|
||||
@ -51,19 +51,18 @@ void AP_MotorsMatrix::set_update_rate( uint16_t speed_hz )
|
||||
rc_set_freq( mask, _speed_hz );
|
||||
}
|
||||
|
||||
// set frame orientation (normally + or X)
|
||||
void AP_MotorsMatrix::set_frame_orientation( uint8_t new_orientation )
|
||||
// set frame class (i.e. quad, hexa, heli) and type (i.e. x, plus)
|
||||
void AP_MotorsMatrix::set_frame_class_and_type(motor_frame_class frame_class, motor_frame_type frame_type)
|
||||
{
|
||||
// return if nothing has changed
|
||||
if( new_orientation == _flags.frame_orientation ) {
|
||||
// exit immediately if armed or no change
|
||||
if (armed() || (frame_class == _last_frame_class && _last_frame_type == frame_type)) {
|
||||
return;
|
||||
}
|
||||
|
||||
// call parent
|
||||
AP_Motors::set_frame_orientation( new_orientation );
|
||||
_last_frame_class = frame_class;
|
||||
_last_frame_type = frame_type;
|
||||
|
||||
// setup the motors
|
||||
setup_motors();
|
||||
setup_motors(frame_class, frame_type);
|
||||
|
||||
// enable fast channels or instant pwm
|
||||
set_update_rate(_speed_hz);
|
||||
@ -363,12 +362,249 @@ void AP_MotorsMatrix::remove_motor(int8_t motor_num)
|
||||
}
|
||||
}
|
||||
|
||||
// remove_all_motors - removes all motor definitions
|
||||
void AP_MotorsMatrix::remove_all_motors()
|
||||
void AP_MotorsMatrix::setup_motors(motor_frame_class frame_class, motor_frame_type frame_type)
|
||||
{
|
||||
for( int8_t i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
|
||||
// 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_QUAD:
|
||||
switch (frame_type) {
|
||||
case MOTOR_FRAME_TYPE_PLUS:
|
||||
add_motor(AP_MOTORS_MOT_1, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
|
||||
add_motor(AP_MOTORS_MOT_2, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
|
||||
add_motor(AP_MOTORS_MOT_3, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
|
||||
add_motor(AP_MOTORS_MOT_4, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
|
||||
success = true;
|
||||
break;
|
||||
case MOTOR_FRAME_TYPE_X:
|
||||
add_motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1);
|
||||
add_motor(AP_MOTORS_MOT_2, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
|
||||
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);
|
||||
success = true;
|
||||
break;
|
||||
case MOTOR_FRAME_TYPE_V:
|
||||
add_motor(AP_MOTORS_MOT_1, 45, 0.7981f, 1);
|
||||
add_motor(AP_MOTORS_MOT_2, -135, 1.0000f, 3);
|
||||
add_motor(AP_MOTORS_MOT_3, -45, -0.7981f, 4);
|
||||
add_motor(AP_MOTORS_MOT_4, 135, -1.0000f, 2);
|
||||
success = true;
|
||||
break;
|
||||
case MOTOR_FRAME_TYPE_H:
|
||||
// H frame set-up - same as X but motors spin in opposite directiSons
|
||||
add_motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
|
||||
add_motor(AP_MOTORS_MOT_2, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
|
||||
add_motor(AP_MOTORS_MOT_3, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
|
||||
add_motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
|
||||
success = true;
|
||||
break;
|
||||
case MOTOR_FRAME_TYPE_VTAIL:
|
||||
/*
|
||||
Tested with: Lynxmotion Hunter Vtail 400
|
||||
- inverted rear outward blowing motors (at a 40 degree angle)
|
||||
- should also work with non-inverted rear outward blowing motors
|
||||
- no roll in rear motors
|
||||
- no yaw in front motors
|
||||
- should fly like some mix between a tricopter and X Quadcopter
|
||||
|
||||
Roll control comes only from the front motors, Yaw control only from the rear motors.
|
||||
Roll & Pitch factor is measured by the angle away from the top of the forward axis to each arm.
|
||||
|
||||
Note: if we want the front motors to help with yaw,
|
||||
motors 1's yaw factor should be changed to sin(radians(40)). Where "40" is the vtail angle
|
||||
motors 3's yaw factor should be changed to -sin(radians(40))
|
||||
*/
|
||||
add_motor(AP_MOTORS_MOT_1, 60, 60, 0, 1);
|
||||
add_motor(AP_MOTORS_MOT_2, 0, -160, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
|
||||
add_motor(AP_MOTORS_MOT_3, -60, -60, 0, 4);
|
||||
add_motor(AP_MOTORS_MOT_4, 0, 160, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
|
||||
success = true;
|
||||
break;
|
||||
case MOTOR_FRAME_TYPE_ATAIL:
|
||||
/*
|
||||
The A-Shaped VTail is the exact same as a V-Shaped VTail, with one difference:
|
||||
- The Yaw factors are reversed, because the rear motors are facing different directions
|
||||
|
||||
With V-Shaped VTails, the props make a V-Shape when spinning, but with
|
||||
A-Shaped VTails, the props make an A-Shape when spinning.
|
||||
- Rear thrust on a V-Shaped V-Tail Quad is outward
|
||||
- Rear thrust on an A-Shaped V-Tail Quad is inward
|
||||
|
||||
Still functions the same as the V-Shaped VTail mixing below:
|
||||
- Yaw control is entirely in the rear motors
|
||||
- Roll is is entirely in the front motors
|
||||
*/
|
||||
add_motor(AP_MOTORS_MOT_1, 60, 60, 0, 1);
|
||||
add_motor(AP_MOTORS_MOT_2, 0, -160, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
|
||||
add_motor(AP_MOTORS_MOT_3, -60, -60, 0, 4);
|
||||
add_motor(AP_MOTORS_MOT_4, 0, 160, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2);
|
||||
success = true;
|
||||
break;
|
||||
default:
|
||||
// quad frame class does not support this frame type
|
||||
break;
|
||||
}
|
||||
break; // quad
|
||||
|
||||
case MOTOR_FRAME_HEXA:
|
||||
switch (frame_type) {
|
||||
case MOTOR_FRAME_TYPE_PLUS:
|
||||
add_motor(AP_MOTORS_MOT_1, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
|
||||
add_motor(AP_MOTORS_MOT_2, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
|
||||
add_motor(AP_MOTORS_MOT_3,-120, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5);
|
||||
add_motor(AP_MOTORS_MOT_4, 60, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
|
||||
add_motor(AP_MOTORS_MOT_5, -60, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6);
|
||||
add_motor(AP_MOTORS_MOT_6, 120, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
|
||||
success = true;
|
||||
break;
|
||||
case MOTOR_FRAME_TYPE_X:
|
||||
add_motor(AP_MOTORS_MOT_1, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2);
|
||||
add_motor(AP_MOTORS_MOT_2, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5);
|
||||
add_motor(AP_MOTORS_MOT_3, -30, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6);
|
||||
add_motor(AP_MOTORS_MOT_4, 150, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
|
||||
add_motor(AP_MOTORS_MOT_5, 30, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1);
|
||||
add_motor(AP_MOTORS_MOT_6,-150, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4);
|
||||
success = true;
|
||||
break;
|
||||
default:
|
||||
// hexa frame class does not support this frame type
|
||||
break;
|
||||
}
|
||||
break;
|
||||
|
||||
case MOTOR_FRAME_OCTA:
|
||||
switch (frame_type) {
|
||||
case MOTOR_FRAME_TYPE_PLUS:
|
||||
add_motor(AP_MOTORS_MOT_1, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
|
||||
add_motor(AP_MOTORS_MOT_2, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5);
|
||||
add_motor(AP_MOTORS_MOT_3, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
|
||||
add_motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
|
||||
add_motor(AP_MOTORS_MOT_5, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8);
|
||||
add_motor(AP_MOTORS_MOT_6, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6);
|
||||
add_motor(AP_MOTORS_MOT_7, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7);
|
||||
add_motor(AP_MOTORS_MOT_8, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
|
||||
success = true;
|
||||
break;
|
||||
case MOTOR_FRAME_TYPE_X:
|
||||
add_motor(AP_MOTORS_MOT_1, 22.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
|
||||
add_motor(AP_MOTORS_MOT_2, -157.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5);
|
||||
add_motor(AP_MOTORS_MOT_3, 67.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
|
||||
add_motor(AP_MOTORS_MOT_4, 157.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
|
||||
add_motor(AP_MOTORS_MOT_5, -22.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8);
|
||||
add_motor(AP_MOTORS_MOT_6, -112.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6);
|
||||
add_motor(AP_MOTORS_MOT_7, -67.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7);
|
||||
add_motor(AP_MOTORS_MOT_8, 112.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
|
||||
success = true;
|
||||
break;
|
||||
case MOTOR_FRAME_TYPE_V:
|
||||
add_motor_raw(AP_MOTORS_MOT_1, 1.0f, 0.34f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7);
|
||||
add_motor_raw(AP_MOTORS_MOT_2, -1.0f, -0.32f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
|
||||
add_motor_raw(AP_MOTORS_MOT_3, 1.0f, -0.32f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6);
|
||||
add_motor_raw(AP_MOTORS_MOT_4, -0.5f, -1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
|
||||
add_motor_raw(AP_MOTORS_MOT_5, 1.0f, 1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8);
|
||||
add_motor_raw(AP_MOTORS_MOT_6, -1.0f, 0.34f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
|
||||
add_motor_raw(AP_MOTORS_MOT_7, -1.0f, 1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
|
||||
add_motor_raw(AP_MOTORS_MOT_8, 0.5f, -1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5);
|
||||
success = true;
|
||||
break;
|
||||
default:
|
||||
// octa frame class does not support this frame type
|
||||
break;
|
||||
} // octa frame type
|
||||
break;
|
||||
|
||||
case MOTOR_FRAME_OCTAQUAD:
|
||||
switch (frame_type) {
|
||||
case MOTOR_FRAME_TYPE_PLUS:
|
||||
add_motor(AP_MOTORS_MOT_1, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1);
|
||||
add_motor(AP_MOTORS_MOT_2, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7);
|
||||
add_motor(AP_MOTORS_MOT_3, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5);
|
||||
add_motor(AP_MOTORS_MOT_4, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
|
||||
add_motor(AP_MOTORS_MOT_5, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8);
|
||||
add_motor(AP_MOTORS_MOT_6, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2);
|
||||
add_motor(AP_MOTORS_MOT_7, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
|
||||
add_motor(AP_MOTORS_MOT_8, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6);
|
||||
success = true;
|
||||
break;
|
||||
case MOTOR_FRAME_TYPE_X:
|
||||
add_motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1);
|
||||
add_motor(AP_MOTORS_MOT_2, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7);
|
||||
add_motor(AP_MOTORS_MOT_3, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5);
|
||||
add_motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
|
||||
add_motor(AP_MOTORS_MOT_5, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8);
|
||||
add_motor(AP_MOTORS_MOT_6, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2);
|
||||
add_motor(AP_MOTORS_MOT_7, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
|
||||
add_motor(AP_MOTORS_MOT_8, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6);
|
||||
success = true;
|
||||
break;
|
||||
case MOTOR_FRAME_TYPE_V:
|
||||
add_motor(AP_MOTORS_MOT_1, 45, 0.7981f, 1);
|
||||
add_motor(AP_MOTORS_MOT_2, -45, -0.7981f, 7);
|
||||
add_motor(AP_MOTORS_MOT_3, -135, 1.0000f, 5);
|
||||
add_motor(AP_MOTORS_MOT_4, 135, -1.0000f, 3);
|
||||
add_motor(AP_MOTORS_MOT_5, -45, 0.7981f, 8);
|
||||
add_motor(AP_MOTORS_MOT_6, 45, -0.7981f, 2);
|
||||
add_motor(AP_MOTORS_MOT_7, 135, 1.0000f, 4);
|
||||
add_motor(AP_MOTORS_MOT_8, -135, -1.0000f, 6);
|
||||
success = true;
|
||||
break;
|
||||
case MOTOR_FRAME_TYPE_H:
|
||||
// H frame set-up - same as X but motors spin in opposite directions
|
||||
add_motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
|
||||
add_motor(AP_MOTORS_MOT_2, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 7);
|
||||
add_motor(AP_MOTORS_MOT_3, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5);
|
||||
add_motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
|
||||
add_motor(AP_MOTORS_MOT_5, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 8);
|
||||
add_motor(AP_MOTORS_MOT_6, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
|
||||
add_motor(AP_MOTORS_MOT_7, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4);
|
||||
add_motor(AP_MOTORS_MOT_8, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6);
|
||||
success = true;
|
||||
break;
|
||||
default:
|
||||
// octaquad frame class does not support this frame type
|
||||
break;
|
||||
}
|
||||
break;
|
||||
|
||||
case MOTOR_FRAME_Y6:
|
||||
switch (frame_type) {
|
||||
case MOTOR_FRAME_TYPE_NEW_PLUS:
|
||||
// Y6 motor definition with all top motors spinning clockwise, all bottom motors counter clockwise
|
||||
add_motor_raw(AP_MOTORS_MOT_1, -1.0f, 0.500f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
|
||||
add_motor_raw(AP_MOTORS_MOT_2, -1.0f, 0.500f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
|
||||
add_motor_raw(AP_MOTORS_MOT_3, 0.0f, -1.000f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
|
||||
add_motor_raw(AP_MOTORS_MOT_4, 0.0f, -1.000f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
|
||||
add_motor_raw(AP_MOTORS_MOT_5, 1.0f, 0.500f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5);
|
||||
add_motor_raw(AP_MOTORS_MOT_6, 1.0f, 0.500f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6);
|
||||
success = true;
|
||||
break;
|
||||
default:
|
||||
add_motor_raw(AP_MOTORS_MOT_1, -1.0f, 0.666f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
|
||||
add_motor_raw(AP_MOTORS_MOT_2, 1.0f, 0.666f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5);
|
||||
add_motor_raw(AP_MOTORS_MOT_3, 1.0f, 0.666f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6);
|
||||
add_motor_raw(AP_MOTORS_MOT_4, 0.0f, -1.333f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4);
|
||||
add_motor_raw(AP_MOTORS_MOT_5, -1.0f, 0.666f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
|
||||
add_motor_raw(AP_MOTORS_MOT_6, 0.0f, -1.333f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
|
||||
success = true;
|
||||
break;
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
// matrix doesn't support the configured class
|
||||
break;
|
||||
} // switch frame_class
|
||||
|
||||
// normalise factors to magnitude 0.5
|
||||
normalise_rpy_factors();
|
||||
|
||||
_flags.initialised_ok = success;
|
||||
}
|
||||
|
||||
// normalizes the roll, pitch and yaw factors so maximum magnitude is 0.5
|
||||
|
||||
@ -20,15 +20,15 @@ public:
|
||||
{};
|
||||
|
||||
// init
|
||||
void Init();
|
||||
void init(motor_frame_class frame_class, motor_frame_type frame_type);
|
||||
|
||||
// set frame class (i.e. quad, hexa, heli) and type (i.e. x, plus)
|
||||
void set_frame_class_and_type(motor_frame_class frame_class, motor_frame_type frame_type);
|
||||
|
||||
// set update rate to motors - a value in hertz
|
||||
// you must have setup_motors before calling this
|
||||
void set_update_rate(uint16_t speed_hz);
|
||||
|
||||
// set frame orientation (normally + or X)
|
||||
void set_frame_orientation(uint8_t new_orientation);
|
||||
|
||||
// enable - starts allowing signals to be sent to motors
|
||||
void enable();
|
||||
|
||||
@ -40,9 +40,6 @@ public:
|
||||
// output_to_motors - sends minimum values out to the motors
|
||||
void output_to_motors();
|
||||
|
||||
// setup_motors - configures the motors for a given frame type - should be overwritten by child classes
|
||||
virtual void setup_motors() { remove_all_motors(); };
|
||||
|
||||
// get_motor_mask - returns a bitmask of which outputs are being used for motors (1 means being used)
|
||||
// this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
|
||||
uint16_t get_motor_mask();
|
||||
@ -63,6 +60,9 @@ protected:
|
||||
// remove_motor
|
||||
void remove_motor(int8_t motor_num);
|
||||
|
||||
// configures the motors for the defined frame_class and frame_type
|
||||
void setup_motors(motor_frame_class frame_class, motor_frame_type frame_type);
|
||||
|
||||
// normalizes the roll, pitch and yaw factors so maximum magnitude is 0.5
|
||||
void normalise_rpy_factors();
|
||||
|
||||
@ -74,4 +74,6 @@ protected:
|
||||
float _yaw_factor[AP_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to yaw (normally 1 or -1)
|
||||
float _thrust_rpyt_out[AP_MOTORS_MAX_NUM_MOTORS]; // combined roll, pitch, yaw and throttle outputs to motors in 0~1 range
|
||||
uint8_t _test_order[AP_MOTORS_MAX_NUM_MOTORS]; // order of the motors in the test sequence
|
||||
motor_frame_class _last_frame_class; // most recently requested frame class (i.e. quad, hexa, octa, etc)
|
||||
motor_frame_type _last_frame_type; // most recently requested frame type (i.e. plus, x, v, etc)
|
||||
};
|
||||
|
||||
@ -1,67 +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_MotorsOcta.cpp - ArduCopter motors library
|
||||
* Code by RandyMackay. DIYDrones.com
|
||||
*
|
||||
*/
|
||||
|
||||
#include "AP_MotorsOcta.h"
|
||||
|
||||
// setup_motors - configures the motors for a octa
|
||||
void AP_MotorsOcta::setup_motors()
|
||||
{
|
||||
// call parent
|
||||
AP_MotorsMatrix::setup_motors();
|
||||
|
||||
// hard coded config for supported frames
|
||||
if( _flags.frame_orientation == AP_MOTORS_PLUS_FRAME ) {
|
||||
// plus frame set-up
|
||||
add_motor(AP_MOTORS_MOT_1, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
|
||||
add_motor(AP_MOTORS_MOT_2, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5);
|
||||
add_motor(AP_MOTORS_MOT_3, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
|
||||
add_motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
|
||||
add_motor(AP_MOTORS_MOT_5, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8);
|
||||
add_motor(AP_MOTORS_MOT_6, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6);
|
||||
add_motor(AP_MOTORS_MOT_7, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7);
|
||||
add_motor(AP_MOTORS_MOT_8, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
|
||||
|
||||
}else if( _flags.frame_orientation == AP_MOTORS_V_FRAME ) {
|
||||
// V frame set-up
|
||||
add_motor_raw(AP_MOTORS_MOT_1, 1.0f, 0.34f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7);
|
||||
add_motor_raw(AP_MOTORS_MOT_2, -1.0f, -0.32f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
|
||||
add_motor_raw(AP_MOTORS_MOT_3, 1.0f, -0.32f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6);
|
||||
add_motor_raw(AP_MOTORS_MOT_4, -0.5f, -1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
|
||||
add_motor_raw(AP_MOTORS_MOT_5, 1.0f, 1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8);
|
||||
add_motor_raw(AP_MOTORS_MOT_6, -1.0f, 0.34f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
|
||||
add_motor_raw(AP_MOTORS_MOT_7, -1.0f, 1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
|
||||
add_motor_raw(AP_MOTORS_MOT_8, 0.5f, -1.0f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5);
|
||||
|
||||
}else {
|
||||
// X frame set-up
|
||||
add_motor(AP_MOTORS_MOT_1, 22.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
|
||||
add_motor(AP_MOTORS_MOT_2, -157.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5);
|
||||
add_motor(AP_MOTORS_MOT_3, 67.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
|
||||
add_motor(AP_MOTORS_MOT_4, 157.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
|
||||
add_motor(AP_MOTORS_MOT_5, -22.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8);
|
||||
add_motor(AP_MOTORS_MOT_6, -112.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6);
|
||||
add_motor(AP_MOTORS_MOT_7, -67.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7);
|
||||
add_motor(AP_MOTORS_MOT_8, 112.5f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
|
||||
}
|
||||
|
||||
// normalise factors to magnitude 0.5
|
||||
normalise_rpy_factors();
|
||||
}
|
||||
@ -1,24 +0,0 @@
|
||||
/// @file AP_MotorsOcta.h
|
||||
/// @brief Motor control class for Octacopters
|
||||
#pragma once
|
||||
|
||||
#include <AP_Common/AP_Common.h>
|
||||
#include <AP_Math/AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
|
||||
#include <RC_Channel/RC_Channel.h> // RC Channel Library
|
||||
#include "AP_MotorsMatrix.h" // Parent Motors Matrix library
|
||||
|
||||
/// @class AP_MotorsOcta
|
||||
class AP_MotorsOcta : public AP_MotorsMatrix {
|
||||
public:
|
||||
|
||||
/// Constructor
|
||||
AP_MotorsOcta(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) :
|
||||
AP_MotorsMatrix(loop_rate, speed_hz)
|
||||
{ };
|
||||
|
||||
// setup_motors - configures the motors for an octa
|
||||
virtual void setup_motors();
|
||||
|
||||
protected:
|
||||
|
||||
};
|
||||
@ -1,75 +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_MotorsOctaQuad.cpp - ArduCopter motors library
|
||||
* Code by RandyMackay. DIYDrones.com
|
||||
*
|
||||
*/
|
||||
|
||||
#include "AP_MotorsOctaQuad.h"
|
||||
|
||||
// setup_motors - configures the motors for a octa
|
||||
void AP_MotorsOctaQuad::setup_motors()
|
||||
{
|
||||
// call parent
|
||||
AP_MotorsMatrix::setup_motors();
|
||||
|
||||
// hard coded config for supported frames
|
||||
if( _flags.frame_orientation == AP_MOTORS_PLUS_FRAME ) {
|
||||
// plus frame set-up
|
||||
add_motor(AP_MOTORS_MOT_1, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1);
|
||||
add_motor(AP_MOTORS_MOT_2, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7);
|
||||
add_motor(AP_MOTORS_MOT_3, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5);
|
||||
add_motor(AP_MOTORS_MOT_4, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
|
||||
add_motor(AP_MOTORS_MOT_5, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8);
|
||||
add_motor(AP_MOTORS_MOT_6, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2);
|
||||
add_motor(AP_MOTORS_MOT_7, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
|
||||
add_motor(AP_MOTORS_MOT_8, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6);
|
||||
}else if( _flags.frame_orientation == AP_MOTORS_V_FRAME ) {
|
||||
// V frame set-up
|
||||
add_motor(AP_MOTORS_MOT_1, 45, 0.7981f, 1);
|
||||
add_motor(AP_MOTORS_MOT_2, -45, -0.7981f, 7);
|
||||
add_motor(AP_MOTORS_MOT_3, -135, 1.0000f, 5);
|
||||
add_motor(AP_MOTORS_MOT_4, 135, -1.0000f, 3);
|
||||
add_motor(AP_MOTORS_MOT_5, -45, 0.7981f, 8);
|
||||
add_motor(AP_MOTORS_MOT_6, 45, -0.7981f, 2);
|
||||
add_motor(AP_MOTORS_MOT_7, 135, 1.0000f, 4);
|
||||
add_motor(AP_MOTORS_MOT_8, -135, -1.0000f, 6);
|
||||
}else if( _flags.frame_orientation == AP_MOTORS_H_FRAME ) {
|
||||
// H frame set-up - same as X but motors spin in opposite directions
|
||||
add_motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
|
||||
add_motor(AP_MOTORS_MOT_2, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 7);
|
||||
add_motor(AP_MOTORS_MOT_3, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5);
|
||||
add_motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
|
||||
add_motor(AP_MOTORS_MOT_5, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 8);
|
||||
add_motor(AP_MOTORS_MOT_6, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
|
||||
add_motor(AP_MOTORS_MOT_7, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4);
|
||||
add_motor(AP_MOTORS_MOT_8, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6);
|
||||
}else{
|
||||
// X frame set-up
|
||||
add_motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1);
|
||||
add_motor(AP_MOTORS_MOT_2, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 7);
|
||||
add_motor(AP_MOTORS_MOT_3, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 5);
|
||||
add_motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
|
||||
add_motor(AP_MOTORS_MOT_5, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 8);
|
||||
add_motor(AP_MOTORS_MOT_6, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2);
|
||||
add_motor(AP_MOTORS_MOT_7, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
|
||||
add_motor(AP_MOTORS_MOT_8, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 6);
|
||||
}
|
||||
|
||||
// normalise factors to magnitude 0.5
|
||||
normalise_rpy_factors();
|
||||
}
|
||||
@ -1,24 +0,0 @@
|
||||
/// @file AP_MotorsOctaQuad.h
|
||||
/// @brief Motor control class for OctaQuadcopters
|
||||
#pragma once
|
||||
|
||||
#include <AP_Common/AP_Common.h>
|
||||
#include <AP_Math/AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
|
||||
#include <RC_Channel/RC_Channel.h> // RC Channel Library
|
||||
#include "AP_MotorsMatrix.h" // Parent Motors Matrix library
|
||||
|
||||
/// @class AP_MotorsOcta
|
||||
class AP_MotorsOctaQuad : public AP_MotorsMatrix {
|
||||
public:
|
||||
|
||||
/// Constructor
|
||||
AP_MotorsOctaQuad(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) :
|
||||
AP_MotorsMatrix(loop_rate, speed_hz)
|
||||
{ };
|
||||
|
||||
// setup_motors - configures the motors for a quad
|
||||
virtual void setup_motors();
|
||||
|
||||
protected:
|
||||
|
||||
};
|
||||
@ -1,106 +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_MotorsQuad.cpp - ArduCopter motors library
|
||||
* Code by RandyMackay. DIYDrones.com
|
||||
*
|
||||
*/
|
||||
|
||||
#include "AP_MotorsQuad.h"
|
||||
|
||||
// setup_motors - configures the motors for a quad
|
||||
void AP_MotorsQuad::setup_motors()
|
||||
{
|
||||
// call parent
|
||||
AP_MotorsMatrix::setup_motors();
|
||||
|
||||
// hard coded config for supported frames
|
||||
if( _flags.frame_orientation == AP_MOTORS_PLUS_FRAME ) {
|
||||
// plus frame set-up
|
||||
add_motor(AP_MOTORS_MOT_1, 90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
|
||||
add_motor(AP_MOTORS_MOT_2, -90, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
|
||||
add_motor(AP_MOTORS_MOT_3, 0, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
|
||||
add_motor(AP_MOTORS_MOT_4, 180, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
|
||||
|
||||
}else if( _flags.frame_orientation == AP_MOTORS_V_FRAME ) {
|
||||
// V frame set-up
|
||||
add_motor(AP_MOTORS_MOT_1, 45, 0.7981f, 1);
|
||||
add_motor(AP_MOTORS_MOT_2, -135, 1.0000f, 3);
|
||||
add_motor(AP_MOTORS_MOT_3, -45, -0.7981f, 4);
|
||||
add_motor(AP_MOTORS_MOT_4, 135, -1.0000f, 2);
|
||||
|
||||
}else if( _flags.frame_orientation == AP_MOTORS_H_FRAME ) {
|
||||
// H frame set-up - same as X but motors spin in opposite directiSons
|
||||
add_motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
|
||||
add_motor(AP_MOTORS_MOT_2, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
|
||||
add_motor(AP_MOTORS_MOT_3, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
|
||||
add_motor(AP_MOTORS_MOT_4, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
|
||||
}else if(_flags.frame_orientation == AP_MOTORS_VTAIL_FRAME) {
|
||||
/*
|
||||
Tested with: Lynxmotion Hunter Vtail 400
|
||||
- inverted rear outward blowing motors (at a 40 degree angle)
|
||||
- should also work with non-inverted rear outward blowing motors
|
||||
- no roll in rear motors
|
||||
- no yaw in front motors
|
||||
- should fly like some mix between a tricopter and X Quadcopter
|
||||
|
||||
Roll control comes only from the front motors, Yaw control only from the rear motors.
|
||||
Roll & Pitch factor is measured by the angle away from the top of the forward axis to each arm.
|
||||
|
||||
Note: if we want the front motors to help with yaw,
|
||||
motors 1's yaw factor should be changed to sin(radians(40)). Where "40" is the vtail angle
|
||||
motors 3's yaw factor should be changed to -sin(radians(40))
|
||||
*/
|
||||
|
||||
add_motor(AP_MOTORS_MOT_1, 60, 60, 0, 1);
|
||||
add_motor(AP_MOTORS_MOT_2, 0, -160, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
|
||||
add_motor(AP_MOTORS_MOT_3, -60, -60, 0, 4);
|
||||
add_motor(AP_MOTORS_MOT_4, 0, 160, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
|
||||
} else if (_flags.frame_orientation == AP_MOTORS_ATAIL_FRAME) {
|
||||
/*
|
||||
The A-Shaped VTail is the exact same as a V-Shaped VTail, with one difference:
|
||||
- The Yaw factors are reversed, because the rear motors are facing different directions
|
||||
|
||||
With V-Shaped VTails, the props make a V-Shape when spinning, but with
|
||||
A-Shaped VTails, the props make an A-Shape when spinning.
|
||||
- Rear thrust on a V-Shaped V-Tail Quad is outward
|
||||
- Rear thrust on an A-Shaped V-Tail Quad is inward
|
||||
|
||||
Still functions the same as the V-Shaped VTail mixing below:
|
||||
- Yaw control is entirely in the rear motors
|
||||
- Roll is is entirely in the front motors
|
||||
*/
|
||||
add_motor(AP_MOTORS_MOT_1, 60, 60, 0, 1);
|
||||
add_motor(AP_MOTORS_MOT_2, 0, -160, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
|
||||
add_motor(AP_MOTORS_MOT_3, -60, -60, 0, 4);
|
||||
add_motor(AP_MOTORS_MOT_4, 0, 160, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2);
|
||||
} else if ( _flags.frame_orientation == AP_MOTORS_QUADPLANE ) {
|
||||
// quadplane frame set-up, X arrangement on motors 5 to 8
|
||||
add_motor(AP_MOTORS_MOT_5, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1);
|
||||
add_motor(AP_MOTORS_MOT_6, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
|
||||
add_motor(AP_MOTORS_MOT_7, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4);
|
||||
add_motor(AP_MOTORS_MOT_8, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2);
|
||||
}else{
|
||||
// X frame set-up
|
||||
add_motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1);
|
||||
add_motor(AP_MOTORS_MOT_2, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
|
||||
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);
|
||||
}
|
||||
|
||||
// normalise factors to magnitude 0.5
|
||||
normalise_rpy_factors();
|
||||
}
|
||||
@ -1,24 +0,0 @@
|
||||
/// @file AP_MotorsQuad.h
|
||||
/// @brief Motor control class for Quadcopters
|
||||
#pragma once
|
||||
|
||||
#include <AP_Common/AP_Common.h>
|
||||
#include <AP_Math/AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
|
||||
#include <RC_Channel/RC_Channel.h> // RC Channel Library
|
||||
#include "AP_MotorsMatrix.h" // Parent Motors Matrix library
|
||||
|
||||
/// @class AP_MotorsQuad
|
||||
class AP_MotorsQuad : public AP_MotorsMatrix {
|
||||
public:
|
||||
|
||||
/// Constructor
|
||||
AP_MotorsQuad(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) :
|
||||
AP_MotorsMatrix(loop_rate, speed_hz)
|
||||
{ };
|
||||
|
||||
// setup_motors - configures the motors for a quad
|
||||
virtual void setup_motors();
|
||||
|
||||
protected:
|
||||
|
||||
};
|
||||
@ -60,7 +60,7 @@ const AP_Param::GroupInfo AP_MotorsSingle::var_info[] = {
|
||||
AP_GROUPEND
|
||||
};
|
||||
// init
|
||||
void AP_MotorsSingle::Init()
|
||||
void AP_MotorsSingle::init(motor_frame_class frame_class, motor_frame_type frame_type)
|
||||
{
|
||||
// set update rate for the 3 motors (but not the servo on channel 7)
|
||||
set_update_rate(_speed_hz);
|
||||
@ -81,6 +81,15 @@ void AP_MotorsSingle::Init()
|
||||
|
||||
// allow mapping of motor7
|
||||
add_motor_num(CH_7);
|
||||
|
||||
// record successful initialisation if what we setup was the desired frame_class
|
||||
_flags.initialised_ok = (frame_class == MOTOR_FRAME_SINGLE);
|
||||
}
|
||||
|
||||
// set frame class (i.e. quad, hexa, heli) and type (i.e. x, plus)
|
||||
void AP_MotorsSingle::set_frame_class_and_type(motor_frame_class frame_class, motor_frame_type frame_type)
|
||||
{
|
||||
_flags.initialised_ok = (frame_class == MOTOR_FRAME_SINGLE);
|
||||
}
|
||||
|
||||
// set update rate to motors - a value in hertz
|
||||
|
||||
@ -31,7 +31,10 @@ public:
|
||||
};
|
||||
|
||||
// init
|
||||
virtual void Init();
|
||||
void init(motor_frame_class frame_class, motor_frame_type frame_type);
|
||||
|
||||
// set frame class (i.e. quad, hexa, heli) and type (i.e. x, plus)
|
||||
void set_frame_class_and_type(motor_frame_class frame_class, motor_frame_type frame_type);
|
||||
|
||||
// set update rate to motors - a value in hertz
|
||||
void set_update_rate( uint16_t speed_hz );
|
||||
|
||||
@ -79,7 +79,7 @@ const AP_Param::GroupInfo AP_MotorsTri::var_info[] = {
|
||||
};
|
||||
|
||||
// init
|
||||
void AP_MotorsTri::Init()
|
||||
void AP_MotorsTri::init(motor_frame_class frame_class, motor_frame_type frame_type)
|
||||
{
|
||||
add_motor_num(AP_MOTORS_MOT_1);
|
||||
add_motor_num(AP_MOTORS_MOT_2);
|
||||
@ -95,6 +95,15 @@ void AP_MotorsTri::Init()
|
||||
|
||||
// allow mapping of motor7
|
||||
add_motor_num(AP_MOTORS_CH_TRI_YAW);
|
||||
|
||||
// record successful initialisation if what we setup was the desired frame_class
|
||||
_flags.initialised_ok = (frame_class == MOTOR_FRAME_TRI);
|
||||
}
|
||||
|
||||
// set frame class (i.e. quad, hexa, heli) and type (i.e. x, plus)
|
||||
void AP_MotorsTri::set_frame_class_and_type(motor_frame_class frame_class, motor_frame_type frame_type)
|
||||
{
|
||||
_flags.initialised_ok = (frame_class == MOTOR_FRAME_TRI);
|
||||
}
|
||||
|
||||
// set update rate to motors - a value in hertz
|
||||
|
||||
@ -25,7 +25,10 @@ public:
|
||||
};
|
||||
|
||||
// init
|
||||
virtual void Init();
|
||||
void init(motor_frame_class frame_class, motor_frame_type frame_type);
|
||||
|
||||
// set frame class (i.e. quad, hexa, heli) and type (i.e. x, plus)
|
||||
void set_frame_class_and_type(motor_frame_class frame_class, motor_frame_type frame_type);
|
||||
|
||||
// set update rate to motors - a value in hertz
|
||||
void set_update_rate( uint16_t speed_hz );
|
||||
|
||||
@ -1,50 +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_MotorsY6.cpp - ArduCopter motors library
|
||||
* Code by RandyMackay. DIYDrones.com
|
||||
*
|
||||
*/
|
||||
|
||||
#include "AP_MotorsY6.h"
|
||||
|
||||
// setup_motors - configures the motors for a hexa
|
||||
void AP_MotorsY6::setup_motors()
|
||||
{
|
||||
// call parent
|
||||
AP_MotorsMatrix::setup_motors();
|
||||
|
||||
if (_flags.frame_orientation >= AP_MOTORS_NEW_PLUS_FRAME) {
|
||||
// Y6 motor definition with all top motors spinning clockwise, all bottom motors counter clockwise
|
||||
add_motor_raw(AP_MOTORS_MOT_1, -1.0f, 0.500f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
|
||||
add_motor_raw(AP_MOTORS_MOT_2, -1.0f, 0.500f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
|
||||
add_motor_raw(AP_MOTORS_MOT_3, 0.0f, -1.000f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 3);
|
||||
add_motor_raw(AP_MOTORS_MOT_4, 0.0f, -1.000f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 4);
|
||||
add_motor_raw(AP_MOTORS_MOT_5, 1.0f, 0.500f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5);
|
||||
add_motor_raw(AP_MOTORS_MOT_6, 1.0f, 0.500f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6);
|
||||
}else{
|
||||
// original Y6 motor definition
|
||||
add_motor_raw(AP_MOTORS_MOT_1, -1.0f, 0.666f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 2);
|
||||
add_motor_raw(AP_MOTORS_MOT_2, 1.0f, 0.666f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 5);
|
||||
add_motor_raw(AP_MOTORS_MOT_3, 1.0f, 0.666f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 6);
|
||||
add_motor_raw(AP_MOTORS_MOT_4, 0.0f, -1.333f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4);
|
||||
add_motor_raw(AP_MOTORS_MOT_5, -1.0f, 0.666f, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 1);
|
||||
add_motor_raw(AP_MOTORS_MOT_6, 0.0f, -1.333f, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
|
||||
}
|
||||
|
||||
// normalise factors to magnitude 0.5
|
||||
normalise_rpy_factors();
|
||||
}
|
||||
@ -1,26 +0,0 @@
|
||||
/// @file AP_MotorsY6.h
|
||||
/// @brief Motor control class for Y6 frames
|
||||
#pragma once
|
||||
|
||||
#include <AP_Common/AP_Common.h>
|
||||
#include <AP_Math/AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
|
||||
#include <RC_Channel/RC_Channel.h> // RC Channel Library
|
||||
#include "AP_MotorsMatrix.h" // Parent Motors Matrix library
|
||||
|
||||
#define AP_MOTORS_Y6_YAW_DIRECTION 1 // this really should be a user selectable parameter
|
||||
|
||||
/// @class AP_MotorsY6
|
||||
class AP_MotorsY6 : public AP_MotorsMatrix {
|
||||
public:
|
||||
|
||||
/// Constructor
|
||||
AP_MotorsY6(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) :
|
||||
AP_MotorsMatrix(loop_rate, speed_hz)
|
||||
{ };
|
||||
|
||||
// setup_motors - configures the motors for a Y6
|
||||
virtual void setup_motors();
|
||||
|
||||
protected:
|
||||
|
||||
};
|
||||
@ -42,7 +42,6 @@ AP_Motors::AP_Motors(uint16_t loop_rate, uint16_t speed_hz) :
|
||||
{
|
||||
// init other flags
|
||||
_flags.armed = false;
|
||||
_flags.frame_orientation = AP_MOTORS_X_FRAME;
|
||||
_flags.interlock = false;
|
||||
|
||||
// setup throttle filtering
|
||||
|
||||
@ -18,19 +18,6 @@
|
||||
|
||||
#define AP_MOTORS_MAX_NUM_MOTORS 8
|
||||
|
||||
// frame definitions
|
||||
#define AP_MOTORS_PLUS_FRAME 0
|
||||
#define AP_MOTORS_X_FRAME 1
|
||||
#define AP_MOTORS_V_FRAME 2
|
||||
#define AP_MOTORS_H_FRAME 3 // same as X frame but motors spin in opposite direction
|
||||
#define AP_MOTORS_VTAIL_FRAME 4 // Lynxmotion Hunter VTail 400/500
|
||||
#define AP_MOTORS_ATAIL_FRAME 5 // A-Shaped VTail Quads
|
||||
#define AP_MOTORS_NEW_PLUS_FRAME 10 // NEW frames are same as original 4 but with motor orders changed to be clockwise from the front
|
||||
#define AP_MOTORS_NEW_X_FRAME 11
|
||||
#define AP_MOTORS_NEW_V_FRAME 12
|
||||
#define AP_MOTORS_NEW_H_FRAME 13 // same as X frame but motors spin in opposite direction
|
||||
#define AP_MOTORS_QUADPLANE 14 // motors on 5..8
|
||||
|
||||
// motor update rate
|
||||
#define AP_MOTORS_SPEED_DEFAULT 490 // default output rate to the motors
|
||||
|
||||
@ -38,11 +25,34 @@
|
||||
class AP_Motors {
|
||||
public:
|
||||
|
||||
enum motor_frame_class {
|
||||
MOTOR_FRAME_UNDEFINED = 0,
|
||||
MOTOR_FRAME_QUAD = 1,
|
||||
MOTOR_FRAME_HEXA = 2,
|
||||
MOTOR_FRAME_OCTA = 3,
|
||||
MOTOR_FRAME_OCTAQUAD = 4,
|
||||
MOTOR_FRAME_Y6 = 5,
|
||||
MOTOR_FRAME_HELI = 6,
|
||||
MOTOR_FRAME_TRI = 7,
|
||||
MOTOR_FRAME_SINGLE = 8,
|
||||
MOTOR_FRAME_COAX = 9
|
||||
};
|
||||
enum motor_frame_type {
|
||||
MOTOR_FRAME_TYPE_PLUS = 0,
|
||||
MOTOR_FRAME_TYPE_X = 1,
|
||||
MOTOR_FRAME_TYPE_V = 2,
|
||||
MOTOR_FRAME_TYPE_H = 3,
|
||||
MOTOR_FRAME_TYPE_VTAIL = 4,
|
||||
MOTOR_FRAME_TYPE_ATAIL = 5,
|
||||
MOTOR_FRAME_TYPE_Y6B = 6,
|
||||
MOTOR_FRAME_TYPE_NEW_PLUS = 10
|
||||
};
|
||||
|
||||
// Constructor
|
||||
AP_Motors(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT);
|
||||
|
||||
// set frame orientation (normally + or X)
|
||||
virtual void set_frame_orientation( uint8_t new_orientation ) { _flags.frame_orientation = new_orientation; }
|
||||
// check initialisation succeeded
|
||||
bool initialised_ok() const { return _flags.initialised_ok; }
|
||||
|
||||
// arm, disarm or check status status of motors
|
||||
bool armed() const { return _flags.armed; }
|
||||
@ -108,7 +118,10 @@ public:
|
||||
virtual void set_update_rate( uint16_t speed_hz ) { _speed_hz = speed_hz; }
|
||||
|
||||
// init
|
||||
virtual void Init() = 0;
|
||||
virtual void init(motor_frame_class frame_class, motor_frame_type frame_type) = 0;
|
||||
|
||||
// set frame class (i.e. quad, hexa, heli) and type (i.e. x, plus)
|
||||
virtual void set_frame_class_and_type(motor_frame_class frame_class, motor_frame_type frame_type) = 0;
|
||||
|
||||
// enable - starts allowing signals to be sent to motors
|
||||
virtual void enable() = 0;
|
||||
@ -163,8 +176,8 @@ protected:
|
||||
// flag bitmask
|
||||
struct AP_Motors_flags {
|
||||
uint8_t armed : 1; // 0 if disarmed, 1 if armed
|
||||
uint8_t frame_orientation : 4; // PLUS_FRAME 0, X_FRAME 1, V_FRAME 2, H_FRAME 3, NEW_PLUS_FRAME 10, NEW_X_FRAME, NEW_V_FRAME, NEW_H_FRAME
|
||||
uint8_t interlock : 1; // 1 if the motor interlock is enabled (i.e. motors run), 0 if disabled (motors don't run)
|
||||
uint8_t initialised_ok : 1; // 1 if initialisation was successful
|
||||
} _flags;
|
||||
|
||||
// internal variables
|
||||
|
||||
Loading…
Reference in New Issue
Block a user