diff --git a/libraries/AP_Motors/AP_Motors.h b/libraries/AP_Motors/AP_Motors.h index 2911f739ff..707c2b4538 100644 --- a/libraries/AP_Motors/AP_Motors.h +++ b/libraries/AP_Motors/AP_Motors.h @@ -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" diff --git a/libraries/AP_Motors/AP_MotorsCoax.cpp b/libraries/AP_Motors/AP_MotorsCoax.cpp index 495c685a19..ca32591009 100644 --- a/libraries/AP_Motors/AP_MotorsCoax.cpp +++ b/libraries/AP_Motors/AP_MotorsCoax.cpp @@ -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 diff --git a/libraries/AP_Motors/AP_MotorsCoax.h b/libraries/AP_Motors/AP_MotorsCoax.h index 55cb187999..d3f9302fb3 100644 --- a/libraries/AP_Motors/AP_MotorsCoax.h +++ b/libraries/AP_Motors/AP_MotorsCoax.h @@ -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 ); diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index c519c65b80..d2d23a40eb 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -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 diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index 552aecc6a1..13d380e3b8 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -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; diff --git a/libraries/AP_Motors/AP_MotorsHexa.cpp b/libraries/AP_Motors/AP_MotorsHexa.cpp deleted file mode 100644 index 5311d8f67a..0000000000 --- a/libraries/AP_Motors/AP_MotorsHexa.cpp +++ /dev/null @@ -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 . - */ - -/* - * 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(); -} diff --git a/libraries/AP_Motors/AP_MotorsHexa.h b/libraries/AP_Motors/AP_MotorsHexa.h deleted file mode 100644 index 4993da25d7..0000000000 --- a/libraries/AP_Motors/AP_MotorsHexa.h +++ /dev/null @@ -1,24 +0,0 @@ -/// @file AP_MotorsHexa.h -/// @brief Motor control class for Hexacopters -#pragma once - -#include -#include // ArduPilot Mega Vector/Matrix math Library -#include // 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: - -}; diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp index a4b9662bf4..74f4d963d0 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.cpp +++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp @@ -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_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(); -} diff --git a/libraries/AP_Motors/AP_MotorsOcta.h b/libraries/AP_Motors/AP_MotorsOcta.h deleted file mode 100644 index 0b3ffeb65f..0000000000 --- a/libraries/AP_Motors/AP_MotorsOcta.h +++ /dev/null @@ -1,24 +0,0 @@ -/// @file AP_MotorsOcta.h -/// @brief Motor control class for Octacopters -#pragma once - -#include -#include // ArduPilot Mega Vector/Matrix math Library -#include // 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: - -}; diff --git a/libraries/AP_Motors/AP_MotorsOctaQuad.cpp b/libraries/AP_Motors/AP_MotorsOctaQuad.cpp deleted file mode 100644 index fff9fc1286..0000000000 --- a/libraries/AP_Motors/AP_MotorsOctaQuad.cpp +++ /dev/null @@ -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 . - */ - -/* - * 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(); -} diff --git a/libraries/AP_Motors/AP_MotorsOctaQuad.h b/libraries/AP_Motors/AP_MotorsOctaQuad.h deleted file mode 100644 index ae655ffc32..0000000000 --- a/libraries/AP_Motors/AP_MotorsOctaQuad.h +++ /dev/null @@ -1,24 +0,0 @@ -/// @file AP_MotorsOctaQuad.h -/// @brief Motor control class for OctaQuadcopters -#pragma once - -#include -#include // ArduPilot Mega Vector/Matrix math Library -#include // 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: - -}; diff --git a/libraries/AP_Motors/AP_MotorsQuad.cpp b/libraries/AP_Motors/AP_MotorsQuad.cpp deleted file mode 100644 index 928ff293ab..0000000000 --- a/libraries/AP_Motors/AP_MotorsQuad.cpp +++ /dev/null @@ -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 . - */ - -/* - * 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(); -} diff --git a/libraries/AP_Motors/AP_MotorsQuad.h b/libraries/AP_Motors/AP_MotorsQuad.h deleted file mode 100644 index 78514d493e..0000000000 --- a/libraries/AP_Motors/AP_MotorsQuad.h +++ /dev/null @@ -1,24 +0,0 @@ -/// @file AP_MotorsQuad.h -/// @brief Motor control class for Quadcopters -#pragma once - -#include -#include // ArduPilot Mega Vector/Matrix math Library -#include // 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: - -}; diff --git a/libraries/AP_Motors/AP_MotorsSingle.cpp b/libraries/AP_Motors/AP_MotorsSingle.cpp index 7daf7efbd1..da77d0370c 100644 --- a/libraries/AP_Motors/AP_MotorsSingle.cpp +++ b/libraries/AP_Motors/AP_MotorsSingle.cpp @@ -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 diff --git a/libraries/AP_Motors/AP_MotorsSingle.h b/libraries/AP_Motors/AP_MotorsSingle.h index d6f511e480..4350bafe70 100644 --- a/libraries/AP_Motors/AP_MotorsSingle.h +++ b/libraries/AP_Motors/AP_MotorsSingle.h @@ -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 ); diff --git a/libraries/AP_Motors/AP_MotorsTri.cpp b/libraries/AP_Motors/AP_MotorsTri.cpp index a70a79d57a..9e14bb71ed 100644 --- a/libraries/AP_Motors/AP_MotorsTri.cpp +++ b/libraries/AP_Motors/AP_MotorsTri.cpp @@ -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 diff --git a/libraries/AP_Motors/AP_MotorsTri.h b/libraries/AP_Motors/AP_MotorsTri.h index 58f1f2c578..9751981e1b 100644 --- a/libraries/AP_Motors/AP_MotorsTri.h +++ b/libraries/AP_Motors/AP_MotorsTri.h @@ -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 ); diff --git a/libraries/AP_Motors/AP_MotorsY6.cpp b/libraries/AP_Motors/AP_MotorsY6.cpp deleted file mode 100644 index fc36707d47..0000000000 --- a/libraries/AP_Motors/AP_MotorsY6.cpp +++ /dev/null @@ -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 . - */ - -/* - * 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(); -} diff --git a/libraries/AP_Motors/AP_MotorsY6.h b/libraries/AP_Motors/AP_MotorsY6.h deleted file mode 100644 index 1f2ff50cef..0000000000 --- a/libraries/AP_Motors/AP_MotorsY6.h +++ /dev/null @@ -1,26 +0,0 @@ -/// @file AP_MotorsY6.h -/// @brief Motor control class for Y6 frames -#pragma once - -#include -#include // ArduPilot Mega Vector/Matrix math Library -#include // 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: - -}; diff --git a/libraries/AP_Motors/AP_Motors_Class.cpp b/libraries/AP_Motors/AP_Motors_Class.cpp index 7b9c40d1d6..eb849bf0ff 100644 --- a/libraries/AP_Motors/AP_Motors_Class.cpp +++ b/libraries/AP_Motors/AP_Motors_Class.cpp @@ -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 diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index d5c548dc9d..7a108fdaa5 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -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